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

void car0104_stop() {
    pwmWrite(PWM_PIN_SPEED,75);
    pwmWrite(PWM_PIN_CHANGE,75);
}

//铲车后退
void car0104_back(int gval) {
	int b=7;
	int c=2;
   if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	}
	else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 85);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 87);
	}else if(gval >70){
		int flont_speed = 80+(gval-70)/10+b;
		if(flont_speed>95) flont_speed = 95;
		pwmWrite(PWM_PIN_SPEED, flont_speed);
	}
}

//铲车前进
void car0104_flont(int gval) {
	int b=7;
	int c=2;
	if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 65);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 63);
	} else if(gval >70){
		int back_speed = 70 - (gval-70)/10-b;
		if(back_speed<55) back_speed =55;
		pwmWrite(PWM_PIN_SPEED, back_speed);
	}    
}

//铲车右
void car0104_right(int gval) {
	int b=5;
   if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
	}
	else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 80);
	}else if (gval <= 90) {
		pwmWrite(PWM_PIN_CHANGE, 82);
	}else if(gval >90){
		int flont_speed = 82+(gval-70)/10+b;
		if(flont_speed>95) flont_speed = 95;
		pwmWrite(PWM_PIN_CHANGE, flont_speed);
	}
}

//铲车左
void car0104_lift(int gval) {
		int b=5;
	if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 70);
	}else if (gval <= 90) {
		pwmWrite(PWM_PIN_CHANGE, 68);
	} else if(gval >90){
		int back_speed = 68 - (gval-70)/10-b;
		if(back_speed<55) back_speed =55;
		pwmWrite(PWM_PIN_CHANGE, back_speed);
	}    
}

void  car0104_change(int *buf) {
    int mode = buf[1];
    int val = buf[2];
	
    if(mode == 1 ) {
        car0104_flont(val+10);
    }else if(mode == 2 ) {
        car0104_back(val+10);
    }else if(mode == 3) {
		if(val !=0) car0104_lift(100);
		else car0104_lift(0);
    }else if(mode == 4) {
		if(val !=0) car0104_right(100);  
		else  car0104_lift(0);
    }
    
}
