/* Multi Threading Turtle Bot Code Robert Cross and Jackson Root 6/7/2024 This is the version where all the parts of the code run at once. All of the emotions can be triggered at any time based on their priorities. This one doesn't quit work as intended, and its a WIP. I would love to meet with a TA and go over what's wrong! */ #include #include #include #define FASTLED_INTERNAL #include #include // Include the ESP32 LEDC library VL53L0X sensor; #define NUM_LEDS 2 CRGB leds[NUM_LEDS]; #define M2A 17 #define M2B 16 #define M1A 19 #define M1B 18 #define WSLED 4 #define RED_PIN 21 #define YELLOW_PIN 22 #define BLUE_PIN 23 unsigned long currentMillis = 0; // declare time keepers unsigned long emotionTime = 0; unsigned long detectionTime = 0; unsigned long happyTime = 0; unsigned long localTime = 0; int mode = 0; // declares what emotion mode is happening int i = 0; // index variable bool setFlag = 0; // declare flag for angry int distance[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // declare distance variable int avgDistance[2] = {0, 0}; void errorLed(int count = 1) { for (int i = 0; i < count; i++) { leds[0] = CRGB::Red; FastLED.show(); delay(100); leds[0] = CRGB::Black; FastLED.show(); delay(100); } delay(1000); } void reverse(uint8_t mySpeed) { ledcWrite(2, mySpeed); // Set M2A to mySpeed ledcWrite(3, 0); // Set M2B to LOW ledcWrite(0, mySpeed); // Set M1A to mySpeed ledcWrite(1, 0); // Set M1B to LOW } void forward(uint8_t mySpeed) { ledcWrite(3, mySpeed); // Set M2B to mySpeed ledcWrite(2, 0); // Set M2A to LOW ledcWrite(1, mySpeed); // Set M1B to mySpeed ledcWrite(0, 0); // Set M1A to LOW } void cw(uint8_t mySpeed) { ledcWrite(3, mySpeed); // Set M2B to mySpeed ledcWrite(2, 0); // Set M2A to LOW ledcWrite(1, 0); // Set M1B to LOW ledcWrite(0, mySpeed); // Set M1A to mySpeed } void ccw(uint8_t mySpeed) { ledcWrite(2, mySpeed); // Set M2A to mySpeed ledcWrite(3, 0); // Set M2B to LOW ledcWrite(0, 0); // Set M1A to LOW ledcWrite(1, mySpeed); // Set M1B to mySpeed } void setColor(int redValue, int yellowValue, int blueValue) { analogWrite(RED_PIN, redValue); analogWrite(YELLOW_PIN, yellowValue); analogWrite(BLUE_PIN, blueValue); } void setup() { Serial.begin(115200); Serial.println("Initializing..."); randomSeed(analogRead(0)); // Seed the random number generator pinMode(27, INPUT); // declare pins for button in shell pinMode(14, INPUT); FastLED.addLeds(leds, NUM_LEDS); // GRB ordering is assumed // This section creates the I2C (Wire) connection and queries for a VL53L0X Wire.begin(); sensor.setTimeout(500); if (!sensor.init()) { Serial.println("Failed to detect and initialize sensor"); while (1) { errorLed(); } } // Set the 4 motor control lines outputs pinMode(M1A, OUTPUT); pinMode(M1B, OUTPUT); pinMode(M2A, OUTPUT); pinMode(M2B, OUTPUT); // different color LED pins as outputs pinMode(RED_PIN, OUTPUT); pinMode(YELLOW_PIN, OUTPUT); pinMode(BLUE_PIN, OUTPUT); // These lines connect 4 PWM channels to the 4 motor control Pins 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); } void loop() { Serial.println("Looping..."); // troubleshooting currentMillis = millis(); // set current time // SHORT DETECTION LOOP if (currentMillis - detectionTime >= 25) { detectionTime = currentMillis; // reset timer //store a list of past readings for(i = 8; i >= 0; i--){ // shift list up each time measurement is taken distance[i+1] = distance[i]; } distance[0] = sensor.readRangeSingleMillimeters(); // update first distance avgDistance[0] = (distance[0]+distance[1]+distance[2]+distance[3]+distance[4]) / 5; //calculate a current and past average avgDistance[1] = (distance[5]+distance[6]+distance[7]+distance[8]+distance[9]) / 5; if (digitalRead(27) == LOW || digitalRead(14) == LOW) { //check for button press if (mode > 1) { //check if higher priority than current action // Happy: Yellow, Twitching in place mode = 1; // happy movement mode emotionTime = currentMillis; // declare time of call happyTime = currentMillis; //reset internal time } } else if (avgDistance[0] < 50) { // check to see if something is too close if (mode > 2) { //check if higher priority than current action mode = 2; // angry movement emotionTime = currentMillis; //declare time of call } } else if (avgDistance[1] - avgDistance[0] >= 75) { // check if a large change in distance is detected very quickly if(mode > 3) { //check if higher priority than current action // Scared: Purple, Random sporiadic movement mode = 3; //scared movement emotionTime = currentMillis; //declare time of call scaredTime = currentMillis; //reset internal time } } else if (currentMillis - emotionTime > 3000) { //if none of triggers are reactivated after 3 seconds, the robot will return to default state (sad) //Sad: Blue, slow moping around mode = 4; //sad movement emotionTime = currentMillis; //declare time of call } } // EMOTION DEFINITIONS switch (mode) { case 1: //happy leds[0].setRGB(255, 255, 0); //set color to yellow leds[1].setRGB(255, 255, 0); FastLED.show(); //This command makes the LEDs change to match values in the array leds[] if (currentMillis - happyTime < 500) { //if it has been less than 500 ms since happy was called ccw(180); // turn one direction } else if (currentMillis - happyTime < 1000) { //if it has been less than 1 second since happy was called cw(180); // turn other direction } else if (currentMillis - happyTime > 1000) { happyTime = currentMillis; //if its been longer than 1 second reset timer and psuedo loop } break; //exit emotion definition switch case 2: // angry leds[0].setRGB(255, 0, 0); //set color to red leds[1].setRGB(255, 0, 0); FastLED.show(); //This command makes the LEDs change to match values in the array leds[] if(avgDistance[0] < 150) { // if something is way too close localTime = currentMillis; //reset local timer setFlag = 1; //set flag } if (setFlag == 1) { //if flag has been created if (currentMillis - localTime < 200) { //for 200 ms back up reverse(255); }else if (currentMillis - localTime > 200) { // if angry flag has expired setFlag = 0; //reset flag forward(255); //go forward again } } else { //if no flag then just continue forward forward(255); } break; //exit emotion definition switch case 3: //scared leds[0].setRGB(133, 0, 255); //set color to purple leds[1].setRGB(133, 0, 255); FastLED.show(); //This command makes the LEDs change to match values in the array leds[] if (currentMillis - emotionTime < 200) { // first move backwards for 200 ms // Startle Response: Quick backward motion reverse(240); // Move backward at high speed localTime = currentMillis; // reset scared timer } else if (currentMillis - localTime >= random(100, 300)) { //generate random motion for 100 ms to 300 ms over and over after // Rapid random movements to simulate panic int randDirection = random(4); // Randomly choose a direction int randSpeed = random(100, 255); // Random speed between 100 and 255 switch(randDirection) { //move that direciton based on the renadom choice case 0: forward(randSpeed); break; case 1: reverse(randSpeed); break; case 2: ccw(randSpeed); break; case 3: cw(randSpeed); break; } localTime = currentMillis; // reset scared timer } break; //exit emotion definition switch case 4: // sad BROKEN leds[0].setRGB(0, 0, 255); //set color to blue leds[1].setRGB(0, 0, 255); FastLED.show(); //This command makes the LEDs change to match values in the array leds[] // move around using modified bump bot code if(avgDistance[0] < 300) { // if something is too close localTime = currentMillis; //reset local timer setFlag = 1; //create flag } if (setFlag == 1) { //if flag has been previously found if (currentMillis - localTime < 200) { // for 200 ms reverse reverse(200); } else if (currentMillis - localTime < 400) { //for another 200 ms turn cw cw(255); }else if (currentMillis - localTime > 400) { //revers and turn time has expired, reset flag and go forward again setFlag = 0; //reset flag forward(180); } } else { //if no flag found then just continue forward forward(180); } break; //exit emotion definition switch } }