Detect tiles using light sensors

.gitignore

@@ -5,3 +5,4 @@
 .vscode/settings.json
 .vscode/ipch
 env.h
+/.history

include/robot/control.h

@@ -9,10 +9,18 @@ void drive(float leftPower, float rightPower, std::string id);
 void driveTicks(int tickDistance, std::string id);
 void turn(float angleRadians, std::string id);
 void stop();
-void readLight();
+void readLight(int loopDelayMs);
 void startMotorAndEncoderTest();
+bool checkMoveFinished();
 void startDriveTest();
+bool checkIfCanUpdateMovement();
+void createDriveUntilNewTile();
+void determineNextChoice();
+uint8_t driveUntilNewTile();
 void driveTestOff();
-void controlLoop(int loopDelayMs);
+void updateCritRange();
+void controlLoop(int loopDelayMs, int8_t framesUntilPrint);
+void updateCentering();
+void updateToNextDistance();
 
 #endif

include/robot/lightSensor.h

@@ -5,9 +5,10 @@
 #include "Arduino.h"
 
 void setupIR();
+void readGarbageVals();
 void activateIR();
 void deactivateIR();
-void startLightReading();
+void startLightReading(bool* onFirstTile, bool* waitingForLight);
 void readLightLevels();
 
 #endif

include/robot/trapezoidalProfile.h

@@ -1,3 +1,5 @@
+#include <cstdint>
+
 struct MotionProfile {
     double maxVelocity;
     double maxAcceleration;
@@ -7,7 +9,7 @@ struct MotionProfile {
     double targetVelocity;
 };
 
-double updateTrapezoidalProfile(MotionProfile &profile, double dt);
+double updateTrapezoidalProfile(MotionProfile &profile, double dt, int8_t framesUntilprint, int critRange);
 
 template <typename T>
 T clamp(T value, T minValue, T maxValue) {

include/utils/config.h

@@ -25,6 +25,8 @@ extern gpio_num_t PHOTODIODE_B_PIN;
 extern gpio_num_t PHOTODIODE_C_PIN;
 extern gpio_num_t PHOTODIODE_D_PIN;
 
+extern gpio_num_t ONBOARD_LED_PIN;
+
 extern int TICKS_PER_ROTATION;
 extern float TRACK_WIDTH_INCHES;
 extern float WHEEL_DIAMETER_INCHES;

include/utils/timer.h

@@ -3,6 +3,7 @@
 
 // Built-In Libraries
 #include <vector>
+//we're gonna need this if we need to use delays which require functions
 #include <functional>
 
 // This callback allows you to just pass in the pointer of a function as a parameter

src/main.cpp

@@ -18,6 +18,10 @@
 
 int delayMilliseconds = 20;
 
+//alright SCREW YOU serial monitor i won't print every frame then if you wanna play that game
+const int8_t PRINT_INTERVAL = 60;
+int8_t framesUntilPrint = 60;
+
 // Setup gets run at startup
 void setup() {
     // Serial port for debugging purposes
@@ -35,6 +39,9 @@ void setup() {
 
     if (DO_DRIVE_TEST) startDriveTest();
 
+    delay(2000);
+
+    //start reading the light
     if (DO_DRIVE_TICKS_TEST) driveTicks(20000, "NULL");
 
     if (DO_HARDWARE_TEST) timerDelay(5000, &startMotorAndEncoderTest);
@@ -45,22 +52,33 @@ void loop() {
     // Checks if any timers have expired
     timerStep();
 
-    if (!RUN_OFFLINE) {
+    if (!RUN_OFFLINE) 
+    {
         // Checks whether bot is still connected to WiFi. Reconnect if not
         if (getWiFiConnectionStatus() && !checkWiFiConnection()) reconnectWiFI();
         // Checks whether bot is still connected to the server. Reconnect if not
         if (getServerConnectionStatus() && !checkServerConnection()) reconnectServer();
 
+        // If the bot is connected to the server, check for received data, and accept it if available
+        if (getServerConnectionStatus()) acceptData();
+        // Checks whether bot is still connected to the server. Reconnect if not
+        if (getServerConnectionStatus() && !checkServerConnection()) reconnectServer();
+
         // If the bot is connected to the server, check for received data, and accept it if available
         if (getServerConnectionStatus()) acceptData();
     }
 
     // Run control loop
-    controlLoop(delayMilliseconds);
+    controlLoop(delayMilliseconds, framesUntilPrint);
 
     // This delay determines how often the code in loop is run
     // (Forcefully pauses the thread for about the amount of milliseconds passed in)
   	delay(delayMilliseconds);
+    framesUntilPrint--;
+    if(framesUntilPrint < 0)
+    {
+        framesUntilPrint = PRINT_INTERVAL;
+    }
 }
 
 // This is used at the end of each file due to the name definition at the beginning