Merge branch 'feature/modify_0206' into 'master'

drivers/devicecontrol/tank0206_control.c

@@ -79,58 +79,95 @@ void tank0206_mode_right_back(unsigned char gval) {
 	}  
 }
 
+int tank0206_change_grading(int mode){
+	int mode_val=0;
+		switch(mode){
+			case 2:
+				mode_val=10;
+				break;
+			case 3:
+				mode_val=20;
+				break;
+			case 4:
+				mode_val=40;
+				break;
+			case 5:
+				mode_val=50;
+				break;
+			case 6:
+				mode_val=60;
+				break;
+			case 7:
+				mode_val=70;
+				break;
+			default:
+				break;
+		}
+		return mode_val;
+}
+
 void  tank0206_change(unsigned char *buf) {
+	unsigned char type = buf[0];
     unsigned char mode = buf[1];
     unsigned char val = buf[2];
-	static int modecount_tank0206=0;
-	static int tank0206_index =0;
-    if(mode == 1 ) {
-        tank0206_mode_lift_flont(val);
-		tank0206_mode_right_flont(val);
-		modecount_tank0206=0;
-    }else if(mode == 2 ) {
-        tank0206_mode_lift_back(val);
-        tank0206_mode_right_back(val);
-		modecount_tank0206=1;
-    }
 
-	if((mode == 1||mode ==2)&&val == 0) {
-		modecount_tank0206=0;
-		tank0206_index =0;
-	}
-	if((mode == 1||mode ==2)&&val != 0) {
-		tank0206_index =1;
-	}
+	static int tank0206_steering_t=0;
+	static int tank0206_front_t =0;
 
-	if(mode == 3) {
-		if(modecount_tank0206 == 0) {
-			//tank0206_mode_lift_back(val+10);
-			if(tank0206_index ==1)tank0206_mode_right_flont(0);
-			else {
-				tank0206_mode_right_back(val+30);
-				tank0206_mode_lift_flont(val+30);
-			}
-		}
-		if(modecount_tank0206 == 1) {
-			//tank0206_mode_lift_flont(val+10);
-			if(tank0206_index ==1)tank0206_mode_lift_back(0);
-		}
+	static int tank0206_front_val=0;
+	static int tank0206_steering_val=0;
 
-    }else if(mode == 4) {
-        if(modecount_tank0206 == 0) {
-			if(tank0206_index ==1)tank0206_mode_lift_flont(0);
-			else{
-				tank0206_mode_lift_back(val+30);
-				tank0206_mode_right_flont(val+30);
-			}
-			//tank0206_mode_right_back(val+10);
-		}
-		if(modecount_tank0206 == 1) {
-			tank0206_mode_right_flont(0);
-			//tank0206_mode_lift_flont(val+10);
+	int tank0206_count=30;
+
+	if(type != 1) tank0206_count=tank0206_change_grading(type);
+
+	if((mode == 1 ||mode == 2)&&val == 0) tank0206_front_t =0;
+	if((mode == 3 ||mode == 4)&&val == 0) tank0206_steering_t=0;
+
+	if(mode == 1&&val != 0) {
+		tank0206_front_val=val;
+		tank0206_front_t =1;
+	}else if(mode == 2&&val != 0) {
+		tank0206_front_t =2;
+		tank0206_front_val=val;
 	}
 
+	if(mode == 3&&val != 0) {
+		tank0206_steering_t =1;
+		tank0206_steering_val = val;
+	}else if(mode == 4&&val != 0) {
+		tank0206_steering_t =2;
+		tank0206_steering_val = val;
+	}
 
+	if(tank0206_front_t ==0&&tank0206_steering_t==0){
+	    tank0206_mode_lift_flont(0);
+		tank0206_mode_right_flont(0);
+	}else if(tank0206_front_t ==1&&tank0206_steering_t==0){
+		tank0206_mode_lift_flont(tank0206_front_val);
+		tank0206_mode_right_flont(tank0206_front_val);
+	}else if(tank0206_front_t ==2&&tank0206_steering_t==0){
+		tank0206_mode_lift_back(tank0206_front_val);
+        tank0206_mode_right_back(tank0206_front_val);
+	}else if(tank0206_front_t ==0&&tank0206_steering_t==1){
+		tank0206_mode_right_back(tank0206_steering_val+tank0206_count);
+		tank0206_mode_lift_flont(tank0206_steering_val+tank0206_count);
+	}else if(tank0206_front_t ==0&&tank0206_steering_t==2){
+		tank0206_mode_lift_back(tank0206_steering_val+tank0206_count);
+		tank0206_mode_right_flont(tank0206_steering_val+tank0206_count);
+	}
+	else if(tank0206_front_t ==1&&tank0206_steering_t==1){
+		tank0206_mode_lift_flont(tank0206_steering_val+tank0206_count);
+        tank0206_mode_right_flont(0);
+	}else if(tank0206_front_t ==1&&tank0206_steering_t==2){
+		tank0206_mode_lift_flont(0);
+        tank0206_mode_right_flont(tank0206_steering_val+tank0206_count);
+	}else if(tank0206_front_t ==2&&tank0206_steering_t==1){
+		tank0206_mode_lift_back(tank0206_steering_val+tank0206_count);
+        tank0206_mode_right_back(0);
+	}else if(tank0206_front_t ==2&&tank0206_steering_t==2){
+		tank0206_mode_lift_back(0);
+        tank0206_mode_right_back(tank0206_steering_val+tank0206_count);
 	}
     
 }