+ 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;
+