push changes

main/CMakeLists.txt

@@ -1,10 +1,10 @@
 set(srcs
         "main.c"
-        "arduino_main.cpp"
+        "RobotController.cpp"
 )
 
 set(requires "bluepad32" "bluepad32_arduino" "arduino" "btstack" "callbacks")
 
-idf_component_register(SRCS "${srcs}"
+idf_component_register(SRCS "MazeSolver.cpp" "ColorFind.cpp" "LineFollow.cpp" "Teleop.cpp" "Shooter.cpp" "DistanceSensor.cpp" "LineSensor.cpp" "Drivetrain.cpp" "RobotController.cpp" "ColorSensor.cpp" "${srcs}"
                     INCLUDE_DIRS "."
                     REQUIRES "${requires}")

main/ColorFind.cpp

@@ -0,0 +1,54 @@
+#include "Routine.h"
+#include "ColorSensor.h"
+#include "Drivetrain.h"
+
+class ColorFind : public Routine{
+    ColorSensor* colorSensor;
+    Drivetrain* drivetrain;
+
+    public:
+    int r = 0;
+    int g = 0;
+    int b = 0;
+    int a = 0;
+
+    ColorFind(ColorSensor* colorSensor, Drivetrain* drivetrain){
+        this->colorSensor = colorSensor;
+        this->drivetrain = drivetrain;
+
+        std::array<int,4> colors = colorSensor->getColors();
+        r = colors[0];
+        g = colors[1];
+        b = colors[2];
+        a = colors[3];
+
+        delay(100);
+
+        colors = colorSensor->getColors();
+        int rVal = std::abs(r - colors[0]);
+        int gVal = std::abs(g - colors[1]);   
+        int bVal = std::abs(b - colors[2]);
+        int aVal = std::abs(a - colors[3]);
+
+        drivetrain->setSpeed(190, 190);
+
+        delay(1000);
+
+        drivetrain->setSpeed(0, 0);
+    }
+
+    void update(){
+        std::array<int,4> colors = colorSensor->getColors();
+        int rVal = std::abs(r - colors[0]);
+        int gVal = std::abs(g - colors[1]);   
+        int bVal = std::abs(b - colors[2]);
+        int aVal = std::abs(a - colors[3]);
+
+        if(rVal < 200 && gVal < 200 && bVal < 200){
+            drivetrain->setSpeed(0, 0);
+        }
+        else{
+            drivetrain->setSpeed(190, 190);
+        }
+    }
+};

main/ColorSensor.cpp

@@ -0,0 +1,45 @@
+#include "sdkconfig.h"
+#include <Arduino.h>
+
+#include <Wire.h>
+#include <Arduino_APDS9960.h>
+#include <bits/stdc++.h>
+#include <array>
+
+#include "ColorSensor.h"
+#include <ArduinoConsole.h>
+
+ColorSensor::ColorSensor(int SDA, int SCL){
+    // create the APDS9960 instance on the heap using the local TwoWire
+    colorSensor = new APDS9960(sensorProtocol, APSD9960INT);
+    //sets up I2C protocol (use constructor parameters for SDA/SCL)
+    sensorProtocol.begin(SDA, SCL, sensorFrequency);
+    //sets up color sensor
+    colorSensor->setInterruptPin(APSD9960INT);
+
+    while (!colorSensor->begin()) {
+        delay(150); // Wait before retrying
+    }
+
+    delay(150);
+}
+std::array<int,4> ColorSensor::getColors(){
+    int r = 0;
+    int g = 0;
+    int b = 0;
+    int a = 0;
+
+
+    while (!colorSensor->colorAvailable()) { // Wait until color is read from the sensor 
+        delay(5); 
+    }
+
+    Serial.println("7");
+
+    colorSensor->readColor(r, g, b, a);
+    delay(50);
+
+    Console.printf("Color Sensor: R: %d G: %d B: %d A: %d\n", r, g, b, a);
+
+    return {r, g, b, a};
+}

main/ColorSensor.h

@@ -0,0 +1,24 @@
+#ifndef COLORSENSOR_H
+#define COLORSENSOR_H
+
+#include "sdkconfig.h"
+#include <Arduino.h>
+
+#include <Wire.h>
+#include <Arduino_APDS9960.h>
+#include <bits/stdc++.h>
+
+class ColorSensor{
+    int SDA;
+    int SCL;
+    int APSD9960INT = 0;
+    int sensorFrequency = 100000;
+    TwoWire sensorProtocol = TwoWire(0);
+    APDS9960 *colorSensor; // pointer to APDS9960 instance
+
+    public:
+    ColorSensor(int pin1, int pin2);
+    std::array<int,4> getColors();
+};
+
+#endif

main/DistanceSensor.cpp

@@ -0,0 +1,17 @@
+#include "DistanceSensor.h"
+
+DistanceSensor::DistanceSensor(int pinLeft, int pinMiddle, int pinRight){
+    IRSensorLeft = new ESP32SharpIR(ESP32SharpIR::GP2Y0A21YK0F, pinLeft);
+    IRSensorMiddle = new ESP32SharpIR(ESP32SharpIR::GP2Y0A21YK0F, pinMiddle);
+    IRSensorRight = new ESP32SharpIR(ESP32SharpIR::GP2Y0A21YK0F, pinRight);
+}
+
+std::array<int,3> DistanceSensor::getValues(){
+    int distanceLeft = IRSensorLeft->getDistance();
+    int distanceMiddle = IRSensorMiddle->getDistance();
+    int distanceRight = IRSensorRight->getDistance();
+
+    Serial.printf("Distance Sensor: Left: %d cm, Middle: %d cm, Right: %d cm\n", distanceLeft, distanceMiddle, distanceRight);
+
+    return {distanceLeft, distanceMiddle, distanceRight};
+}

main/DistanceSensor.h

@@ -0,0 +1,22 @@
+#ifndef DISTANCESENSOR_H
+#define DISTANCESENSOR_H
+
+#include "sdkconfig.h"
+#include <Arduino.h>
+
+#include <Wire.h>
+#include <Arduino_APDS9960.h>
+#include <bits/stdc++.h>
+#include <ESP32SharpIR.h>
+
+class DistanceSensor{
+    ESP32SharpIR *IRSensorLeft;
+    ESP32SharpIR *IRSensorMiddle;
+    ESP32SharpIR *IRSensorRight;
+
+    public:
+    DistanceSensor(int pinLeft, int pinMiddle, int pinRight);
+    std::array<int,3> getValues();
+};
+
+#endif

main/Drivetrain.cpp

@@ -0,0 +1,62 @@
+#include "Drivetrain.h"
+
+Drivetrain::Drivetrain(int pin1, int pin2, int pin3, int pin4) {
+    motorPin1 = pin1;
+    motorPin2 = pin2;
+    motorPin3 = pin3;
+    motorPin4 = pin4;
+
+    frameCount = 0;
+}
+
+void Drivetrain::setSpeed(int left, int right) {
+    left *= -1; // Invert left motor direction
+    right *= -1;
+    //Serial.printf("Left Motor %d Right Motor %d\n", left, right);
+    if (left < 0) {
+        left *= -1;
+        digitalWrite(motorPin1, LOW);
+        analogWrite(motorPin2, left);
+    } 
+    else if(left > 0) {
+        digitalWrite(motorPin2, LOW);
+        analogWrite(motorPin1, left);
+    }
+    else{
+        digitalWrite(motorPin1, LOW);
+        digitalWrite(motorPin2, LOW);
+        analogWrite(motorPin1, 0);
+        analogWrite(motorPin2, 0);
+    }
+
+    if (right < 0) {
+        right *= -1;
+        digitalWrite(motorPin3, LOW);
+        analogWrite(motorPin4, right);
+    } 
+    else if(right > 0){
+        digitalWrite(motorPin4, LOW);
+        analogWrite(motorPin3, right);
+    }
+    else{
+        digitalWrite(motorPin3, LOW);
+        digitalWrite(motorPin4, LOW);
+        analogWrite(motorPin3, 0);
+        analogWrite(motorPin4, 0);
+    }
+
+    if(frameCount == 5){
+        digitalWrite(motorPin1, LOW);
+        digitalWrite(motorPin2, LOW);
+        analogWrite(motorPin1, 0);
+        analogWrite(motorPin2, 0);
+        digitalWrite(motorPin3, LOW);
+        digitalWrite(motorPin4, LOW);
+        analogWrite(motorPin3, 0);
+        analogWrite(motorPin4, 0);
+        frameCount = 0;
+    }
+
+    frameCount++;
+    delay(12);
+}

main/Drivetrain.h

@@ -0,0 +1,19 @@
+#ifndef DRIVETRAIN_H
+#define DRIVETRAIN_H
+
+#include <Arduino.h>
+
+class Drivetrain {
+    int motorPin1;
+    int motorPin2;
+    int motorPin3;
+    int motorPin4;
+
+    int frameCount;
+
+public:
+    Drivetrain(int pin1, int pin2, int pin3, int pin4);
+    void setSpeed(int left, int right);
+};
+
+#endif

main/LineFollow.cpp

@@ -0,0 +1,64 @@
+#include "LineSensor.h"
+#include "Drivetrain.h"
+#include "Routine.h"
+
+class LineFollow : public Routine{
+    LineSensor* lineSensor;
+    Drivetrain* drivetrain;
+
+    int lastValue = 0;
+
+    public:
+    LineFollow(LineSensor* lineSensor, Drivetrain* drivetrain){
+        this->lineSensor = lineSensor;
+        this->drivetrain = drivetrain;
+    }
+
+    void update(){
+        std::array<uint16_t,8> sensors = lineSensor->getValues();
+
+        if(sensors[3] > 750 || sensors[4] > 750){
+            drivetrain->setSpeed(210, 210);
+            lastValue = 0;
+        }
+        else{
+            int left = 0;
+            int right = 0;
+            if(sensors[0] > 750){
+                left++;
+            }
+            if(sensors[1] > 750){
+                left++;
+            }
+            if(sensors[2] > 750){
+                left++;
+            }
+            if(sensors[5] > 750){
+                right++;
+            }
+            if(sensors[6] > 750){
+                right++;
+            }
+            if(sensors[7] > 750){
+                right++;
+            }
+            if(left > right){
+                drivetrain->setSpeed(-210, 210);
+                lastValue = 1;
+            }
+            else if(right > left){
+                drivetrain->setSpeed(210, -210);
+                lastValue = 2;
+            }
+            else{
+                if(lastValue == 1){
+                    drivetrain->setSpeed(-210, 210);
+                }
+                else{
+                    drivetrain->setSpeed(210, -210);
+                }
+            }
+        }
+
+    }
+};

main/LineSensor.cpp

@@ -0,0 +1,24 @@
+#include "LineSensor.h"
+
+LineSensor::LineSensor(uint8_t sensorPin1, uint8_t sensorPin2, uint8_t sensorPin3, uint8_t sensorPin4, uint8_t sensorPin5, uint8_t sensorPin6, uint8_t sensorPin7, uint8_t sensorPin8) {
+    qtr.setTypeAnalog();
+
+    uint8_t pins[8] = {sensorPin1, sensorPin2, sensorPin3, sensorPin4, sensorPin5, sensorPin6, sensorPin7, sensorPin8};
+    qtr.setSensorPins(pins, 8);
+
+    // calibration sequence
+    for (uint8_t i = 0; i < 250; i++) {
+        Console.printf("calibrating %d/250\n", i);
+        qtr.calibrate();
+        delay(20);
+    }
+}
+
+std::array<uint16_t,8> LineSensor::getValues() {
+    uint16_t sensors[8] = {0,0,0,0,0,0,0,0};
+    qtr.readLineBlack(sensors);
+    Console.printf("Line Sensor: S1: %d S2: %d S3: %d S4: %d\n", sensors[0], sensors[1], sensors[2], sensors[3]);
+    delay(30);
+    std::array<uint16_t, 8> vals = {sensors[0], sensors[1], sensors[2], sensors[3], sensors[4], sensors[5], sensors[6], sensors[7]};
+    return vals;
+}

main/LineSensor.h

@@ -0,0 +1,15 @@
+#ifndef LINESENSOR_H
+#define LINESENSOR_H
+
+#include <Arduino.h>
+#include <QTRSensors.h>
+#include <ArduinoConsole.h>
+
+class LineSensor {
+    QTRSensors qtr;
+public:
+    LineSensor(uint8_t sensorPin1, uint8_t sensorPin2, uint8_t sensorPin3, uint8_t sensorPin4, uint8_t sensorPin5, uint8_t sensorPin6, uint8_t sensorPin7, uint8_t sensorPin8);
+    std::array<uint16_t,8> getValues();
+};
+
+#endif