//Motor #1
#define M1_INPUT1_FWD 3
#define M1_INPUT2_REV 4
#define M1_ENABLE 5
#define LIMIT_SWITCH1 2
#define SWITCH_A_ROCKER 6

bool LIMIT_SWITCH1_State; // State of the Slider Switch for Joystick
bool SWITCH_A_ROCKER_State;  // State of the Rocker Switch HIGH_A,OFF,HIGH_B


//Motor #2
#define M2_ENABLE 9
#define M2_INPUT1_FWD 10
#define M2_INPUT2_REV 11
#define LIMIT_SWITCH2 8
#define SWITCH_B_ROCKER 7

bool LIMIT_SWITCH2_State; // State of the Slider Switch for Joystick
bool SWITCH_B_ROCKER_State;  // State of the Rocker Switch HIGH_A,OFF,HIGH_B

void setup()
{
  pinMode(M1_ENABLE, OUTPUT);  //0-255
  pinMode(M1_INPUT1_FWD, OUTPUT);
  pinMode(M1_INPUT2_REV, OUTPUT);
  pinMode (LIMIT_SWITCH1, INPUT_PULLUP);  //High or Low  for Limit Switch
    
  pinMode(M2_ENABLE, OUTPUT);  //0-255
  pinMode(M2_INPUT1_FWD, OUTPUT);
  pinMode(M2_INPUT2_REV, OUTPUT);
  pinMode (LIMIT_SWITCH2, INPUT_PULLUP);  //High or Low for Limit Switch

  pinMode(SWITCH_A_ROCKER, INPUT_PULLUP);  //High or Low for Rocker A Switch
  pinMode(SWITCH_B_ROCKER, INPUT_PULLUP);  //High or Low for Rocker B Switch
}


void loop() {
  // Read the states of the switches and limit switches
  LIMIT_SWITCH1_State = digitalRead(LIMIT_SWITCH1);
  LIMIT_SWITCH2_State = digitalRead(LIMIT_SWITCH2);
  SWITCH_A_ROCKER_State = digitalRead(SWITCH_A_ROCKER);
  SWITCH_B_ROCKER_State = digitalRead(SWITCH_B_ROCKER); // Change to Motor #1's Rocker B Switch

  // Motor #1 control
  if (SWITCH_A_ROCKER_State == LOW && LIMIT_SWITCH1_State == LOW) {
    // If switch A is selected and limit switch 1 is activated, run Motor 1
    digitalWrite(M1_ENABLE, HIGH); // Enable Motor 1
    digitalWrite(M1_INPUT1_FWD, HIGH); // Set Motor 1 to run forward
    digitalWrite(M1_INPUT2_REV, LOW);
  } else {
    // Otherwise, stop Motor 1
    digitalWrite(M1_ENABLE, LOW); // Disable Motor 1
  }

  // Motor #2 control
  if (SWITCH_B_ROCKER_State == LOW && LIMIT_SWITCH2_State == LOW) {
    // If switch B is selected and limit switch 2 is activated, run Motor 2
    digitalWrite(M2_ENABLE, HIGH); // Enable Motor 2
    digitalWrite(M2_INPUT1_FWD, HIGH); // Set Motor 2 to run forward
    digitalWrite(M2_INPUT2_REV, LOW);
  } else {
    // Otherwise, stop Motor 2
    digitalWrite(M2_ENABLE, LOW); // Disable Motor 2
  }
}