int motor1_enable = A8;
int motor1_in1 = A9;
int motor1_in2 = A10;
int motor2_enable = 7;
int motor2_in1 = 6;
int motor2_in2 = 5;
int motor3_enable = 10;
int motor3_in1 = 9;
int motor3_in2 = 8;
int motor4_enable = 2;
int motor4_in1 = 3;
int motor4_in2 = 4;
int motor5_enable = 46;
int motor5_in1 = 45;
int motor5_in2 = 4;
int light_enable = A6;
int light_in1 = A7;
int light_in2 = A5;

void setup() {
   Serial.begin(9600);

   pinMode(motor1_enable, OUTPUT);
   pinMode(motor1_in1, OUTPUT);
   pinMode(motor1_in2, OUTPUT);
   pinMode(motor2_enable, OUTPUT);
   pinMode(motor2_in1, OUTPUT);
   pinMode(motor2_in2, OUTPUT);
   pinMode(motor3_enable, OUTPUT);
   pinMode(motor3_in1, OUTPUT);
   pinMode(motor3_in2, OUTPUT);
   pinMode(motor4_enable, OUTPUT);
   pinMode(motor4_in1, OUTPUT);
   pinMode(motor4_in2, OUTPUT);
   pinMode(motor5_enable, OUTPUT);
   pinMode(motor5_in1, OUTPUT);
   pinMode(motor5_in2, OUTPUT);
   pinMode(light_enable, OUTPUT);
   pinMode(light_in1, OUTPUT);
   pinMode(light_in2, OUTPUT);
}

void wristOpen() {
  digitalWrite(motor1_enable, HIGH);
  digitalWrite(motor1_in1, HIGH);
  digitalWrite(motor1_in2, LOW);
}

void wristClose() {
  digitalWrite(motor1_enable, HIGH);
  digitalWrite(motor1_in2, HIGH);
  digitalWrite(motor1_in1, LOW);
}

void wristStop() {
  digitalWrite(motor1_enable, LOW);
  digitalWrite(motor1_in1, LOW);
  digitalWrite(motor1_in2, LOW);
}

void handUp() {
  digitalWrite(motor2_enable, HIGH);
  digitalWrite(motor2_in1, HIGH);
  digitalWrite(motor2_in2, LOW);
}

void handDown() {
  digitalWrite(motor2_enable, HIGH);
  digitalWrite(motor2_in2, HIGH);
  digitalWrite(motor2_in1, LOW);
}

void handStop() {
  digitalWrite(motor2_enable, LOW);
  digitalWrite(motor2_in2, LOW);
  digitalWrite(motor2_in1, LOW);
}

void elbowUp() {
  digitalWrite(motor3_enable, HIGH);
  digitalWrite(motor3_in1, HIGH);
  digitalWrite(motor3_in2, LOW);
}

void elbowDown() {
  digitalWrite(motor3_enable, HIGH);
  digitalWrite(motor3_in2, HIGH);
  digitalWrite(motor3_in1, LOW);
}

void elbowStop() {
  digitalWrite(motor3_enable, LOW);
  digitalWrite(motor3_in2, LOW);
  digitalWrite(motor3_in1, LOW);
}

void shoulderUp() {
  digitalWrite(motor4_enable, HIGH);
  digitalWrite(motor4_in1, HIGH);
  digitalWrite(motor4_in2, LOW);
}

void shoulderDown() {
  digitalWrite(motor4_enable, HIGH);
  digitalWrite(motor4_in2, HIGH);
  digitalWrite(motor4_in1, LOW);
}

void shoulderStop() {
  digitalWrite(motor4_enable, LOW);
  digitalWrite(motor4_in1, LOW);
  digitalWrite(motor4_in2, LOW);
}

void baseLeft() {
  digitalWrite(motor5_enable, HIGH);
  digitalWrite(motor5_in1, HIGH);
  digitalWrite(motor5_in2, LOW);
}

void baseRight() {
  digitalWrite(motor5_enable, HIGH);
  digitalWrite(motor5_in2, HIGH);
  digitalWrite(motor5_in1, LOW);
}

void baseStop() {
  digitalWrite(motor5_enable, LOW);
  digitalWrite(motor5_in2, LOW);
  digitalWrite(motor5_in1, LOW);
}

void lightOn() {
  digitalWrite(light_enable, HIGH);
  digitalWrite(light_in1, HIGH);
  digitalWrite(light_in2, LOW);
}

void lightOff() {
  digitalWrite(light_enable, HIGH);
  digitalWrite(light_in2, HIGH);
  digitalWrite(light_in1, LOW);
}

void lightStop() {
  digitalWrite(light_enable, LOW);
  digitalWrite(light_in1, LOW);
  digitalWrite(light_in2, LOW);
}

void stopMove() {
  digitalWrite(motor1_enable, LOW);
  digitalWrite(motor1_in1, LOW);
  digitalWrite(motor1_in2, LOW);

  digitalWrite(motor2_enable, LOW);
  digitalWrite(motor2_in1, LOW);
  digitalWrite(motor2_in2, LOW);

  digitalWrite(motor3_enable, LOW);
  digitalWrite(motor3_in1, LOW);
  digitalWrite(motor3_in2, LOW);

  digitalWrite(motor4_enable, LOW);
  digitalWrite(motor4_in1, LOW);
  digitalWrite(motor4_in2, LOW);

  digitalWrite(motor5_enable, LOW);
  digitalWrite(motor5_in1, LOW);
  digitalWrite(motor5_in2, LOW);

  digitalWrite(light_enable, LOW);
  digitalWrite(light_in1, LOW);
  digitalWrite(light_in2, LOW);
}

void loop() {
  if (Serial.available() > 0) {
    int inByte = Serial.read();

    if (inByte == 'a') {
      wristClose();
    }
    if (inByte == 'q') {
      wristOpen();
    }
    if (inByte == 'y') {
      wristStop();
    }

    if (inByte == 'w') {
      handUp();
    }
    if (inByte == 's') {
      handDown();
    }
    if (inByte == 'x') {
      handStop();
    }

    if (inByte == 'e') {
      elbowUp();
    }
    if (inByte == 'd') {
      elbowDown();
    }
    if (inByte == 'c') {
      elbowStop();
    }

    if (inByte == 'f') {
      shoulderDown();
    }
    if (inByte == 'r') {
      shoulderUp();
    }
    if (inByte == 'v') {
      shoulderStop();
    }

    if (inByte == '.') {
      baseLeft();
    }
    if (inByte == ',') {
      baseRight();
    }
    if (inByte == '-') {
      baseStop();
    }

    if (inByte == 'g') {
      lightOff();
    }
    if (inByte == 't') {
      lightOn();
    }
    if (inByte == 'b') {
      lightStop();
    }

    if (inByte == '0') {
      stopMove();
    }