adeck/rib-test-code
2
0 Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

2026 - better accel, printing, and more cleanup

Browse Source
This commit is contained in:
Amelia Deck committed 2026-10-07 12:57:02 -05:00
1 parent 8f59f0fb15
commit c480518b7c
4 files changed
+137 -275
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

platformio.ini
+4 -2
View File
@@ -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
src/globals.h
+1 -1
View File
@@ -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
+130 -271
View File
@@ -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());
float vr = get_voltage(0);
float vl = get_voltage(1);
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 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");
} else
fb = appjoystick->getFeedback();*/
int stickX = (int)(astate->stickX);
if (abs(stickX) < DEADBAND_HALF_WIDTH)
stickX = 0;
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);
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 stickY = (int)(astate->stickY);
if (abs(stickY) < DEADBAND_HALF_WIDTH)
stickY = 0;
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);
}
src/zserio.cpp
+2 -1
View File
@@ -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;