+ if (motor3_mode == MOTOR_PID) {
+ if (speed3_wish != speed3_wish_old) {
+ if (abs(speed3_wish - speed3_wish_old) > 500) esum3 = 0;
+ speed3_wish_old = speed3_wish;
+ }
+
+ uint8_t dir_change = (speed3_wish > 0 && speed3 < 0) || (speed3_wish < 0 && speed3 > 0); // Prevent dangerous immediate engine reverse
+ if (speed3_wish == 0 || dir_change) {
+ motor3 = 0;
+ eold3 = 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=PWM_BREAK;
+ else if (motor3 < 0 && speed3_wish > 0) motor3=PWM_BREAK;
+ else if (motor3 > 255) motor3 = 255;
+ else if (motor3 < -255) motor3 = -255;
+ }
+ }
+ if (motor4_mode == MOTOR_PID) {
+ if (speed4_wish != speed4_wish_old) {
+ if (abs(speed4_wish - speed4_wish_old) > 500) esum4 = 0;
+ speed4_wish_old = speed4_wish;
+ }
+
+ uint8_t dir_change = (speed4_wish > 0 && speed4 < 0) || (speed4_wish < 0 && speed4 > 0); // Prevent dangerous immediate engine reverse
+ if (speed4_wish == 0 || dir_change) {
+ motor4 = 0;
+ eold4 = 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=PWM_BREAK;
+ else if (motor4 < 0 && speed4_wish > 0) motor4=PWM_BREAK;
+ else if (motor4 > 255) motor4 = 255;
+ else if (motor4 < -255) motor4 = -255;
+ }
+ }