2026 - better accel, printing, and more cleanup

platformio.ini

@@ -18,6 +18,8 @@ upload_port = /run/media/amelia/RPI-RP2/
 board_build.core = earlephilhower
 lib_deps = 
 	xreef/PCF8574 library@^2.3.6
-	gbr1/rp2040-encoder-library@^0.1.1
+	gbr1/rp2040-encoder-library@^0.2.0
-	trevorwslee/DumbDisplay Arduino Library@^0.9.841
+	; trevorwslee/DumbDisplay Arduino Library@^0.9.841
 debug_tool = cmsis-dap
+monitor_speed = 115200
+monitor_port = /dev/ttyACM0

src/globals.h

@@ -48,7 +48,7 @@ extern long last_p;
 #define drive_right(e,x) drive_osmc(e,DENABLE1,x,0,ALI1,BLI1,AHI1,BHI1)
 #define drive_left(e,x) drive_osmc(e,DENABLE2,x,0,ALI2,BLI2,AHI2,BHI2)
 
-#define DEADBAND_HALF_WIDTH 10  // Control input deadband radius
+#define DEADBAND_HALF_WIDTH 8  // Control input deadband radius
 #define FAILTIME 200    //Failsafe timeout in milliseconds
 #define DEBUGPRINT(x) SerCommDbg.println(x)
 #define SerCommDbg Serial   //Serial port for debugging info

src/main.cpp

@@ -6,17 +6,17 @@
 #include "MCP3XXX.h"
 #include "PCF8574.h"
 #include "pio_encoder.h"
-#include "dumbdisplay.h"
+// #include "dumbdisplay.h"
-#include "wifidumbdisplay.h"
+// #include "wifidumbdisplay.h"
 #include "zserio.h"
 #include "SerialUART.h"
 
 const char* ssid = "TEST";
 const char* password = "pink4bubble";
-DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
+// DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
-LcdDDLayer *optionsdisplay = NULL;
+// LcdDDLayer *optionsdisplay = NULL;
-SevenSegmentRowDDLayer *sevenSeg; 
+// SevenSegmentRowDDLayer *sevenSeg; 
-JoystickDDLayer *appjoystick;
+// JoystickDDLayer *appjoystick;
 
 packet_t pA, pB, safe;
 packet_t *astate, *incoming;
@@ -36,17 +36,8 @@ int mode = 1;
 char left_enabled = 0, right_enabled = 0;
 int current_offset[2];
 int setup_complete = false;
+float left_power_state = 0.0f, right_power_state = 0.0f;
 
