#include "common.h"
#include "car0101_control.h"
#include "modules_common.h"
#include "gpio_common.h"

/*将角度转化为对应的舵机pwm值*/
void car_calculate_L_R(int angle) {
    if (angle < 0) {
        angle = 0;
    } else if (angle > 180) {
        angle = 180;
    }
	float pulse_width=( angle / 180.0) *2 + 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(int gval) {
   	int a=0,b=1;
	if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 71-b+a);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 70-b+a);
	} else if(gval >70){
		int back_speed = 70 - (gval-70)/10-b+a;
		if (back_speed < 55) {
			back_speed = 55;
		}
		pwmWrite(PWM_PIN_SPEED, back_speed);
	}  
}

void car0101_mode_2_back(int gval) {
	int b=0,a=1;
   if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	}
	else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 79+b-a);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 80+b-a);
	}else if(gval >70){
		int flont_speed = 80+(gval-70)/10+b-a;
		if (flont_speed > 95) {
			flont_speed = 95;
		}
		pwmWrite(PWM_PIN_SPEED, flont_speed);
	}
}


void car0101_mode_3_left(int gval) {
	int b=15;
	if(gval<45){
		car_calculate_L_R(90);
	}else if(gval<70){
		car_calculate_L_R(50+gval+b);
	}else if(gval>=70){
		car_calculate_L_R(135);
	}		
}

void car0101_mode_4_right(int gval) {
	int b=10;
	if(gval<45){
		car_calculate_L_R(90);
	}else if(gval<70){
		car_calculate_L_R(130-gval-b);
	}else if(gval>=70){
		car_calculate_L_R(35);
	}  
}

void car0101_control_change(int *buf) {//车速度和转向引脚数值处理函数
    int mode=buf[1];
    int 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;
    } 
}
