//Control code for Hexabot
//By Rich Pantaleo

int fwdpin = 2; //Set the pin for the joystick switch in the forward position
int revpin = 3; //Set the pin for the joystick switch in the reverse position
int leftpin = 4; //Set the pin for the joystick switch in the left position
int rightpin = 5; //Set the pin for the joystick switch in the right position
int leftRelay1 = 9; //Set a pin for the transistor controlling one of the relays in the h-bridge for the left motor
int leftRelay2 = 10; //Set a pin for the transistor controlling the other relay in the h-bridge for the left motor
int rightRelay1 = 11; //Set a pin for the transistor controlling one of the relays in the h-bridge for the right motor
int rightRelay2 = 12; //Set a pin for the transistor controlling the other relay in the h-bridge for the left motor
int LED = 13; //Set a pin for the LED
int modeSwitch = 7; //Set the pin for the switch to change modes
int pushButton = 6; //Set the pine for the push button
int fwd, rev, left, right, mode, buttonVal; //Declare variables for reading inputs
int buttonCount = 0; //Counter to count button presses
boolean buttonDown = false; //Flag to store if the button has been pushed or not


void setup(){
  pinMode(fwdpin, INPUT);//Set inputs and outputs
  pinMode(revpin, INPUT);
  pinMode(leftpin, INPUT);
  pinMode(rightpin, INPUT);
  pinMode(leftRelay1, OUTPUT);
  pinMode(leftRelay2, OUTPUT);
  pinMode(rightRelay1, OUTPUT);
  pinMode(rightRelay2, OUTPUT);
  pinMode(modeSwitch, INPUT);
  pinMode(pushButton, INPUT);
  pinMode(LED, OUTPUT);
  digitalWrite(pushButton, LOW); //Set the value of the push button pin to low
  digitalWrite(modeSwitch, LOW); //Set the value of the mode switch pin to low
  digitalWrite(leftRelay1, LOW); //Turn off all of the relays
  digitalWrite(leftRelay2, LOW);
  digitalWrite(rightRelay1, LOW);
  digitalWrite(rightRelay2, LOW);
}

void loop(){
  digitalWrite(LED, HIGH); //Turn on the LED
  fwd = digitalRead(fwdpin); //Read the value of the fwdpin
  rev = digitalRead(revpin); //Read the value of the revpin
  left = digitalRead(leftpin); //Read the value of the leftpin
  right = digitalRead(rightpin); //Read the value of the rightpin
  mode = digitalRead(modeSwitch); //Read the value of the mode switch
  motorsoff();
  


if (mode == HIGH){//If the mode switch is turned on
    buttonVal = digitalRead(pushButton); //Read the value of the push button

    if (buttonVal == HIGH && buttonDown == false){ //If the button is being pressed
      buttonDown = true;                          //The button is down
      buttonCount = buttonCount  + 1;             //Add 1 to the counter
              
    }
    if (buttonVal == LOW){ //If the push button pin value is low
      buttonDown = false;  //The button is no longer being pressed
    }
      
    if (buttonCount == 5){ //If the button has been pressed five times
    digitalWrite(LED, LOW);//Blink the LED for ten seconds
    delay(1000);
    digitalWrite(LED, HIGH);
    delay(1000);  
    digitalWrite(LED, LOW);
    delay(1000);
    digitalWrite(LED, HIGH);
    delay(1000);  
    digitalWrite(LED, LOW);
    delay(1000);
    digitalWrite(LED, HIGH);
    delay(1000);  
    digitalWrite(LED, LOW);
    delay(1000);
    digitalWrite(LED, HIGH);
    delay(1000);  
    digitalWrite(LED, LOW);
    delay(1000);
    digitalWrite(LED, HIGH);
    delay(1000);  
    
    motorsforward(); //Move the robot forward for 3 seconds
    delay(3000);
    motorsleft(); //Spin the robot left for 2 seconds
    delay(2000);
    motorsrevright(); //Turn the robot right by reversing the right motor for 2 seconds
    delay(2000);
    motorsbackward(); //Move the robot backward for 3 seconds
    delay(2000);
    motorsright(); //Spin the robot right for 2 seconds
    delay(3000);
    motorsoff(); //Turn the motors off
    
    buttonCount = 0; //Reset the counter
  }    
  }


  
if (mode == LOW){ //If the mode switch is not flipped 

  if (fwd == HIGH && rev == LOW && left == LOW && right == LOW){ //if the joytick is in the forward position
    motorsforward(); //move the robot forward
  }
  else if (fwd == HIGH && rev == LOW && left == LOW && right == HIGH){ //if the joystick is in the slight right forward position
    motorsfwdright(); //Turn the robot right by using the left motor
  }
  else if (fwd == HIGH && rev == LOW && left == HIGH && right == LOW){ //if the joystick is in the slight left foward position
    motorsfwdleft(); //Turn the robot slightly to the left using the right motor
  }
  else if (right == HIGH && left == LOW && fwd == LOW && rev == LOW){ //if the joystick is in the hard right position
    motorsright(); //Spin the robot to the right
  }
  else if (left == HIGH && right == LOW && fwd == LOW && rev == LOW){ //if the joystick is in the hard left position
    motorsleft(); //Spin the robot to the left
  }
  else if (rev == HIGH && fwd == LOW && left == LOW && right == LOW){ //if the joystick is in the reverse position
    motorsbackward(); //Move the robot backwards
  }
  else if (rev == HIGH && fwd == LOW && left == LOW && right == HIGH){ //if the joystick is in the slight right reverse position
    motorsrevright(); //Turn the robot right by using the right motor in reverse
  }
  else if (rev == HIGH && fwd == LOW && left == HIGH && right == LOW){ //if the joystick is in the slight left reverse position
    motorsrevleft(); //Turn the robot left by using the left motor in reverse
  }
  else{ //if the joystick is not in the center position
    motorsoff(); //Turn the motors off
  }
  }
  

}

