+ if (motor3_mode == MOTOR_PID) {
+ if (speed3_wish == 0) {
+ motor3 = 0;
+ eold3 = 0;
+ esum3 = 0;
+ error_state &= ~(1<<6);
+ } else {
+ int16_t e = speed3_wish - speed3;
+ esum3+=e;
+ motor3 = KP*e + KI*PID_T*esum3 + KD/PID_T*(e - eold3);
+ eold3 = e;
+
+ if (motor3 > 0 && speed3_wish < 0) motor3=0;
+ else if (motor3 < 0 && speed3_wish > 0) motor3=0;
+ else if (motor3 > 255) motor3 = 255;
+ else if (motor3 < -255) motor3 = -255;
+ }
+ }
+ if (motor4_mode == MOTOR_PID) {
+ if (speed4_wish == 0) {
+ motor4 = 0;
+ eold4 = 0;
+ esum4 = 0;
+ error_state &= ~(1<<7);
+ } else {
+ int16_t e = speed4_wish - speed4;
+ esum4+=e;
+ motor4 = KP*e + KI*PID_T*esum4 + KD/PID_T*(e - eold4);
+ eold4 = e;
+
+ if (motor4 > 0 && speed4_wish < 0) motor4=0;
+ else if (motor4 < 0 && speed4_wish > 0) motor4=0;
+ else if (motor4 > 255) motor4 = 255;
+ else if (motor4 < -255) motor4 = -255;
+ }
+ }