/*
Final Assignment: Happy emotion
  Program makes the robot spin in circles and flash LED to indicate happiness.
  The happy 'behavior' is alternating spinning right and left approximately every 5 seconds.
  This makes the robot appear to be happily spinning in circles.
  The LED is also flashing yellow to simulate this 'behavior'.

Team members: Maddie Lyle and CJ Chavez

Last update: 6/7/2024
*/

//initial motor setup
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

#define SLOWEST_SPEED 190

//Library setup
#include <Wire.h>
#include <FastLED.h>


//lighting setup
#define NUM_LEDS 1
CRGB leds[NUM_LEDS];
#define WSLED 4

//setup for counters that use millis
unsigned long initialTime = 0;
unsigned long firstLastTime = 0;
unsigned long secondLastTime = 0;

void leftSpin(uint8_t left, uint8_t right){ //function that makes the robot spin counterclockwise
  left = map(left, 128, 0, SLOWEST_SPEED, 255); //these 3 lines set left to reverse
  ledcWrite(3, left);  //sets M2B to pwm
  ledcWrite(2, 0);  //sets M2A to LOW
  right = map(right, 128, 0, SLOWEST_SPEED, 255); //these 3 lines set right to forward
  ledcWrite(1, 0); //sets M1B to LOW
  ledcWrite(0, right); //sets M1A to pwm
}

void rightSpin(uint8_t left, uint8_t right){  //function that makes the robot spin clockwise
  right = map(right, 128, 0, SLOWEST_SPEED, 255); //these 3 lines set right to reverse
  ledcWrite(1, right);  //sets M1B to pwm
  ledcWrite(0, 0);  //sets M1A to LOW
  left = map(left, 128, 0, SLOWEST_SPEED, 255); //these 3 lines set left to forward
  ledcWrite(3, 0); //sets M2B to LOW
  ledcWrite(2, left); //sets M2A to pwm
}

void setup() {
  //setup for motor
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  ledcSetup(0, 30000, 8);
  ledcAttachPin(M1A, 0);
  ledcSetup(1, 30000, 8);
  ledcAttachPin(M1B, 1);
  ledcSetup(2, 30000, 8);
  ledcAttachPin(M2A, 2);
  ledcSetup(3, 30000, 8);
  ledcAttachPin(M2B, 3);
  
  //serial monitor setup
  Serial.begin(115200);

  //final LED setup
  FastLED.addLeds<NEOPIXEL, WSLED> (leds, NUM_LEDS);
}

void loop() {
  unsigned long millisNow = millis(); //measures current time

  if((millisNow - initialTime) < 5000){ //starts program w/ robot spinning left
    leftSpin(256, 256);
  }
  if((millisNow - firstLastTime) >= 5000){  //checks if it's been 5 seconds since last change in direction
    if(ledcRead(3) != 0){ //checks if already spinning left
      rightSpin(256, 256);
    }
    else if(ledcRead(3) == 0){ //checks if already spinning right
      leftSpin(256, 256);
    }
    firstLastTime = millisNow;  //updates counter so the program has to recount 5 seconds before switching directions
  }
  if((millisNow - secondLastTime) >= 500){  //checks if it's been .5 seconds since last change in lighting
    if(leds[0] == CRGB::Black){ //switches from dark to yellow lighting
      leds[0] = CRGB::Yellow;
    }
    else{ //switches from yellow to dark lighting
      leds[0] = CRGB::Black;
    }
    FastLED.show();
    secondLastTime = millisNow; //counter update for lighting
  }
}