/*
Kellen Swanson and Beck Johnson
6/5/24
Emotion Challenge Bot Code 
*/

//Each motor (M1 and M2) has two pins connected.
//Setting one pin HIGH and the other LOW will make the motor turn one way
//Setting them in reverse will make it turn the opposite way
//Seting them both HIGH or both LOW stops the motor
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

//This function sets B LOW and A LOW for both motors
void off(){
 digitalWrite(M2B, LOW); //Set M2B to LOW
 digitalWrite(M2A, LOW); //Set M2A to LOW
 digitalWrite(M1B, LOW);//Set M1B to LOW
 digitalWrite(M1A, LOW); //Set M1A to LOW
}

//This function sets B HIGH and A LOW for both motors
void forward(){
 digitalWrite(M2B, HIGH); //Set M2B to HIGH
 digitalWrite(M2A, LOW); //Set M2A to LOW
 digitalWrite(M1B, HIGH);//Set M1B to HIGH
 digitalWrite(M1A, LOW); //Set M1A to LOW
}

//This function sets B LOW and A HIGH for both motors
void reverse(){
 digitalWrite(M2B, LOW); //Set M2B to LOW
 digitalWrite(M2A, HIGH); //Set M2A to HIGH
 digitalWrite(M1B, LOW);//Set M1B to LOW
 digitalWrite(M1A, HIGH); //Set M1A to HIGH
}

//This function sets B HIGH and A LOW for M2 and the opposite for M1
void ccw(){
 digitalWrite(M2B, HIGH); //Set M2B to HIGH
 digitalWrite(M2A, LOW); //Set M2A to LOW
 digitalWrite(M1B, LOW);//Set M1B to LOW
 digitalWrite(M1A, HIGH); //Set M1A to HIGH
}

//This function sets B HIGH and A LOW for M1 and the opposite for M2
void cw(){
 digitalWrite(M2B, LOW); //Set M2B to LOW
 digitalWrite(M2A, HIGH); //Set M2A to HIGH
 digitalWrite(M1B, HIGH);//Set M1B to HIGH
 digitalWrite(M1A, LOW); //Set M1A to LOW
}

//Code for robot shifts to the right
void shiftr(){
 digitalWrite(M2B, LOW); 
 digitalWrite(M2A, HIGH); 
 digitalWrite(M1B, LOW);
 digitalWrite(M1A, LOW);
}

//Code for robot shifting left
void shiftl(){
 digitalWrite(M2B, LOW); 
 digitalWrite(M2A, LOW); 
 digitalWrite(M1B, LOW);
 digitalWrite(M1A, HIGH);
}

//Code for the happy emotion
void happy(){
  digitalWrite(33, HIGH); //Green light on always to imitate frolicking in a field
  forward();
  delay(1000);
  cw();
  delay(1500);
  forward();
  delay(500);
  ccw();
  delay(1500);
  cw();
  delay(1300);
  reverse();
  delay(300);
  cw();
  delay(1000);
  off();
  digitalWrite(33, LOW);
}


//Code for sad emotion
void sad(){
  forward();
  digitalWrite(32, HIGH); //Continuously turns blue light on and off like the robot is crying
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  forward();
  digitalWrite(32, HIGH); 
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
  delay(500); 
  forward();
  digitalWrite(32, HIGH);
  delay(100);
  off();
  digitalWrite(32, LOW);
}

//Code for scared emotion
void scared(){
  shiftr();
  digitalWrite(25, HIGH); //Red light on, blue light off
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH); //Red light off, blue light on
  digitalWrite(25, LOW); //Repeats this order of light swithcing for whole emotion
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH); 
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH); 
  digitalWrite(25, LOW); 
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH); 
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH); 
  digitalWrite(25, LOW); 
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH); 
  digitalWrite(25, LOW); 
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  shiftr();
  digitalWrite(25, HIGH);
  digitalWrite(32, LOW);
  delay(100);
  off();
  delay(200);
  shiftl();
  digitalWrite(32, HIGH);
  digitalWrite(25, LOW);
  delay(100);
  off();
  delay(200);
  digitalWrite(32, LOW);
  digitalWrite(25, LOW); //Turns both lights off
}


//Code for the angry emotion
void angry(){
  digitalWrite(25, HIGH); //Sets the lights to red
  cw();
  delay(100);
  off();
  delay(500);
  cw();
  delay(100);
  off();
  delay(500);
  cw();
  delay(100);
  off();
  delay(500);
  cw();
  delay(100);
  off();
  delay(500);
  cw();
  delay(100);
  off();
  delay(500);
  cw();
  delay(100);
  off();
  delay(500);
  forward();
  delay(1000);
  off();
  digitalWrite(25, LOW); //Turns the red light off
}

void setup() {
  // put your setup code here, to run once:
  
  // Set the 4 motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);
  
  pinMode(32, OUTPUT);
  pinMode(33, OUTPUT);
  pinMode(25, OUTPUT);
  digitalWrite(32, LOW);
  digitalWrite(33, LOW);
  digitalWrite(25, LOW);
  //32 Writes to blue
  //33 Writes to green
  //25 writes to red
  
  Serial.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:

  //Extra time to set the robot down safely
  delay(5000);
  
  //Calling Happy emotion
  happy();
  delay(5000);

  //Calling Sad emotion
  sad();
  delay(5000);

  //Calling Scared emotion
  scared();
  delay(5000);

  //Calling Angry emotion
  angry();
  delay(5000);

}