+ if (cmd_vel.bUpdate) {
+ float speed_wish_right = cmd_vel.angle*M_PI*WHEEL_DIST/2 + cmd_vel.speed;
+ float speed_wish_left = cmd_vel.speed*2-speed_wish_right;
+
+ cmd_vel.bUpdate = 0;
+ speed_wish_left*=STEP_PER_M;
+ speed_wish_right*=STEP_PER_M;
+
+ speed1_wish = speed_wish_left;
+ speed2_wish = speed_wish_left;
+ speed3_wish = speed_wish_right;
+ speed4_wish = speed_wish_right;
+ motor1_mode = MOTOR_PID;
+ motor2_mode = MOTOR_PID;
+ motor3_mode = MOTOR_PID;
+ motor4_mode = MOTOR_PID;
+ }
+
+ if (run_update >= 312) { // ~100Hz