+
+ if (cmd_vel.bUpdate) {
+ float speed_wish_right, speed_wish_left;
+ float speed, angle;
+
+ cli();
+ speed = cmd_vel.speed;
+ angle = cmd_vel.angle;
+ cmd_vel.bUpdate = 0;
+ sei();
+
+ speed_wish_right = angle*M_PI*WHEEL_DIST/2 + speed;
+ speed_wish_left = speed*2-speed_wish_right;
+
+ speed_wish_left*=STEP_PER_M;
+ speed_wish_right*=STEP_PER_M;
+
+ if (speed_wish_left > 0 && aft_handicap_fwd > 0) {
+ speed1_wish = speed_wish_left * (100-aft_handicap_fwd)/100.0;
+ } else {
+ speed1_wish = speed_wish_left;
+ }
+ if (speed_wish_left < 0 && front_handicap_bwd > 0) {
+ speed2_wish = speed_wish_left * (100-front_handicap_bwd)/100.0;
+ } else {
+ speed2_wish = speed_wish_left;
+ }
+ if (speed_wish_right < 0 && front_handicap_bwd > 0) {
+ speed3_wish = speed_wish_right * (100-front_handicap_bwd)/100.0;
+ } else {
+ speed3_wish = speed_wish_right;
+ }
+ if (speed_wish_right > 0 && aft_handicap_fwd > 0) {
+ speed4_wish = speed_wish_right * (100-aft_handicap_fwd)/100.0;
+ } else {
+ speed4_wish = speed_wish_right;
+ }
+ motor1_mode = MOTOR_PID;
+ motor2_mode = MOTOR_PID;
+ motor3_mode = MOTOR_PID;
+ motor4_mode = MOTOR_PID;
+ }