adeck/rib-test-code
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
rib-test-code/src/main.cpp
Cole Deck f28f5ac12c initial commit
2023-09-01 17:26:52 -05:00

734 lines
18 KiB
C++
Raw Blame History

#include <Arduino.h>
#include <WiFi.h>
#include <Wire.h>
#include "globals.h"
#include <SPI.h>
#include "MCP3XXX.h"
#include "PCF8574.h"
#include "pio_encoder.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;
packet_t pA, pB, safe;
packet_t *astate, *incoming;
comm_state cs;
// character 0 1 2 3 4 5 6 7 8 9 A b C d E F
byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0x1f, 0x4e, 0x3d, 0x4f, 0x47 };
PCF8574 ioex1(0x20, 20, 21);
PCF8574 ioex2(0x21, 20, 21);
PioEncoder enc1(18); // right
PioEncoder enc2(14); // left
MCP3008 adc;
int count = 0;
int mode = 1;
char left_enabled = 0, right_enabled = 0;
int current_offset[2];
int setup_complete = false;
// 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) {
return (astate->btnlo >> num) & 0x01;
} else if (num > 7 && num <= 15) {
return (astate->btnhi >> (num - 8)) & 0x01;
} else {
return 0;
}
}
unsigned int getDPad() {
// four bits: left down right up
return (astate->btnhi >> 4);
}
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);
digitalWrite(AHI1, LOW);
digitalWrite(BHI1, LOW);
digitalWrite(ALI2, LOW);
digitalWrite(BLI2, LOW);
digitalWrite(AHI2, LOW);
digitalWrite(BHI2, LOW);
digitalWrite(DENABLE1, HIGH);
digitalWrite(DENABLE2, HIGH);
pinMode(ALI1, OUTPUT);
pinMode(AHI1, OUTPUT);
pinMode(BLI1, OUTPUT);
pinMode(BHI1, OUTPUT);
pinMode(ALI2, OUTPUT);
pinMode(AHI2, OUTPUT);
pinMode(BLI2, OUTPUT);
pinMode(BHI2, OUTPUT);
pinMode(DENABLE1, OUTPUT);
pinMode(DENABLE2, OUTPUT);
//pinMode(DREADY1, INPUT);
//pinMode(DREADY2, INPUT);
digitalWrite(22, LOW);
pinMode(22, OUTPUT);
analogWriteFreq(4000); // set PWM frequency to 16kHz
}
char try_enable_osmc(char enabled, char enablepin, float vbatt,
char ali, char bli, char ahi, char bhi) {
// check that power is present at the OSMC
if (vbatt > 13) {
if (!enabled){
delay(10); //"Short" delay required in order to prevent blowout! 10ms is conservative.
//delay(1000);
digitalWrite(enablepin, LOW);
}
return 1;
}
else { // controller has no power; zero inputs in case we power it again
digitalWrite(enablepin, HIGH);
digitalWrite(ali, LOW);
digitalWrite(bli, LOW);
digitalWrite(ahi, LOW);
digitalWrite(bhi, LOW);
return 0;
}
}
float get_voltage(byte battery) {
int read = adc.analogRead(battery);
//Serial.println(read);
if (read == 1023)
return 0.0;
read = 0;
for(int i = 0; i < 5; i++) {
read += adc.analogRead(battery);
delay(1);
}
return (read/5.0) * 5 * 10 / 1024.0;
}
void set_offset() {
int n = 100;
current_offset[0] = current_offset[1] = 0;
for (int i = 0; i < n; i++) {
current_offset[0] += adc.analogRead(2);
current_offset[1] += adc.analogRead(3);
delay(10);
}
current_offset[1] /= n;
current_offset[1] -= 512;
//Serial.println(current_offset[0]);
current_offset[0] /= n;
//Serial.println(current_offset[0]);
current_offset[0] -= 512;
//Serial.println(current_offset[0]);
}
float get_current(byte sensor) {
int read = 0;
for(int i = 0; i < 20; i++) {
read += adc.analogRead(sensor + 2);
delay(1);
//Serial.println(read/i);
}
//Serial.println(read/5.0);
return (read/20.0 - 512.0 - current_offset[sensor]) / 1.28;
}
// OSMC motor controller stuff
// Low side outputs must be PWM capable and NOT 5 or 6 (on Uno)
// Do not change timer0,
// Pins 7 and 8 use timer4 in phase correct mode
// Pins 11 and 12 use timer1 in phase correct mode
// OSMC ALI and BLI are the low side driver inputs and must ALWAYS be low/zero when the ready signal is not provided
// OSMC AHI and BHI are the high side driver inputs.
/*
* ----------- Vdd
* | |
* AHI BHI
* | |
* ---M---
* | |
* ALI BLI
* | |
* --------- GND
*/
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);
return;
}
//Stop!
if (abs(power) < 5) {
digitalWrite(ali, LOW);
digitalWrite(bli, LOW);
delayMicroseconds(63);
if (brake != 0) {
digitalWrite(ahi, HIGH);
digitalWrite(bhi, HIGH);
} else {
digitalWrite(ahi, LOW);
digitalWrite(bhi, LOW);
}
return;
}
//Serial.print("Driving OSMC with power ");
//Serial.println(power);
//Forward!
if (power > 0) {
digitalWrite(bhi, LOW);
digitalWrite(ali, LOW);
delayMicroseconds(63);
digitalWrite(ahi, HIGH);
analogWrite(bli, power);
}
//Reverse!
if (power < 0) {
digitalWrite(ahi, LOW);
digitalWrite(bli, LOW);
delayMicroseconds(63);
digitalWrite(bhi, HIGH);
analogWrite(ali, abs(power));
}
}
void set_mosfet(bool pin, bool value) {
ioex1.digitalWrite(pin, value); // first 2 pins of ioex1 (top) are the mosfets
}
void set_digit(byte digit, byte value)
{
Wire.beginTransmission(0x38);
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) {
Wire.beginTransmission(0x38);
Wire.write(0x20 + digit);
Wire.write(value);
Wire.endTransmission();
}
void set_blank() {
Wire.beginTransmission(0x38);
Wire.write(0x20);
Wire.write((byte)0);
Wire.write((byte)0);
Wire.endTransmission();
}
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
}
void set_dec(byte num) {
byte digit1 = num / 10;
//while (digit1 > 9) {
// digit1 -= 10;
//}
byte digit2 = num;
while (digit2 > 9) {
digit2 -= 10;
}
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..");
Wire.setSDA(20);
Wire.setSCL(21);
Wire.begin();
Wire.beginTransmission(0x38);
Wire.write(0x01); // register: decode mode
Wire.write(0x00); // disable decode mode for all digits
Wire.write(0x3f); // intensity max
Wire.write(0x03); // scan limit 3
Wire.write(0x01); // normal operation
Wire.endTransmission();
Wire.beginTransmission(0x38);
Wire.write(0x07); // display mode register
Wire.write(0x01); // display test mode
Wire.endTransmission();
delay(100);
Wire.beginTransmission(0x38);
Wire.write(0x07);
Wire.write(0x00); // disable display test mode
Wire.endTransmission();
Serial.println(" done");
Serial.print("Initializing ADC..");
set_hex(0x1);
SPI1.setRX(12);
SPI1.setCS(13);
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);
Serial.print("Initializing OSMCs..");
set_hex(0x2);
osmc_init();
Serial.println(" done");
delay(20);
//delay(2000);
bool ioex1p, ioex2p = false;
Serial.print("Initializing I/O expanders..");
set_hex(0x3);
if(ioex1.begin()) {
delay(200);
Serial.print(" 1");
ioex1p = true;
} else {
delay(20);
}
set_hex(0x4);
if(ioex2.begin()) {
delay(20);
Serial.print(" 2");
ioex2p = true;
} else {
delay(20);
}
Serial.println(" done");
delay(20);
Serial.print("Initializing encoders..");
set_hex(0x5);
enc1.begin();
enc2.begin();
Serial.println(" done");
delay(20);
Serial.print("Initializing xBee radio..");
set_hex(0x6);
SerComm.setRX(17);
SerComm.setTX(16);
SerComm.begin(57600);
comm_init(); //Initialize the communication FSM
safe.stickX = 127;
safe.stickY = 127;
safe.btnhi = 0;
safe.btnlo = 0;
safe.cksum = 0b1000000010001011;
Serial.println(" done");
delay(20);
Serial.println("Initialization complete.");
Serial.println("Running self-tests..");
byte pass = 0;
set_hex(0x7);
Serial.print("Checking LED driver..");
Wire.beginTransmission(0x38);
if(Wire.endTransmission() != 0) {
Serial.println(" WARNING: LED driver not detected");
set_hex(0xF7);
delay(500);
} else {
Serial.println(" done");
delay(20);
pass++;
}
// TODO
set_hex(0x8);
Serial.print("Checking ADC..");
byte startpass = pass;
for (size_t i = 0; i < adc.numChannels(); ++i)
{
if (adc.analogRead(i) != 0 && adc.analogRead(i) != 1023) {
pass = startpass+1; // check that at least one reading is successful to confirm MCP3008 is responding
}
}
if (pass == startpass+1) {
Serial.println(" done");
delay(20);
} else {
Serial.println(" WARNING: ADC not detected");
set_hex(0xF8);
delay(500);
}
Serial.print("Calibrating current sensors..");
set_offset();
Serial.println("done");
Serial.print("Checking OSMC 1..");
set_hex(0x9);
if (get_voltage(0) > 13) {
Serial.println(" done");
delay(20);
pass++;
} else {
Serial.println(" WARNING: OSMC 1 battery too low or OSMC not present");
set_hex(0xF9);
delay(500);
}
set_hex(0xA);
Serial.print("Checking OSMC 2..");
if (get_voltage(1) > 13) {
Serial.println(" done");
delay(20);
pass++;
} else {
Serial.println(" WARNING: OSMC 2 battery too low or OSMC not present");
set_hex(0xFA);
delay(500);
}
set_hex(0xB);
Serial.print("Checking I/O expander 1..");
for (int i = 0; i < 8; i++) {
ioex1.pinMode(i,OUTPUT, LOW);
}
if(ioex1p == false) {
Serial.println(" WARNING: I/O expander not detected");
set_hex(0xFB);
delay(500);
} else {
Serial.println(" done");
delay(20);
pass++;
}
set_hex(0xC);
Serial.print("Checking I/O expander 2..");
for (int i = 0; i < 8; i++) {
ioex2.pinMode(i,OUTPUT, LOW);
}
if(!ioex2p == false) {
Serial.println(" WARNING: I/O expander not detected");
set_hex(0xFC);
delay(500);
} else {
Serial.println(" done");
delay(20);
pass++;
}
Serial.print("Self tests complete: ");
Serial.print(pass);
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!
}
void setup1() {
while(!setup_complete)
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());
}
void loop() {
rp2040.wdt_reset();
comm_parse();
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 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;
}
if(turbo)
acceleration = 8;
else
acceleration = 3;
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;
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);
delay(50);
//DDYield();
}
int loopcount = 0;
void loop1() {
rp2040.wdt_reset();
//digitalWrite(LED_BUILTIN, HIGH);
if(loopcount == 20) {
//print_status();
loopcount = 0;
delay(25);
}
else {
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);
}
Reference in New Issue View Git Blame Copy Permalink