+ // copy to tmp
+ cli();
+ cur_pos1 = pos1;
+ cur_pos2 = pos2;
+ cur_pos3 = pos3;
+ cur_pos4 = pos4;
+ sei();
+
+ pos1_diff = cur_pos1 - pos1_last;
+ pos2_diff = cur_pos2 - pos2_last;
+ pos3_diff = cur_pos3 - pos3_last;
+ pos4_diff = cur_pos4 - pos4_last;
+
+ new_speed1 = pos1_diff/PID_T;
+ new_speed2 = pos2_diff/PID_T;
+ new_speed3 = pos3_diff/PID_T;
+ new_speed4 = pos4_diff/PID_T;
+
+ diff_left_m = (pos1_diff + pos2_diff)/(2*STEP_PER_M_LEFT);
+ diff_right_m = (pos3_diff + pos4_diff)/(2*STEP_PER_M_RIGHT);
+ angle_diff = (diff_right_m - diff_left_m) / WHEEL_DIST;
+
+ angle_new = angle.f + angle_diff;
+ if (angle_new > 2*M_PI) angle_new-=2*M_PI;
+ else if (angle_new < -2*M_PI) angle_new+=2*M_PI;
+
+ translation = (diff_left_m + diff_right_m)/2.0;
+ pos_x_new = pos_x.f + cos(angle_new)*translation;
+ pos_y_new = pos_y.f + sin(angle_new)*translation;
+
+ tmp_speed_lin = translation/PID_T;
+ tmp_speed_rot = angle_diff/PID_T;
+
+ // copy from tmp
+ cli();
+ angle.f = angle_new;
+ pos_x.f = pos_x_new;
+ pos_y.f = pos_y_new;
+ speed1 = new_speed1;
+ speed2 = new_speed2;
+ speed3 = new_speed3;
+ speed4 = new_speed4;
+ cur_speed_lin = tmp_speed_lin;
+ cur_speed_rot = tmp_speed_rot;
+ sei();
+
+ pos1_last = cur_pos1;
+ pos2_last = cur_pos2;
+ pos3_last = cur_pos3;
+ pos4_last = cur_pos4;
+}
+
+
+static void update_pid(void) {
+ static int16_t eold1=0;
+ static int16_t eold2=0;
+ static int16_t eold3=0;
+ static int16_t eold4=0;
+ static int32_t esum1=0;
+ static int32_t esum2=0;
+ static int32_t esum3=0;
+ static int32_t esum4=0;
+
+ // protect motors from damage if stalling
+ if (labs(esum1) > 120000 && speed1 == 0) {
+ motor1 = 0;
+ motor1_mode = MOTOR_MANUAL;
+ error_state |= (1<<4);
+ esum1 = 0;
+ }
+ if (labs(esum2) > 120000 && speed2 == 0) {
+ motor2 = 0;
+ motor2_mode = MOTOR_MANUAL;
+ error_state |= (1<<5);
+ esum2 = 0;
+ }
+ if (labs(esum3) > 120000 && speed3 == 0) {
+ motor3 = 0;
+ motor3_mode = MOTOR_MANUAL;
+ error_state |= (1<<6);
+ esum3 = 0;
+ }
+ // protect motors from damage if stalling
+ if (labs(esum4) > 120000 && speed4 == 0) {
+ motor4 = 0;
+ motor4_mode = MOTOR_MANUAL;
+ error_state |= (1<<7);
+ esum4 = 0;
+ }