diff --git a/server/commande_telephone.ino b/server/commande_telephone.ino index 4dd8397..059b78f 100644 --- a/server/commande_telephone.ino +++ b/server/commande_telephone.ino @@ -196,175 +196,67 @@ void chooseCommand() } } +void writeMotor(uint8_t left, uint8_t right) +{ + digitalWrite(IN4a, left & 0b1000); + digitalWrite(IN3a, left & 0b0100); + digitalWrite(IN2a, left & 0b0010); + digitalWrite(IN1a, left & 0b0001); + digitalWrite(IN4b, right & 0b1000); + digitalWrite(IN3b, right & 0b0100); + digitalWrite(IN2b, right & 0b0010); + digitalWrite(IN1b, right & 0b0001); +} + void forward() { - digitalWrite(IN4a, HIGH); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, HIGH); + writeMotor(0b1000, 0b0001); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, HIGH); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, HIGH); - digitalWrite(IN1b, LOW); + writeMotor(0b0100, 0b0010); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, HIGH); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, HIGH); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b0010, 0b0100); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, HIGH); - digitalWrite(IN4b, HIGH); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b0001, 0b1000); delay(delayTime); } void backward() { - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, HIGH); - digitalWrite(IN4b, HIGH); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b0001, 0b1000); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, HIGH); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, HIGH); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b0010, 0b0100); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, HIGH); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, HIGH); - digitalWrite(IN1b, LOW); + writeMotor(0b0100, 0b0010); delay(delayTime); - digitalWrite(IN4a, HIGH); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, HIGH); + writeMotor(0b1000, 0b0001); delay(delayTime); } void right() { - digitalWrite(IN4a, HIGH); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, HIGH); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b1000, 0b1000); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, HIGH); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, HIGH); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b0100, 0b0100); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, HIGH); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, HIGH); - digitalWrite(IN1b, LOW); + writeMotor(0b0010, 0b0010); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, HIGH); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, HIGH); + writeMotor(0b0001, 0b0001); delay(delayTime); } void left() { - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, HIGH); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, HIGH); + writeMotor(0b0001, 0b0001); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, HIGH); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, HIGH); - digitalWrite(IN1b, LOW); + writeMotor(0b0010, 0b0010); delay(delayTime); - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, HIGH); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, HIGH); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b0100, 0b0100); delay(delayTime); - digitalWrite(IN4a, HIGH); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - digitalWrite(IN4b, HIGH); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b1000, 0b1000); delay(delayTime); } void stopMotors(void) { - digitalWrite(IN4a, LOW); - digitalWrite(IN3a, LOW); - digitalWrite(IN2a, LOW); - digitalWrite(IN1a, LOW); - - digitalWrite(IN4b, LOW); - digitalWrite(IN3b, LOW); - digitalWrite(IN2b, LOW); - digitalWrite(IN1b, LOW); + writeMotor(0b0000, 0b0000); }