-// driving vars
-int target_left_power = 0;
-int target_right_power = 0;
-int left_power = 0;
-int right_power = 0;
-int acceleration = 1;
-bool turbo = false;
-int left_cooldown = 0;
-int right_cooldown = 0;
-int olddisplay = 99999;
 
 unsigned int getButton(unsigned int num) {
   if (num <= 7) {
@@ -63,21 +54,6 @@ unsigned int getDPad() {
 }
 
 
-void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
-    if (pLayer == optionsdisplay) {
-        // clicked the "clear" button
-        if(turbo) {
-          turbo = false;
-          pLayer->backgroundColor("lightgray");
-        }
-        else {
-          turbo = true;
-          pLayer->backgroundColor("red");
-        }
-        //delay(100);
-    } 
-}
-
 void osmc_init() {
   digitalWrite(ALI1, LOW);
   digitalWrite(BLI1, LOW);
@@ -89,7 +65,6 @@ void osmc_init() {
   digitalWrite(BHI2, LOW);
   digitalWrite(DENABLE1, HIGH);
   digitalWrite(DENABLE2, HIGH);
-  
   pinMode(ALI1, OUTPUT);
   pinMode(AHI1, OUTPUT);
   pinMode(BLI1, OUTPUT);
@@ -100,12 +75,21 @@ void osmc_init() {
   pinMode(BHI2, OUTPUT);
   pinMode(DENABLE1, OUTPUT);
   pinMode(DENABLE2, OUTPUT);
-  //pinMode(DREADY1, INPUT);
+
-  //pinMode(DREADY2, INPUT);
+  digitalWrite(ALI1, LOW);
+  digitalWrite(BLI1, LOW);
+  digitalWrite(AHI1, LOW);
+  digitalWrite(BHI1, LOW);
+  digitalWrite(ALI2, LOW);
+  digitalWrite(BLI2, LOW);
+  digitalWrite(AHI2, LOW);
+  digitalWrite(BHI2, LOW);
+  digitalWrite(DENABLE1, HIGH);
+  digitalWrite(DENABLE2, HIGH);
 
   digitalWrite(22, LOW);
   pinMode(22, OUTPUT);
-  analogWriteFreq(4000); // set PWM frequency to 16kHz
+  analogWriteFreq(4000); // set PWM frequency to 4kHz
 }
 
 char try_enable_osmc(char enabled, char enablepin, float vbatt,
@@ -186,48 +170,46 @@ float get_current(byte sensor) {
  *   |     |
  *   --------- GND
  */
-void drive_osmc(char enabled, char enablepin, int rawpower, char brake,
+inline void drive_osmc(char enabled, char enablepin, int rawpower, char brake,
                 char ali, char bli, char ahi, char bhi) {
   int power = constrain(rawpower, -176, 176); // NOTE - with optocouplers, ~176 becomes 100%
 
   if (!enabled) {
-    digitalWrite(ali, LOW);
+    digitalWriteFast(ali, LOW);
-    digitalWrite(bli, LOW);
+    digitalWriteFast(bli, LOW);
-    digitalWrite(ahi, LOW);
+    digitalWriteFast(ahi, LOW);
-    digitalWrite(bhi, LOW);
+    digitalWriteFast(bhi, LOW);
-    digitalWrite(enablepin, HIGH);
+    digitalWriteFast(enablepin, HIGH);
     return;
   }
   //Stop!
-  if (abs(power) < 5) {
+  if (abs(power) < 1) {
-    digitalWrite(ali, LOW);
+    digitalWriteFast(ali, LOW);
-    digitalWrite(bli, LOW);
+    digitalWriteFast(bli, LOW);
     delayMicroseconds(63);
     if (brake != 0) {
-      digitalWrite(ahi, HIGH);
+      digitalWriteFast(ahi, HIGH);
-      digitalWrite(bhi, HIGH);
+      digitalWriteFast(bhi, HIGH);
     } else {
-      digitalWrite(ahi, LOW);
+      digitalWriteFast(ahi, LOW);
-      digitalWrite(bhi, LOW);
+      digitalWriteFast(bhi, LOW);
     }
     return;
   }
-  //Serial.print("Driving OSMC with power ");
-  //Serial.println(power);
   //Forward!
   if (power > 0) {
-    digitalWrite(bhi, LOW);
+    digitalWriteFast(bhi, LOW);
-    digitalWrite(ali, LOW);
+    digitalWriteFast(ali, LOW);
     delayMicroseconds(63);
-    digitalWrite(ahi, HIGH);
+    digitalWriteFast(ahi, HIGH);
     analogWrite(bli, power);
   }
   //Reverse!
   if (power < 0) {
-    digitalWrite(ahi, LOW); 
+    digitalWriteFast(ahi, LOW); 
-    digitalWrite(bli, LOW);
+    digitalWriteFast(bli, LOW);
     delayMicroseconds(63);
-    digitalWrite(bhi, HIGH);
+    digitalWriteFast(bhi, HIGH);
     analogWrite(ali, abs(power));
   }
 }
@@ -242,11 +224,6 @@ void set_digit(byte digit, byte value)
   Wire.write(0x20 + digit);
   Wire.write(d[value]);
   Wire.endTransmission();
-  //Serial.print("Set digit ");
-  //Serial.print(digit);
-  //Serial.print(" to ");
-  //Serial.println(value);
-  //delay(5000);
 }
 
 void set_raw(byte digit, byte value) {
@@ -265,19 +242,12 @@ void set_blank() {
 void set_hex(byte num) {
   byte digit1 = num; 
   digit1 = digit1 >> 4; // shift right by 4
-  //while (digit1 > 15) {
-  //  digit1 -= 16;
-  //}
   byte digit2 = num;
   while (digit2 > 15) {
     digit2 -= 16;
   }
   set_digit(0, digit1);
   set_digit(1, digit2);
-  if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
-    olddisplay = num;
-    sevenSeg->showHexNumber(num);
-  }
   set_raw(4, 0x00); // clear second dot
 }
 
@@ -292,29 +262,10 @@ void set_dec(byte num) {
   }
   set_digit(0, digit1);
   set_digit(1, digit2);
-  if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
-    olddisplay = num;
-    sevenSeg->showNumber(num);
-  }
   set_raw(4, 0x02); // set second dot
 }
 
-
+void init_display() {
-void setup() {
-  WiFi.noLowPowerMode();
-  rp2040.enableDoubleResetBootloader();
-
-  pinMode(LED_BUILTIN, OUTPUT);
-  pinMode(32+1, OUTPUT);
-  digitalWrite(32+1, LOW); // set SMPS to full power mode (pin connected thru wifi chip)
-  digitalWrite(LED_BUILTIN, HIGH);
-  Serial.begin(115200);
-  //Serial.println("hello!");
-  delay(2000);
-  Serial.println("Initializing RIB subsystems..");
-
-
-  Serial.print("Enabling LED driver..");
   Wire.setSDA(20);
   Wire.setSCL(21);
   Wire.begin();
@@ -335,6 +286,24 @@ void setup() {
   Wire.write(0x07);
   Wire.write(0x00); // disable display test mode
   Wire.endTransmission();
+}
+
+void setup() {
+  WiFi.noLowPowerMode();
+
+  pinMode(LED_BUILTIN, OUTPUT);
+  digitalWrite(LED_BUILTIN, HIGH);
+  pinMode(32+1, OUTPUT);
+  digitalWrite(32+1, LOW); // set SMPS to full power mode (pin connected thru wifi chip)
+  Serial.begin(115200);
+  if(Serial)
+    delay(2000);
+  Serial.println("");
+  Serial.println("Initializing RIB subsystems..");
+
+
+  Serial.print("Enabling LED driver..");
+  init_display();
   Serial.println(" done");
   
 
@@ -345,13 +314,7 @@ void setup() {
   SPI1.setTX(11);
   SPI1.setSCK(10);
   adc.begin(13);
-  //pinMode(13, OUTPUT);
+
-  //pinMode(11, OUTPUT);
-  //pinMode(10, OUTPUT);
-  //digitalWrite(13, HIGH);
-  //digitalWrite(11, HIGH);
-  //digitalWrite(10, HIGH);
-  //adc.begin(13,11,12,10);
   Serial.println(" done");
   delay(20);
 
@@ -360,8 +323,6 @@ void setup() {
   osmc_init();
   Serial.println(" done");
   delay(20);
-  //delay(2000);
-  
 
   bool ioex1p, ioex2p = false;
   Serial.print("Initializing I/O expanders..");
@@ -507,23 +468,6 @@ void setup() {
   Serial.println("/6 tests passed.");
   Serial.println("RIB is ready to go. Starting program.");
   set_blank();
-  /*dumbdisplay.recordLayerSetupCommands();
-  sevenSeg = dumbdisplay.create7SegmentRowLayer(2); // 2 digits
-  appjoystick = dumbdisplay.createJoystickLayer(255, "lr+tb", 1); // max, directions, scale in UI
-  appjoystick->autoRecenter(true);
-  appjoystick->moveToCenter();
-  optionsdisplay = dumbdisplay.createLcdLayer(5, 1);   
-  optionsdisplay->setFeedbackHandler(FeedbackHandler);
-  optionsdisplay->backgroundColor("lightgray");
-  optionsdisplay->print("TURBO");
-  dumbdisplay.configAutoPin(DD_AP_HORI_2(
-                                appjoystick->getLayerId(),
-                                DD_AP_VERT_2(
-                                    sevenSeg->getLayerId(),
-                                    optionsdisplay->getLayerId())));
-  dumbdisplay.playbackLayerSetupCommands("basic");*/
-  //dumbdisplay.configAutoPin(DD_AP_VERT); // auto vertical pin layout
-  setup_complete = true;
   rp2040.wdt_begin(500); // start watchdog with 500ms limit. Safety feature; reset during crash to disable motors!
 }
 
@@ -532,195 +476,112 @@ void setup1() {
     delay(100);
 }
 void print_status() {
-  Serial.print(get_voltage(0));
+  float vr = get_voltage(0);
-  Serial.print("V  ");
+  float vl = get_voltage(1);
-  Serial.print(get_current(0));
-  Serial.print("A  ENC1: ");
-  Serial.print(enc1.getCount());
-  Serial.print("  ENC2: ");
-  Serial.println(enc2.getCount());
 
-  SerComm.print(get_voltage(0));
+  float ar = get_current(0);
-  SerComm.print("V  ");
+  float al = get_current(1);
-  SerComm.print(get_current(0));
-  SerComm.print("A  ENC1: ");
-  SerComm.print(enc1.getCount());
-  SerComm.print("  ENC2: ");
-  SerComm.println(enc2.getCount());
 
+  int pwr = int(roundf(vr * ar + vl * al));
   
+  int er = enc1.getCount();
+  int el = enc2.getCount();
+  SerComm.printf("L %4.1fV %5.1fA ENC %-6d   ", vl, al, el);
+  SerComm.printf("R %4.1fV %5.1fA ENC %-6d   ", vr, ar, er);
+  SerComm.printf("PWR %dW\n", pwr);
 }
 
+static inline float ramp_toward(float current, float target, float max_step)
+{
+    float diff = target - current;
+    if (diff >  max_step) return current + max_step;
+    if (diff < -max_step) return current - max_step;
+    return target;
+}
+
+
+void motor_ramp_update(int16_t target_left, int16_t target_right, float accel, int16_t *out_left, int16_t *out_right) {
+    float tl = constrain((float)target_left,  -255, 255);
+    float tr = constrain((float)target_right, -255, 255);
+ 
+    if (abs(target_left) == 0 && abs(target_right) == 0) { // disable motors when not touching stick + deadzone
+        left_power_state  = 0.0f;
+        right_power_state = 0.0f;
+    } else {
+        float max_step = accel;
+        if (max_step < 0.0f) max_step = 0.0f;
+ 
+        left_power_state  = ramp_toward(left_power_state,  tl, max_step);
+        right_power_state = ramp_toward(right_power_state, tr, max_step);
+    }
+ 
+    // round
+    *out_left  = (int16_t)(roundf(left_power_state));
+    *out_right = (int16_t)(roundf(right_power_state));
+}
+
+
 
 void loop() {
   rp2040.wdt_reset();
   
   comm_parse();
+  bool turbo = false;
   if (getButton(SHOULDER_TOP_RIGHT))
     turbo = true;
   else 
     turbo = false;
-  //const DDFeedback*  fb;
-  /*if (!dumbdisplay.connected() || !WiFi.isConnected()) {
-    target_left_power = 0;
-    target_right_power = 0;
-    Serial.print("Connection lost");
   
-  } else
+  int stickX = (int)(astate->stickX);
-    fb = appjoystick->getFeedback();*/
+  if (abs(stickX) < DEADBAND_HALF_WIDTH)
+    stickX = 0;
     
-  int zeroed_power =    -1 * ((int)(astate->stickX) - 127);
+  int stickY = (int)(astate->stickY);
-  int zeroed_turn =      ((int)(astate->stickY) - 127);
+  if (abs(stickY) < DEADBAND_HALF_WIDTH)
-  
+    stickY = 0;
-  
-  if (true) { //fb != NULL) {
-    //int x = fb->x - 127;
-    //int y = - fb->y + 127;
-    int x = zeroed_turn;
-    int y = zeroed_power;
-    //Serial.print(x);
-    //Serial.print(" ");
-    //Serial.println(y);
-    
-    double rawdriveangle = atan2(x, y);
-    double driveangle = rawdriveangle * 180 / 3.1415926;
-    target_left_power = y;
-    target_right_power = y;
-    
-    target_left_power += x;
-    target_right_power += -x;
-    target_left_power = constrain(target_left_power, -127, 127);
-    target_right_power = constrain(target_right_power, -127, 127);
-    if(turbo) {
-      target_left_power *= 2;
-      target_right_power *= 2;
-    }
-    target_left_power = target_left_power * 0.675;
-    target_right_power = target_right_power * 0.675;
     
+  int zeroed_power =    -1 * (stickX - 127);
+  int zeroed_turn =      (stickY - 127);
+  int target_left_power = 0;
+  int target_right_power = 0;
 
+  int x = zeroed_turn;
+  int y = zeroed_power;
   
+  double rawdriveangle = atan2(x, y);
+  double driveangle = rawdriveangle * 180 / 3.1415926;
+  target_left_power = y;
+  target_right_power = y;
   
+  target_left_power += x;
+  target_right_power += -x;
+  target_left_power = constrain(target_left_power, -127, 127);
+  target_right_power = constrain(target_right_power, -127, 127);
+  if(turbo) {
+    target_left_power *= 2;
+    target_right_power *= 2;
   }
+  target_left_power = target_left_power * 0.72; // factor of opto isolators
+  target_right_power = target_right_power * 0.72;
   
+  float acceleration = 3.0f;
   if(turbo)
-    acceleration = 8;
+    float acceleration = 100000.0f;
-  else
-    acceleration = 3;
 
-  if(left_cooldown > 0)
+  int16_t left_power, right_power;
-    left_cooldown --;
+  motor_ramp_update(target_left_power, target_right_power, acceleration, &left_power, &right_power);
-
-  if(abs(target_left_power) <= 4 && abs(left_power) > 5) {
-    left_power = 0;
-    left_cooldown = 2;
-  }
-  else if(target_left_power >= left_power + acceleration && left_cooldown == 0)
-    left_power += acceleration;
-  else if(acceleration > target_left_power - left_power && left_cooldown == 0)
-    left_power = target_left_power;
-  else if(target_left_power <= left_power - acceleration && left_cooldown == 0)
-    left_power -= acceleration;
-  else if(acceleration > left_power - target_left_power && left_cooldown == 0)
-    left_power = target_left_power;
-
-  if(right_cooldown > 0)
-    right_cooldown --;
-
-  if(abs(target_right_power) <= 4 && abs(right_power) > 5) {
-    right_power = 0;
-    right_cooldown = 2;
-  }
-  else if(target_right_power >= right_power + acceleration && right_cooldown == 0)
-    right_power += acceleration;
-  else if(acceleration > target_right_power - right_power && right_cooldown == 0)
-    right_power = target_right_power;
-  else if(target_right_power <= right_power - acceleration && right_cooldown == 0)
-    right_power -= acceleration;
-  else if(acceleration > right_power - target_right_power && right_cooldown == 0)
-    right_power = target_right_power;
 
   int avg_speed = (abs(right_power) + abs(left_power))/2;
-  //SerComm.println();
   set_hex(avg_speed);
 
   drive_right(right_enabled, right_power);
   drive_left(left_enabled, -left_power);
-  SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power) + " MEM FREE: "+ String(rp2040.getFreeHeap()));
+  // SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power));
-
+  print_status();
-  //if(left_power != target_left_power || right_power != target_right_power)
-
-  
-  
-  //delay(1000);
-  //set_digit(0, 6);
-  //set_digit(0, 10);
-  //set_digit(1, 9);
-  //set_digit(1, 10);
-  //set_digit(2, 8);
-  //set_digit(2, 10);
-  //set_digit(3, 8);
-  //set_digit(3, 10);
-  //set_digit(4, 8);
-  //set_digit(4, 10);
-  /*if (mode == 0) {
-    set_raw(count / 8, count % 8);
-    if (count < 39) {
-      count ++;
-    } else {
-      count = 0;
-      mode = 1;
-      delay(100);
-    }
-  }*/
-  //print_status();
-  //drive_right(right_enabled, 10);
-  //drive_left(left_enabled, 10);
-  /*if (millis() % 3000 > 1500) {
-    set_mosfet(0, LOW);
-    set_mosfet(1, LOW);
-    //ioex2.digitalWrite(7, LOW);
-  }
-  if (millis() % 3000 < 1500) {
-    set_mosfet(0, HIGH);
-    set_mosfet(1, HIGH);
-    //ioex2.digitalWrite(7, HIGH);
-
-  }*/
-  /*if (mode == 1) {
-    set_dec(count);
-    drive_right(right_enabled, count);
-    //set_hex(count);
-    if (count < 40) {
-      count += 5;
-    } else {
-      //count = 0;
-
-      mode = 2;
-    }
-  }
-  if (mode == 2) {
-    set_dec(count);
-    drive_right(right_enabled, count);
-    //set_hex(count);
-    if (count > 5) {
-      count -= 5;
-    } else {
-      //count = 0;
-      
-      mode = 1;
-    }
-  }*/
-  //delay(200);
   delay(50);
-  //DDYield();
-
 }
 int loopcount = 0;
 void loop1() {
-  rp2040.wdt_reset();
-  //drive_left(left_enabled, 255);
-  //digitalWrite(LED_BUILTIN, HIGH);  
   if(loopcount == 20) {
     //print_status();
     loopcount = 0;
@@ -730,9 +591,7 @@ void loop1() {
     delay(25);
     loopcount++;
   }
-  //SerComm.println("update");
   left_enabled = try_enable_left(left_enabled, get_voltage(1));
   right_enabled = try_enable_right(right_enabled, get_voltage(0));
-  //digitalWrite(LED_BUILTIN, LOW);
   delay(25);
 }

src/zserio.cpp

@@ -79,11 +79,12 @@ void comm_parse() {
         cs=COMM_INVALID;
         SerCommDbg.println("Invalid RC packet received!");
       }
+      digitalWrite(LED_BUILTIN,HIGH);
     }
   }
   
   if(millis()-ptime > FAILTIME){
-    digitalWrite(13,LOW);
+    digitalWrite(LED_BUILTIN,LOW);
     //Been too long, copy safe state over active one
     memcpy(astate,&safe,sizeof(packet_t));
     comm_ok=0;