/*
Final Assignment: Angry emotion
  Program controls motors and LED of robot to indicate the robot is angry.
  Its mad 'behavior' is moving each tire separately forward, like it's stomping.
  The LED for this 'behavior' will be flashing red.

Team members: Maddie Lyle and CJ Chavez

Last update: 5/31/2024
 */

// Libraries
#include <Wire.h>
#include <FastLED.h> 


// Declarations
  //*The motors
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
  //*The LED
#define NUM_LEDS 1 
CRGB leds[NUM_LEDS];
#define WSLED 4
  //*For controlling the LED with millis
unsigned long previousMillis = 0;
const long interval = 500; // Interval for LED flashing

// Moves one tire (left) forward to signify stomp
void forwardLeft(uint8_t mySpeed){
  analogWrite(M1A, mySpeed);
  digitalWrite(M1B, LOW);
}

// Moves one tire (right) forward to signify stomp
void forwardRight(uint8_t mySpeed){
  analogWrite(M2A, mySpeed);
  digitalWrite(M2B, LOW);
}

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS); // Initialize FastLED with one LED
  
  // Set the motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);
}

// Continous output of LED flashing and "stomping" of the motors
void loop() {
  unsigned long currentMillis = millis();

  // Conditional which uses millis to simultaneous run LED flashing program with movement from the motors
    //*The LED flashes red and black, or no color
  if (currentMillis - previousMillis >= interval) {
    // Toggle LED state
    if (leds[0] == CRGB::Black) {
      leds[0] = CRGB::Red;
    } else {
      leds[0] = CRGB::Black;
    }
    FastLED.show();
    previousMillis = currentMillis;
  }

  // Move robot forward with a "stomping" motion
  forwardLeft(200); // Left motor forward
  delay(500); // Delay for "stomp" effect
  forwardLeft(0); // Stop left motor
  delay(200); // Delay between "stomps"

  forwardRight(200); // Right motor forward
  delay(500); // Delay for "stomp" effect
  forwardRight(0); // Stop right motor
  delay(200); // Delay between "stomps"
}