diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index e2515bcd..3d206503 100644 --- a/main/CMakeLists.txt +++ b/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}") diff --git a/main/ColorFind.cpp b/main/ColorFind.cpp new file mode 100644 index 00000000..2801135d --- /dev/null +++ b/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 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 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); + } + } +}; \ No newline at end of file diff --git a/main/ColorSensor.cpp b/main/ColorSensor.cpp new file mode 100644 index 00000000..9e691562 --- /dev/null +++ b/main/ColorSensor.cpp @@ -0,0 +1,45 @@ +#include "sdkconfig.h" +#include + +#include +#include +#include +#include + +#include "ColorSensor.h" +#include + +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 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}; +} \ No newline at end of file diff --git a/main/ColorSensor.h b/main/ColorSensor.h new file mode 100644 index 00000000..f0a4a280 --- /dev/null +++ b/main/ColorSensor.h @@ -0,0 +1,24 @@ +#ifndef COLORSENSOR_H +#define COLORSENSOR_H + +#include "sdkconfig.h" +#include + +#include +#include +#include + +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 getColors(); +}; + +#endif \ No newline at end of file diff --git a/main/DistanceSensor.cpp b/main/DistanceSensor.cpp new file mode 100644 index 00000000..6941cc32 --- /dev/null +++ b/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 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}; +} \ No newline at end of file diff --git a/main/DistanceSensor.h b/main/DistanceSensor.h new file mode 100644 index 00000000..f15f3e60 --- /dev/null +++ b/main/DistanceSensor.h @@ -0,0 +1,22 @@ +#ifndef DISTANCESENSOR_H +#define DISTANCESENSOR_H + +#include "sdkconfig.h" +#include + +#include +#include +#include +#include + +class DistanceSensor{ + ESP32SharpIR *IRSensorLeft; + ESP32SharpIR *IRSensorMiddle; + ESP32SharpIR *IRSensorRight; + + public: + DistanceSensor(int pinLeft, int pinMiddle, int pinRight); + std::array getValues(); +}; + +#endif \ No newline at end of file diff --git a/main/Drivetrain.cpp b/main/Drivetrain.cpp new file mode 100644 index 00000000..d6a91a5d --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/main/Drivetrain.h b/main/Drivetrain.h new file mode 100644 index 00000000..d1b94554 --- /dev/null +++ b/main/Drivetrain.h @@ -0,0 +1,19 @@ +#ifndef DRIVETRAIN_H +#define DRIVETRAIN_H + +#include + +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 \ No newline at end of file diff --git a/main/LineFollow.cpp b/main/LineFollow.cpp new file mode 100644 index 00000000..20a79c2f --- /dev/null +++ b/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 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); + } + } + } + + } +}; \ No newline at end of file diff --git a/main/LineSensor.cpp b/main/LineSensor.cpp new file mode 100644 index 00000000..468f8d51 --- /dev/null +++ b/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 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 vals = {sensors[0], sensors[1], sensors[2], sensors[3], sensors[4], sensors[5], sensors[6], sensors[7]}; + return vals; +} \ No newline at end of file diff --git a/main/LineSensor.h b/main/LineSensor.h new file mode 100644 index 00000000..e472477e --- /dev/null +++ b/main/LineSensor.h @@ -0,0 +1,15 @@ +#ifndef LINESENSOR_H +#define LINESENSOR_H + +#include +#include +#include + +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 getValues(); +}; + +#endif \ No newline at end of file diff --git a/main/MazeSolver.cpp b/main/MazeSolver.cpp new file mode 100644 index 00000000..dd9884ab --- /dev/null +++ b/main/MazeSolver.cpp @@ -0,0 +1,51 @@ +#include "Drivetrain.h" +#include "DistanceSensor.h" +#include "Routine.h" + +class MazeSolver : public Routine { + public: + DistanceSensor* distanceSensor; + Drivetrain* drivetrain; + + MazeSolver(DistanceSensor* distanceSensor, Drivetrain* drivetrain){ + this->distanceSensor = distanceSensor; + this->drivetrain = drivetrain; + } + + void update(){ + std::array distances = distanceSensor->getValues(); + + if(distances[1] > 16){ + drivetrain->setSpeed(210, 210); // Move forward + } + else if(distances[0] < 8){ + drivetrain->setSpeed(210, -210); // Turn right + delay(50); + } + else if(distances[2] < 8){ + drivetrain->setSpeed(-210, 210); // Turn left + delay(50); + + } + else{ + bool left = distances[0] > distances[2]; + for(int i = 30; i > 0; i--){ + int medium = distanceSensor->getValues()[1]; + if(left){ + drivetrain->setSpeed(-200, 200); // Turn left + } + else{ + drivetrain->setSpeed(200, -200); // Turn right + } + delay(30); + int newMedium = distanceSensor->getValues()[1]; + if(newMedium > medium){ + continue; + } + else{ + break; + } + } + } + } +}; \ No newline at end of file diff --git a/main/RobotController.cpp b/main/RobotController.cpp new file mode 100644 index 00000000..22865496 --- /dev/null +++ b/main/RobotController.cpp @@ -0,0 +1,199 @@ +#include "sdkconfig.h" +#include +#include +#include +#include "controller_callbacks.h" + +#include "Drivetrain.h" +#include "LineSensor.h" +#include "ColorSensor.h" +#include "Routine.h" +#include "DistanceSensor.h" +#include "LineFollow.cpp" +#include "ColorFind.cpp" +#include "MazeSolver.cpp" +#include + +#define IN1 16 // Control pin 1 +#define IN2 17 // Control pin 2 +#define IN3 19 // Control pin 3 +#define IN4 18 // Control pin 4 + +#define SERVO 13 + +#define OUT1 14 +#define OUT2 12 +#define OUT3 27 +#define OUT4 26 +#define OUT5 25 +#define OUT6 33 +#define OUT7 32 +#define OUT8 35 + +#define ColorSCL 22 // SCL +#define ColorSDA 21 // SDA + +#define IRLeft 34 +#define IRMiddle 39 +#define IRRight 36 + +#define LED 2 + +Servo armServo; +int servoAngle = 90; +int servoDirection = 1; +long lastServoTime = 0; +const int SERVO_STEP = 20; +const int SERVO_MIN = 0; +const int SERVO_MAX = 180; +const unsigned long SERVO_DELAY = 1; + +Drivetrain* drivetrain; +LineSensor* lineSensor; +ColorSensor* colorSensor; +DistanceSensor* distanceSensor; + +Routine* routine; + +extern ControllerPtr myControllers[BP32_MAX_GAMEPADS]; // BP32 library allows for up to 4 concurrent controller connections, but we only need 1 + +void handleController(ControllerPtr myController) { + float forward = static_cast(myController->axisY())/512.0; // L2 analog value for forward + float turn = static_cast(myController->axisX())/512.0; + //turn left is negative X axis, left motor goes back while right motor goes forward + int speedLeft = -(forward - turn) * 255; + int speedRight = -(forward + turn) * 255; + //Simple direction control - no braking, no state tracking + if(routine == nullptr){ + if (myController->r2()) { // ZR button for forward + drivetrain->setSpeed(speedLeft, speedRight); + } + else if (myController->l2()){ + drivetrain->setSpeed(-speedLeft, -speedRight); + } + else{ + drivetrain->setSpeed(0, 0); + } + + if((myController->r1() || myController->l1()) && myController->r2() == false && myController->l2() == false){ + unsigned long now = millis(); + if(now - lastServoTime >= SERVO_DELAY){ + if (myController->r1()) { // ZR button for forward + servoDirection = 1; + } + else if (myController->l1()){ + servoDirection = -1; + } + servoAngle += servoDirection * SERVO_STEP; + + if(servoAngle >= SERVO_MAX){ + servoAngle = SERVO_MAX; + servoDirection = -servoDirection; + } + else if(servoAngle <= SERVO_MIN){ + servoAngle = SERVO_MIN; + servoDirection = -servoDirection; + } + armServo.write(servoAngle); + lastServoTime = now; + } + } + } + + if (myController->x()) { + routine = new ColorFind(colorSensor, drivetrain); + digitalWrite(LED, HIGH); // Turn on LED + Serial.printf("Color Find Routine Started\n"); + } + else if (myController->y()) { + routine = new LineFollow(lineSensor, drivetrain); + digitalWrite(LED, LOW); // Turn off LED + Serial.printf("Line Follow Routine Started\n"); + } + else if (myController->a()) { + routine = new MazeSolver(distanceSensor, drivetrain); + digitalWrite(LED, LOW); // Turn off LED + Serial.printf("Maze Solver Routine Started\n"); + } + else if (myController->b()) { + routine = nullptr; // Stop any routine + digitalWrite(LED, LOW); // Turn off LED + Serial.printf("No Routine Started\n"); + } + +} + +void setup() { + Serial.begin(115200); + uni_bt_allowlist_set_enabled(true); // Enable allowlist first + BP32.setup(&onConnectedController, &onDisconnectedController); + esp_log_level_set("gpio", ESP_LOG_ERROR); // Suppress info log spam from gpio_isr_service + pinMode(IN1, OUTPUT); + pinMode(IN2, OUTPUT); + pinMode(IN3, OUTPUT); + pinMode(IN4, OUTPUT); + + pinMode(LED, OUTPUT); + + pinMode(SERVO, OUTPUT); + + pinMode(OUT1, INPUT); + pinMode(OUT2, INPUT); + pinMode(OUT3, INPUT); + pinMode(OUT4, INPUT); + pinMode(OUT5, INPUT); + pinMode(OUT6, INPUT); + pinMode(OUT7, INPUT); + pinMode(OUT8, INPUT); + + pinMode(ColorSCL, INPUT); + pinMode(ColorSDA, INPUT); + + pinMode(IRLeft, INPUT); + pinMode(IRMiddle, INPUT); + pinMode(IRRight, INPUT); + + drivetrain = new Drivetrain(IN1, IN2, IN3, IN4); + lineSensor = new LineSensor(OUT1, OUT2, OUT3, OUT4, OUT5, OUT6, OUT7, OUT8); + colorSensor = new ColorSensor(ColorSDA, ColorSCL); + distanceSensor = new DistanceSensor(IRLeft, IRMiddle, IRRight); + + armServo.attach(SERVO); + armServo.write(servoAngle); +} + +void loop() { + static unsigned long lastDebugTime = 0; + + BP32.update(); // Update the gamepad state + + // Print debug info every 5 seconds if no controller is connected + unsigned long currentTime = millis(); + if (currentTime - lastDebugTime >= 5000) { + bool anyConnected = false; + for (auto myController : myControllers) { + if (myController && myController->isConnected()) { + anyConnected = true; + Serial.printf("Controller connected - Battery: %d%%\n", myController->battery()); + } + } + if (!anyConnected) { + Serial.println("Waiting for controller connection..."); + } + lastDebugTime = currentTime; + } + + //Handle connected controllers + for (auto myController : myControllers) { + if (myController && myController->isConnected() && myController->hasData()) { + handleController(myController); + } + } + + if(routine != nullptr){ + //Console.printf("Running Routine\n"); + routine->update(); + } + //Console.printf("Running Loop\n"); + vTaskDelay(1); // Small delay to prevent watchdog issues +} diff --git a/main/Routine.h b/main/Routine.h new file mode 100644 index 00000000..f9361025 --- /dev/null +++ b/main/Routine.h @@ -0,0 +1,9 @@ +#ifndef ROUTINE_H +#define ROUTINE_H + +class Routine { +public: + virtual void update(); +}; + +#endif \ No newline at end of file diff --git a/main/Shooter.cpp b/main/Shooter.cpp new file mode 100644 index 00000000..e69de29b diff --git a/main/Teleop.cpp b/main/Teleop.cpp new file mode 100644 index 00000000..e58d6aa2 --- /dev/null +++ b/main/Teleop.cpp @@ -0,0 +1,7 @@ +#include "Routine.h" + +class Teleop : public Routine{ + void update(){ + + } +}; \ No newline at end of file diff --git a/main/arduino_main.cpp b/main/arduino_main.cpp deleted file mode 100644 index 1ae930f3..00000000 --- a/main/arduino_main.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// SPDX-License-Identifier: Apache-2.0 -// Copyright 2021 Ricardo Quesada -// http://retro.moe/unijoysticle2 - -#include "sdkconfig.h" -#include -#include -#include -#include "controller_callbacks.h" - -extern ControllerPtr myControllers[BP32_MAX_GAMEPADS]; // BP32 library allows for up to 4 concurrent controller connections, but we only need 1 - -void dumpGamepad(ControllerPtr ctl) { - Console.printf( - "DPAD: %d A: %d B: %d X: %d Y: %d LX: %d LY: %d RX: %d RY: %d L1: %d R1: %d L2: %d R2: %d\n", - ctl->dpad(), // D-pad - ctl->a(), // Letter buttons - ctl->b(), - ctl->x(), - ctl->y(), - ctl->axisX(), // (-511 - 512) left X Axis - ctl->axisY(), // (-511 - 512) left Y axis - ctl->axisRX(), // (-511 - 512) right X axis - ctl->axisRY(), // (-511 - 512) right Y axis - ctl->l1(), // Bumpers - ctl->r1(), - ctl->l2(), - ctl->r2() - ); -} - -void setup() { - BP32.setup(&onConnectedController, &onDisconnectedController); - BP32.forgetBluetoothKeys(); - esp_log_level_set("gpio", ESP_LOG_ERROR); // Suppress info log spam from gpio_isr_service - uni_bt_allowlist_set_enabled(true); -} - -void loop() { - vTaskDelay(1); // Ensures WDT does not get triggered when no controller is connected - BP32.update(); - for (auto myController : myControllers) { // Only execute code when controller is connected - if (myController && myController->isConnected() && myController->hasData()) { - - /* - ==================== - Your code goes here! - ==================== - */ - - dumpGamepad(myController); // Prints the gamepad state, delete or comment if don't need - } - } -}