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

1085 lines
28 KiB
C++
Raw Blame History

#include <Arduino.h>
#include <WiFi.h>
#include <Wire.h>
#include <AccelStepperI2C.h>
#include "globals.h"
#include <SPI.h>
#include "MCP3XXX.h"
#include "PCF8574.h"
#include "RP2040_ISR_Servo.h"
#include "pio_encoder.h"
//#include "dumbdisplay.h"
//#include "wifidumbdisplay.h"
#include "zserio.h"
#include "SerialUART.h"
#include <Sabertooth.h>
#include "swerve.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
// stepper board slave config
#define MOTOR_X_ENABLE_PIN 8
#define MOTOR_X_STEP_PIN 2
#define MOTOR_X_DIR_PIN 5
#define MOTOR_Y_ENABLE_PIN 8
#define MOTOR_Y_STEP_PIN 3
#define MOTOR_Y_DIR_PIN 6
#define MOTOR_Z_ENABLE_PIN 8
#define MOTOR_Z_STEP_PIN 4
#define MOTOR_Z_DIR_PIN 7
uint8_t i2cAddress = 0x08; // arduino address
I2Cwrapper wrapper(i2cAddress);
AccelStepperI2C stepperX(&wrapper); // Stepper Motor 1
AccelStepperI2C stepperY(&wrapper); // Stepper Motor 2
AccelStepperI2C stepperZ(&wrapper); // Stepper Motor 3
int stepperX_pos = 0;
int stepperY_pos = 0;
int stepperZ_pos = 0;
#define defMaxSpeed 8000
#define defAcceleration 8000
Sabertooth swivel[2] = { Sabertooth(128, Serial2), Sabertooth(129, Serial2) };
Sabertooth actuators(130, Serial2);
#define MIN_MICROS 880
#define MAX_MICROS 1985
#define OC_OFFSET 0 // microseconds
#define SERVO_PIN_1 5
#define SERVO_PIN_2 6
#define SERVO_PIN_3 7
#define SERVO_PIN_4 9
typedef struct
{
int servoIndex;
uint8_t servoPin;
} ISR_servo_t;
#define NUM_SERVOS 4
ISR_servo_t ISR_servo[NUM_SERVOS] =
{
{ -1, SERVO_PIN_1 }, { -1, SERVO_PIN_2 }, { -1, SERVO_PIN_3 }, { -1, SERVO_PIN_4 }
};
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; // guarantee a change when first value comes in
// motor indeces for set_motor() function
#define FLSTEER 1
#define FRSTEER 2
#define BLSTEER 3
#define BRSTEER 4
#define FLDRIVE 5
#define FRDRIVE 6
#define BLDRIVE 7
#define BRDRIVE 8
#define EXTEND1 9
#define EXTEND2 10
#define LIFT1 11
#define LIFT2 12
#define LIFT3 13
#define LIFTALL 14
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 4kHz
}
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);
return;
}
void set_raw(byte digit, byte value) {
/*Wire.beginTransmission(0x38);
Wire.write(0x20 + digit);
Wire.write(value);
Wire.endTransmission();*/
return;
}
void set_blank() {
/*Wire.beginTransmission(0x38);
Wire.write(0x20);
Wire.write((byte)0);
Wire.write((byte)0);
Wire.endTransmission();*/
return;
}
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 && millis() % 10 < 1) {
olddisplay = 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 && millis() % 10 < 1) {
olddisplay = num;
}
set_raw(4, 0x02); // set second dot
}
void set_motor(byte motor, int speed) {
// 1 - 4 : swivel motors on Sabertooth 1-2
// 5 - 8 : drive motors on Talon SRX
// 9 - 10 : actuators on Sabertooth 3
// 11 - 13 : Steppers on slave board
// 14 : Drive 11-13 with identical position & speed
// speed is -127 to 127
Serial.print("Driving motor ");
Serial.print(motor);
Serial.print(" with speed ");
Serial.println(speed);
if (motor <= 4) {
// swerve controls
swivel[(motor - 1) / 2].motor((motor - 1) % 2, speed);
}
else if (motor <= 8) {
// Talon SRX drive
// Note: can't use the built in arduino-pico Servo.h because it uses the PIO - which is already full
// Can't use PWM because that would slow the PWM frequency to 50hz, kinda need more than that
// So we use hardware interrupt timers instead
//RP2040_ISR_Servos.setPosition(ISR_servo[motor - 5].servoIndex, position);
uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET);
RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 5].servoIndex, newspeed);
}
else if (motor <= 10) {
// actuator controls
actuators.motor((motor - 9), speed);
}
// set servos
// speed needs to be a high number to be reasonable
else if (motor == 11) {
//stepperX.setSpeed((float)speed);
if (abs(speed) > 0)
ioex1.digitalWrite(2, LOW); // enable
stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperX_pos = speed * 96 + stepperX.currentPosition();
stepperX.moveTo(stepperX_pos);
stepperX.runState();
//stepperX.runSpeedState();
}
else if (motor == 12) {
//stepperY.setSpeed((float)speed);
if (abs(speed) > 0)
ioex1.digitalWrite(2, LOW); // enable
stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperY_pos = speed * 96 + stepperY.currentPosition();
stepperY.moveTo(stepperY_pos);
stepperY.runState();
//stepperY.runSpeedState();
}
else if (motor == 13) {
//stepperY.setSpeed((float)speed);
if (abs(speed) > 0)
ioex1.digitalWrite(2, LOW); // enable
stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperZ_pos = speed * 96 + stepperZ.currentPosition();
stepperZ.moveTo(stepperZ_pos);
stepperZ.runState();
//stepperY.runSpeedState();
}
else if (motor == 14) { // all steppers together
if (abs(speed) > 0) {
ioex1.digitalWrite(2, LOW); // enable
stepperX_pos = speed * 96 + stepperX.currentPosition();
stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperX.moveTo(stepperX_pos);
stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperY.moveTo(stepperX_pos);
stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
stepperZ.moveTo(stepperX_pos);
stepperX.runState();
stepperY.runState();
stepperZ.runState();
} else {
ioex1.digitalWrite(2, HIGH); // disable
}
}
}
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();
Wire.beginTransmission(0x38);
Wire.write(0x07);
Wire.write(0x00); // disable display test mode
Wire.endTransmission();
Serial.println(" done");
bool ioex1p, ioex2p = false;
Serial.print("Initializing I/O expanders..");
set_hex(0x1);
if(ioex1.begin()) {
for (int i = 0; i < 8; i++) {
ioex1.pinMode(i,OUTPUT);
ioex1.digitalWrite(i, LOW);
}
delay(20);
Serial.print(" 1");
ioex1p = true;
} else {
delay(20);
}
set_hex(0x2);
if(ioex2.begin()) {
for (int i = 0; i < 8; i++) {
ioex2.pinMode(i,OUTPUT);
ioex2.digitalWrite(i, LOW);
}
delay(20);
Serial.print(" 2");
ioex2p = true;
} else {
delay(20);
}
Serial.print("Initializing ADC..");
set_hex(0x3);
SPI1.setRX(12);
SPI1.setCS(13);
SPI1.setTX(11);
SPI1.setSCK(10);
adc.begin(13);
Serial.println(" done");
delay(20);
//Serial.print("Initializing OSMCs..");
//set_hex(0x2);
//osmc_init();
//Serial.println(" done");
//delay(20);
////delay(2000);
Serial.println(" done");
delay(20);
Serial.print("Initializing motor controllers..");
set_hex(0x4);
ioex1.pinMode(3, OUTPUT); // reset arduino
ioex1.digitalWrite(3, LOW);
delay(20);
ioex1.digitalWrite(3, HIGH);
delay(2000);
digitalWrite(ALI1, LOW);
digitalWrite(BLI1, LOW);
digitalWrite(AHI1, LOW);
digitalWrite(BHI1, LOW);
digitalWrite(ALI2, LOW);
digitalWrite(BLI2, LOW);
digitalWrite(AHI2, LOW);
digitalWrite(BHI2, LOW);
pinMode(ALI1, OUTPUT);
pinMode(AHI1, OUTPUT);
pinMode(BLI1, OUTPUT);
pinMode(BHI1, OUTPUT);
pinMode(ALI2, OUTPUT);
pinMode(AHI2, OUTPUT);
pinMode(BLI2, OUTPUT);
pinMode(BHI2, OUTPUT);
digitalWrite(22, LOW); // finally, enable output buffers
pinMode(22, OUTPUT);
// enable stepper slave board
if (!wrapper.ping()) {
Serial.println("Arduino stepper driver not found!");
while (true) {}
}
wrapper.reset(); // reset the target device
// note: this doesn't fucking work
// we reset by flipping the reset pin above
wrapper.setI2Cdelay(20); // Set small delay for I2C bus communication
stepperX.attach(AccelStepper::DRIVER, MOTOR_X_STEP_PIN, MOTOR_X_DIR_PIN); // Stepper Motor 1 - X
stepperY.attach(AccelStepper::DRIVER, MOTOR_Y_STEP_PIN, MOTOR_Y_DIR_PIN); // Stepper Motor 2 - Y
stepperZ.attach(AccelStepper::DRIVER, MOTOR_Z_STEP_PIN, MOTOR_Z_DIR_PIN); // Stepper Motor 3 - Z
Serial.print("stepperX.myNum=");
Serial.println(stepperX.myNum);
Serial.print("stepperY.myNum=");
Serial.println(stepperY.myNum);
Serial.print("stepperZ.myNum=");
Serial.println(stepperZ.myNum);
if (stepperX.myNum < 0) { // Should not happen after a reset
Serial.println("Error: stepperX could not be allocated");
while (true) {}
}
if (stepperY.myNum < 0) { // Should not happen after a reset
Serial.println("Error: stepperY could not be allocated");
while (true) {}
}
if (stepperZ.myNum < 0) { // Should not happen after a reset
Serial.println("Error: stepperZ could not be allocated");
while (true) {}
}
stepperX.setMaxSpeed(defMaxSpeed);
stepperX.setAcceleration(defAcceleration);
stepperY.setMaxSpeed(defMaxSpeed);
stepperY.setAcceleration(defAcceleration);
stepperZ.setMaxSpeed(defMaxSpeed);
stepperZ.setAcceleration(defAcceleration);
ioex1.digitalWrite(2, HIGH); // setup output pin
ioex1.pinMode(2, OUTPUT);
ioex1.digitalWrite(2, HIGH);
// Sabertooth init
Serial2.setTX(4);
Serial2.setRX(5);
Serial2.begin(9600);
Sabertooth::autobaud(Serial2);
//Sabertooth::autobaud();
// Talon SRX init
for (int index = 0; index < NUM_SERVOS; index++)
{
digitalWrite(ISR_servo[index].servoPin, LOW);
pinMode(ISR_servo[index].servoPin, OUTPUT);
digitalWrite(ISR_servo[index].servoPin, LOW);
}
for (int index = 0; index < NUM_SERVOS; index++)
{
ISR_servo[index].servoIndex = RP2040_ISR_Servos.setupServo(ISR_servo[index].servoPin, MIN_MICROS, MAX_MICROS);
if (ISR_servo[index].servoIndex != -1)
{
//Serial.print(F("Setup OK Servo index = ")); Serial.println(ISR_servo[index].servoIndex);
RP2040_ISR_Servos.setPosition(ISR_servo[index].servoIndex, 0);
}
else
{
Serial.print(F("Setup Failed Servo index = ")); Serial.println(ISR_servo[index].servoIndex);
}
}
for (int i = 5; i < 9; i++)
set_motor(i, 0);
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)
{
Serial.print(" ");
Serial.print(adc.analogRead(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("/4 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_lx = -1 * ((int)(astate->stickX) - 127);
int zeroed_ly = ((int)(astate->stickY) - 127);
int zeroed_rx = -1 * ((int)(astate->stickRX) - 127);
int zeroed_ry = ((int)(astate->stickRY) - 127);
// Goliath / 2 side arcade tank drive code below
/*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();
//drive_left(left_enabled, 255);
//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);
/*if (stepperX.distanceToGo() == 0) { // Give stepper something to do...
if (stepperXdir) {
Serial.println("Driving stepper");
stepperX.moveTo(stepsToGo);
} else {
Serial.println("Driving stepper");
stepperX.moveTo(-stepsToGo);
}
stepperX.runState();
stepperXdir = !stepperXdir;
}
if (stepperY.distanceToGo() == 0) { // Give stepper something to do...
if (stepperYdir)
stepperY.moveTo(stepsToGo);
else
stepperY.moveTo(-stepsToGo);
stepperY.runState();
stepperYdir = !stepperYdir;
}
if (stepperZ.distanceToGo() == 0) { // Give stepper something to do...
if (stepperZdir)
stepperZ.moveTo(stepsToGo);
else
stepperZ.moveTo(-stepsToGo);
stepperZ.runState();
stepperZdir = !stepperZdir;
}*/
if (mode == 1) {
//set_hex(count);
if (count < 125) {
count += 1;
} else {
//count = 0;
mode = 2;
}
}
if (mode == 2) {
//set_hex(count);
if (count > -125) {
count -= 1;
} else {
//count = 0;
mode = 1;
}
}
for (int x = 5; x < 9; x++) {
set_motor(x, count);
}
swivel[0].motor(0, 127);
swivel[0].motor(1, 127);
swivel[1].motor(0, 127);
swivel[1].motor(1, 127);
set_motor(14, count); // drive all steppers in sync
digitalWrite(0, HIGH);
digitalWrite(1, HIGH);
digitalWrite(2, HIGH);
digitalWrite(3, HIGH);
delay(25);
}
Reference in New Issue View Git Blame Copy Permalink