+ 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_LEFT;
+ speed2_wish = tmp_speed.f*STEP_PER_M_LEFT;
+ 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_RIGHT;
+ speed2_wish = tmp_speed.f*STEP_PER_M_RIGHT;
+ 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;
+ cmd_vel.speed = tmp_speed.f;
+ 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;
+ cmd_vel.angle = tmp_angle.f;
+ cmd_vel.bUpdate = 1;
+ TWI_ACK;
+ break;