feat: update tuning parameters and improve distance measurement logic

src/config.h

@@ -5,9 +5,11 @@
 // Speed, Kp, Ki, Kd
 struct Tunings {
   const Tuning slow = { 40, 0.05, 0.0, 0.002 };
-  const Tuning main = { 60, 0.07, 0.0, 0.002 }; };
+  const Tuning main = { 60, 0.09, 0.0, 0.003 }; };
 
 const uint8_t SENSORS[] = {
   19, 18, 17, 16, 15, 14, 11, 10, 9 };
-// Can be commented out
-#define IR_PIN 20
+// 0 to disable
+const uint8_t IR_PIN = 20;
+// will get inaccurate under 5cm
+const int IR_THRESHOLD = 10;

src/lib/state.h

@@ -19,12 +19,7 @@ struct StateTimer {
   // written by chatgpt
   // independant of the timer, for blocking waits
   template<typename F>
-  bool loop(uint32_t ms, F callback) {
+  void loop(uint32_t ms, F callback) {
     unsigned long start = millis();
 
-    if (millis() - start >= ms) {
-      start = millis();
-      callback();
-      return true; }
-
-    return false; } };
+    while (millis() - start < ms) callback(); } };

src/main.cpp

@@ -38,14 +38,15 @@ void loop() {
     printCalibration();
     Serial.println(F("Calibration done")); }
   else {
-    delay(1000);
+    delay(100);
 
     StateTimer timer;
     enum LoopState state = START;
     timer.reset();
 
     while(state != END) {
-      printDistance();
+      // printDistance();
+      scale = min(1.0, 0.5 + (float)getDistance() / 50.0 / 2.0);
 
       switch (state) {
       case START:
@@ -60,7 +61,7 @@ void loop() {
       case TRACK:
         track(tunings.main);
 
-        if(getDistance() < 10) {
+        if(getDistance() < IR_THRESHOLD) {
           state = AVOIDANCE;
           timer.reset(); }
 
@@ -71,29 +72,32 @@ void loop() {
         break;
 
       case AVOIDANCE: {
-        bk(-50);
-        delay(50);
+        bk(100);
+        delay(150);
         fd(0);
         unsigned long startTime = millis();
         sl(30);
 
-        while (getDistance() < 10)
+        while (getDistance() < IR_THRESHOLD)
           delay(10);
 
         unsigned long endTime = millis();
 
         // double the time (first half gets left side out
         // of way, this one gets right side out of way)
-        while(millis() - endTime < 2 * (endTime - startTime));
+        while(millis() - endTime < max(endTime - startTime, (unsigned long)100));
 
-        fd2(50, 30);
+        fd2(70, 30);
 
         do line(); while (sensors[SENSOR_COUNT / 2] < 500);
 
         timer.loop(200, []() {
           track(tunings.slow); });
+        // cooldown
+        timer.loop(1000, []() {
+          track(tunings.main); });
         state = TRACK;
-        // timer.reset();
+        timer.reset();
         break; }
 
       default:

src/modules/avoidance.h

@@ -2,28 +2,19 @@
 #include <lib/filter.h>
 #include <config.h>
 
-#ifdef IR_PIN
 PT1 irFilter;
 
-float getDistanceCM() {
-  int raw = analogRead(IR_PIN);
+float getDistance() {
+  int raw = irFilter.filter(analogRead(IR_PIN));
 
   if(raw < 15) return 999;
 
   return 2100.0 / (raw - 10.0); }
 
-float getDistance() {
-  return irFilter.filter(getDistanceCM()); }
-
 void avoidanceSetup() {
   pinMode(IR_PIN, INPUT); }
 
 void printDistance() {
   uint8_t distance = getDistance();
   Serial.print(F("IR Raw: ")); Serial.print(analogRead(IR_PIN));
-  Serial.print(F("\tDistance: ")); Serial.println(distance); }
-#else
-void avoidanceSetup() {}
-void printDistance() {
-  Serial.print(F("Avoidance sensor is not configured")); }
-#endif
+  Serial.print(F("\tDistance: ")); Serial.println(distance); }

src/modules/motor.h

@@ -2,14 +2,20 @@
 #include <modules/input.h>
 #include <QuickPID.h>
 #include <us82_motor.h>
-float input, output, setpoint, leftmotor, rightmotor, currSpeed;
+#include <lib/filter.h>
+float input, output, setpoint, leftmotor, rightmotor, currSpeed, scale = 1.001;
 
 QuickPID pid = QuickPID(&input, &output, &setpoint, 0, 0, 0, QuickPID::Action::reverse);
 
+PT1 scaleFilter(1);
+
 void pidSetup(Tuning tuning) {
   if(tuning.speed == currSpeed)
     return;
 
+  Serial.print("Updating speed: ");
+  Serial.println(tuning.speed);
+
   currSpeed = tuning.speed;
   pid.SetMode(QuickPID::Control::automatic);
   pid.SetTunings(tuning.kp, tuning.ki, tuning.kd);
@@ -32,7 +38,10 @@ void track(Tuning tuning) {
   // Serial.print("Input: "); Serial.print(input); Serial.print("\tOutput: "); Serial.print(output);
   // Serial.print("\tLeft Motor: "); Serial.print(leftmotor); Serial.print("\tRight Motor: "); Serial.println(rightmotor);
 
-  fd2(leftmotor, rightmotor); }
+  // if scale is not controlled, disable the filter
+  float smoothScale = scale < 1.001 ? scaleFilter.filter(scale) : 1.001;
+
+  fd2(constrain(leftmotor * smoothScale, -100, 100), constrain(rightmotor * smoothScale, -100, 100)); }
 
 void track(Tuning tuning, unsigned long time) {
   const unsigned long startTime = millis();