#include "common.h"
#include "car0101_control.h"
#include "gpio_init.h"

/*将角度转化为对应的舵机pwm值*/
void car_calculate_L_R(int angle) {
    if (angle < 0) {
        angle = 0;
    } else if (angle > 180) {
        angle = 180;
    }
	float pulse_width=(2 / 180.0) * angle + 0.5;
	// 周期（ms）
    float period = 1000.0 / 50;
	int val = (int)((pulse_width / period) * 1000);
	pwmWrite(PWM_PIN_CHANGE,val);
}

/*车中间值，停止舵机中间函数*/
void car0101_middle_pwm() {//车停止pwm值
    pwmWrite(PWM_PIN_SPEED,75);
	car_calculate_L_R(90);
}


/*车的速度转向函数*/
void car0101_mode_1_flont(unsigned char gval) {
	if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_SPEED, 71);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 70);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_SPEED, 70);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 70);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_SPEED, 69);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, 68);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, 68);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_SPEED, 66);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_SPEED, 65);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_SPEED, 64);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_SPEED, 62);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_SPEED, 60);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_SPEED, 59);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_SPEED, 58);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_SPEED, 57);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_SPEED, 56);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_SPEED, 55);
	}    
}

void car0101_mode_2_back(unsigned char gval) {
   	if(gval<50) {
		pwmWrite(PWM_PIN_SPEED,75);
	}else if(50<=gval&&gval<=55) {
		pwmWrite(PWM_PIN_SPEED,76);
	}else if(55<gval&&gval<=60) {
		pwmWrite(PWM_PIN_SPEED,77);
	}else if(60<gval&&gval<=65) {
		pwmWrite(PWM_PIN_SPEED,77);
	}else if(65<gval&&gval<=70) {
		pwmWrite(PWM_PIN_SPEED,78);
	}else if(70<gval&&gval<=75) {
		pwmWrite(PWM_PIN_SPEED,78);
	}else if(75<gval&&gval<=90) {
		pwmWrite(PWM_PIN_SPEED,79);
	}else if(90<gval&&gval<=100) {
		pwmWrite(PWM_PIN_SPEED,80);  
	}else if(110<gval&&gval<=120) {
		pwmWrite(PWM_PIN_SPEED,81); 
	}else if(120<gval&&gval<=130) {
		pwmWrite(PWM_PIN_SPEED,82); 
	}else if(130<gval&&gval<=140) {
		pwmWrite(PWM_PIN_SPEED,83); 
	}else if(140<gval&&gval<=150) {
		pwmWrite(PWM_PIN_SPEED,84); 
	}else if(150<gval&&gval<=160) {
		pwmWrite(PWM_PIN_SPEED,85); 
	}else if(160<gval&&gval<=170) {
		pwmWrite(PWM_PIN_SPEED,86); 
	}else if(170<gval&&gval<=180) {
		pwmWrite(PWM_PIN_SPEED,87); 
	}else if(180<gval&&gval<=190) {
		pwmWrite(PWM_PIN_SPEED,88); 
	}else if(190<gval&&gval<=200) {
		pwmWrite(PWM_PIN_SPEED,89); 
	}
}

void car0101_mode_3_left(unsigned char gval) {
	if (gval < 45) {
		car_calculate_L_R(90);
	} else if (gval <= 51) {
		car_calculate_L_R(110);
	} else if (gval <= 57) {
		car_calculate_L_R(120);
	} else if (gval <= 63) {
		car_calculate_L_R(130);
	} else if (gval <= 69) {
		car_calculate_L_R(130);
	} else if (gval <= 75) {
		car_calculate_L_R(140);
	} else if (gval <= 81) {
		car_calculate_L_R(145);
	} else if (gval <= 87) {
		car_calculate_L_R(150);
	} else if (gval <= 93) {
		car_calculate_L_R(150);
	} else if (gval <= 100) {
		car_calculate_L_R(160);
	}else if (gval <= 107) {
		car_calculate_L_R(170);
	}else if (gval <= 120) {
		car_calculate_L_R(180);
	}		
}

void car0101_mode_4_right(unsigned char gval) {
	if (gval < 45) {
		car_calculate_L_R(90);
	} else if (gval <= 51) {
		car_calculate_L_R(70);
	} else if (gval <= 57) {
		car_calculate_L_R(66);
	} else if (gval <= 63) {
		car_calculate_L_R(62);
	} else if (gval <= 69) {
		car_calculate_L_R(55);
	} else if (gval <= 75) {
		car_calculate_L_R(45);
	} else if (gval <= 81) {
		car_calculate_L_R(40);
	} else if (gval <= 87) {
		car_calculate_L_R(30);
	} else if (gval <= 93) {
		car_calculate_L_R(30);
	} else if (gval <= 100) {
		car_calculate_L_R(20);
	} else if (gval <= 107) {
		car_calculate_L_R(10);
	} else if (gval <= 120) {
		car_calculate_L_R(0);
	}         
}

void car0101_control_change(unsigned char *buf) {//车速度和转向引脚数值处理函数
    unsigned char mode=buf[1];
    unsigned char val=buf[2];
    switch(mode){
        case 1:			
			car0101_mode_1_flont(val);					 
			break;
		case 2:	
			car0101_mode_2_back(val);
			break;
		case 3:
			car0101_mode_3_left(val);
			break;
		case 4:
			car0101_mode_4_right(val);
			break; 
        default:
			break;
    } 
}