void motorsoff(){ //Subroutine to turn the motors off
  digitalWrite(leftRelay1, LOW);
  digitalWrite(rightRelay1, LOW);
  digitalWrite(leftRelay2, LOW);
  digitalWrite(rightRelay2, LOW);
}
 
void motorsforward(){ //subroutine to make the robot move forward by spinning the left and right motors forward
  digitalWrite(leftRelay1, HIGH);
  digitalWrite(rightRelay1, HIGH);
  digitalWrite(leftRelay2, LOW);
  digitalWrite(rightRelay2, LOW);

}

void motorsbackward(){ //subroutine to make the robot move backward by spinning the left and right motors backward
  digitalWrite(leftRelay1, LOW); //Turn this relay in the left motor h-bridge off
  digitalWrite(rightRelay1, LOW); //Turn this relay in the rgiht motor h-bridge off
  digitalWrite(leftRelay2, HIGH); //Turn this relay in the left motor h-bridge on
  digitalWrite(rightRelay2, HIGH); //Turn this relay in the right motor h-bridge on
}

void motorsleft(){ //subroutine to make the robot move left by spinning the left motor backward and the right motors forward
  digitalWrite(leftRelay1, LOW);
  digitalWrite(rightRelay1, HIGH);
  digitalWrite(leftRelay2, HIGH);
  digitalWrite(rightRelay2, LOW);
}

void motorsright(){ //subroutine to make the robot move right by spinning the right motor backward and the left motors forward
  digitalWrite(leftRelay1, HIGH);
  digitalWrite(rightRelay1, LOW);
  digitalWrite(leftRelay2, LOW);
  digitalWrite(rightRelay2, HIGH);
}

void motorsfwdright(){ //subroutine to move the robot slightly forward to the right by spinning the left motor forward
  digitalWrite(leftRelay1, HIGH);
  digitalWrite(rightRelay1, LOW);
  digitalWrite(leftRelay2, LOW);
  digitalWrite(rightRelay2, LOW);
}

void motorsfwdleft(){ //subroutine to move the robot slightly forward to the left by spinning the right motor forward
  digitalWrite(leftRelay1, LOW);
  digitalWrite(rightRelay1, HIGH);
  digitalWrite(leftRelay2, LOW);
  digitalWrite(rightRelay2, LOW);
}

void motorsrevleft(){ //subroutine to move the robot slightly backward to the left by spinning the left motor backward
  digitalWrite(leftRelay1, LOW);
  digitalWrite(rightRelay1, LOW);
  digitalWrite(leftRelay2, HIGH);
  digitalWrite(rightRelay2, LOW);
}

void motorsrevright(){ //subroutine to move the robot slightly backward to the right by spinning the right motor backward
  digitalWrite(leftRelay1, LOW);
  digitalWrite(rightRelay1, LOW);
  digitalWrite(leftRelay2, LOW);
  digitalWrite(rightRelay2, HIGH);
}