void holoMove(float x, float y, float rotateX) {
// Joystick input must be within {-100 - 100}
float leftOriWheel;
float rightOriWheel;
// Movement
leftOriWheel = getMotorPower(getAngle(x, y), LEFT_ORIENTATION_HOLO_WHEEL) * getPower(x, y);
rightOriWheel = getMotorPower(getAngle(x, y), RIGHT_ORIENTATION_HOLO_WHEEL) * getPower(x, y);
motor[FLWheel] = (int) (((((rightOriWheel/100)*FULL_MOTOR_POWER) + rotateX)/200)*100);
motor[FRWheel] = (int) (((((leftOriWheel/100)*FULL_MOTOR_POWER) + -rotateX)/200)*100);
motor[BLWheel] = (int) (((((leftOriWheel/100)*FULL_MOTOR_POWER) + rotateX)/200)*100);
motor[BRWheel] = (int) (((((rightOriWheel/100)*FULL_MOTOR_POWER) + -rotateX)/200)*100);
}
void CMD_MotorsSwitch(){
boolean pow;
int m;
while(!Serial.available()){
if( checkTimeOut() ) return;
}
digitalWrite(SERIALPIN, LOW);
m = Serial.read();
digitalWrite(SERIALPIN, HIGH);
pow = !getMotorPower(m);
setMotorPower( m, pow );
pendMotorsPowerTM();
}
