From c480518b7c39e5552fc66b435490dcc39493cc0f Mon Sep 17 00:00:00 2001 From: Amelia Deck Date: Wed, 7 Oct 2026 12:57:02 -0500 Subject: [PATCH] 2026 - better accel, printing, and more cleanup --- platformio.ini | 6 +- src/globals.h | 2 +- src/main.cpp | 407 ++++++++++++++++--------------------------------- src/zserio.cpp | 3 +- 4 files changed, 140 insertions(+), 278 deletions(-) diff --git a/platformio.ini b/platformio.ini index e29f1f5..33c7ac4 100644 --- a/platformio.ini +++ b/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 - trevorwslee/DumbDisplay Arduino Library@^0.9.841 + gbr1/rp2040-encoder-library@^0.2.0 + ; trevorwslee/DumbDisplay Arduino Library@^0.9.841 debug_tool = cmsis-dap +monitor_speed = 115200 +monitor_port = /dev/ttyACM0 \ No newline at end of file diff --git a/src/globals.h b/src/globals.h index 2069a01..d6b38cc 100644 --- a/src/globals.h +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index 49636c2..b3c526b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,17 +6,17 @@ #include "MCP3XXX.h" #include "PCF8574.h" #include "pio_encoder.h" -#include "dumbdisplay.h" -#include "wifidumbdisplay.h" +// #include "dumbdisplay.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); -LcdDDLayer *optionsdisplay = NULL; -SevenSegmentRowDDLayer *sevenSeg; -JoystickDDLayer *appjoystick; +// DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192); +// LcdDDLayer *optionsdisplay = NULL; +// SevenSegmentRowDDLayer *sevenSeg; +// 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); - digitalWrite(bli, LOW); - digitalWrite(ahi, LOW); - digitalWrite(bhi, LOW); - digitalWrite(enablepin, HIGH); + digitalWriteFast(ali, LOW); + digitalWriteFast(bli, LOW); + digitalWriteFast(ahi, LOW); + digitalWriteFast(bhi, LOW); + digitalWriteFast(enablepin, HIGH); return; } //Stop! - if (abs(power) < 5) { - digitalWrite(ali, LOW); - digitalWrite(bli, LOW); + if (abs(power) < 1) { + digitalWriteFast(ali, LOW); + digitalWriteFast(bli, LOW); delayMicroseconds(63); if (brake != 0) { - digitalWrite(ahi, HIGH); - digitalWrite(bhi, HIGH); + digitalWriteFast(ahi, HIGH); + digitalWriteFast(bhi, HIGH); } else { - digitalWrite(ahi, LOW); - digitalWrite(bhi, LOW); + digitalWriteFast(ahi, LOW); + digitalWriteFast(bhi, LOW); } return; } - //Serial.print("Driving OSMC with power "); - //Serial.println(power); //Forward! if (power > 0) { - digitalWrite(bhi, LOW); - digitalWrite(ali, LOW); + digitalWriteFast(bhi, LOW); + digitalWriteFast(ali, LOW); delayMicroseconds(63); - digitalWrite(ahi, HIGH); + digitalWriteFast(ahi, HIGH); analogWrite(bli, power); } //Reverse! if (power < 0) { - digitalWrite(ahi, LOW); - digitalWrite(bli, LOW); + digitalWriteFast(ahi, 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 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.."); +void init_display() { 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)); - Serial.print("V "); - 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)); - SerComm.print("V "); - SerComm.print(get_current(0)); - SerComm.print("A ENC1: "); - SerComm.print(enc1.getCount()); - SerComm.print(" ENC2: "); - SerComm.println(enc2.getCount()); + float vr = get_voltage(0); + float vl = get_voltage(1); + float ar = get_current(0); + float al = get_current(1); + 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"); + + int stickX = (int)(astate->stickX); + if (abs(stickX) < DEADBAND_HALF_WIDTH) + stickX = 0; - } else - fb = appjoystick->getFeedback();*/ - - int zeroed_power = -1 * ((int)(astate->stickX) - 127); - int zeroed_turn = ((int)(astate->stickY) - 127); - - - 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); + int stickY = (int)(astate->stickY); + if (abs(stickY) < DEADBAND_HALF_WIDTH) + stickY = 0; - 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; - else - acceleration = 3; + float acceleration = 100000.0f; - if(left_cooldown > 0) - left_cooldown --; - - 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; + int16_t left_power, right_power; + motor_ramp_update(target_left_power, target_right_power, acceleration, &left_power, &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())); - - //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); + // SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power)); + print_status(); 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); } \ No newline at end of file diff --git a/src/zserio.cpp b/src/zserio.cpp index 216c971..298fb24 100644 --- a/src/zserio.cpp +++ b/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;