+ case 0x28: // Left speed wish MSB
+ tmp_speed.i = TWDR;
+ TWI_ACK;
+ break;
+ case 0x29: // Left speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x2A: // Left speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x2B: // Left speed wish LSB
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ speed1_wish = tmp_speed.f*STEP_PER_M;
+ speed2_wish = tmp_speed.f*STEP_PER_M;
+ motor1_mode = MOTOR_PID;
+ motor2_mode = MOTOR_PID;
+ TWI_ACK;
+ break;
+ case 0x2C: // Right speed wish MSB
+ tmp_speed.i = TWDR;
+ TWI_ACK;
+ break;
+ case 0x2D: // Right speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x2E: // Right speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x2F: // Right speed wish LSB
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ speed1_wish = tmp_speed.f*STEP_PER_M;
+ speed2_wish = tmp_speed.f*STEP_PER_M;
+ motor1_mode = MOTOR_PID;
+ motor2_mode = MOTOR_PID;
+ TWI_ACK;
+ break;
+ case 0x50: // speed wish MSB
+ tmp_speed.i = TWDR;
+ TWI_ACK;
+ break;
+ case 0x51: // speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x52: // speed wish
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x53: // speed wish LSB
+ tmp_speed.i = tmp_speed.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x54: // angle wish MSB
+ tmp_angle.i = TWDR;
+ TWI_ACK;
+ break;
+ case 0x55: // angle wish
+ tmp_angle.i = tmp_angle.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x56: // angle wish
+ tmp_angle.i = tmp_angle.i << 8 | TWDR;
+ TWI_ACK;
+ break;
+ case 0x57: // angle wish LSB
+ tmp_angle.i = tmp_angle.i << 8 | TWDR;
+ {
+ float speed_wish_right = tmp_angle.f*M_PI*WHEEL_DIST/2 + tmp_speed.f;
+ float speed_wish_left = tmp_speed.f*2-speed_wish_right;
+ speed1_wish = speed_wish_left*STEP_PER_M;
+ speed2_wish = speed_wish_left*STEP_PER_M;
+ speed3_wish = speed_wish_right*STEP_PER_M;
+ speed4_wish = speed_wish_right*STEP_PER_M;
+ }
+ motor1_mode = MOTOR_PID;
+ motor2_mode = MOTOR_PID;
+ motor3_mode = MOTOR_PID;
+ motor4_mode = MOTOR_PID;
+ TWI_ACK;
+ break;