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

1079 lines
32 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 "pio_encoder.h"
//#include "dumbdisplay.h"
//#include "wifidumbdisplay.h"
#include "zserio.h"
#include "SerialUART.h"
#include <Sabertooth.h>
#include <107-Arduino-Servo-RP2040.h>
#include "swerve.h"
#include "manipulator.h"
#include "spinlock.h"
const char* ssid = "TEST";
const char* password = "pink4bubble";
//DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
//LcdDDLayer *optionsdisplay = NULL;
//SevenSegmentRowDDLayer *sevenSeg;
//JoystickDDLayer *appjoystick;
// Swerve Drive control
swerve_drive swrv;
// bool drive_type = DRIVE_TRANSLATION;
int drive_mode = DRIVE_TRANSLATION;
// Manipulator control (arm and claw)
manipulator_arm clawarm;
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);
// JANK: soldered to pico or headers
PioEncoder enc1(18); // Front Left
PioEncoder enc2(0); // Front Right // TODO WIRING AND CONFIRMATION 20230929
PioEncoder enc3(27); // Back Left // TODO WIRING AND CONFIRMATION 20230929
PioEncoder enc4(14); // Back Right
// stepper board slave pins
#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;
// Loop management
uint64_t previous_loop_start_time_core_0 = 0, previous_loop_start_time_core_1 = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop
uint64_t previous_loop_processing_duration_core_0 = 0, previous_loop_processing_duration_core_1 = 0; // Processing time for the previous loop on each core
uint64_t loop_counter_core_0 = 0, loop_counter_core_1 = 0; // Counter for loop() and loop1() respectively, incremented at the end of each loop
// Motor power communication between cores
byte drive_power_command_spinlock_flag = SPINLOCK_UNSET; // Spinlock tracking flag
// Variables used to transfer steering motor power between cores, covered by spinlock
int power_data_transfer_fl = 0;
int power_data_transfer_fr = 0;
int power_data_transfer_bl = 0;
int power_data_transfer_br = 0;
// Variables used to track previous requested motor power, only used by core 1, sabertooth not called if motor power unchanged since previous loop
int power_data_transfer_prev_fl = 0;
int power_data_transfer_prev_fr = 0;
int power_data_transfer_prev_bl = 0;
int power_data_transfer_prev_br = 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 TALON_PIN_1 5
#define TALON_PIN_2 6
#define TALON_PIN_3 7
#define TALON_PIN_4 9
// pins for arm servos
#define ARM_SERVO_PIN_1 2
#define ARM_SERVO_PIN_2 3
#define ARM_SERVO_PIN_3 8
static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3;
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 FRSTEER 1
#define FLSTEER 2
#define BRSTEER 3
#define BLSTEER 4
#define FLDRIVE 5
#define BRDRIVE 6
#define BLDRIVE 7
#define FRDRIVE 8
#define EXTEND1 9
#define EXTEND2 10
#define LIFT1 11
#define LIFT2 12
#define LIFT3 13
#define LIFTALL 14
#define ARMSERVO1 15
#define ARMSERVO2 16
#define ARMSERVO3 17
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 (Clockwise: FR, FL, BR, BL)
// 5 - 8 : drive motors on Talon SRX (Forward: FL, BR, BL, FL)
// 9 - 10 : actuators on Sabertooth 3
// 11 - 13 : Steppers on slave board
// 14 : drive 11-13 with identical position & speed
// 15 - 17 : arm servos
// speed is -127 to 127
Serial.print("Driving motor ");
Serial.print(motor);
Serial.print(" with speed ");
Serial.println(speed);
if (motor <= 4) {
// swerve controls
speed *= (((motor % 2) * 2) - 1); // Flip motors 2 and 4
swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed);
}
else if (motor == 5)
talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
else if (motor == 6)
talon2.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
else if (motor == 7)
talon3.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
else if (motor == 8)
talon4.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
else if (motor <= 10) {
// actuator controls
actuators.motor((motor - 9 + 1), 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
}
}
else if (motor == 15)
arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
else if (motor == 16)
arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
else if (motor == 17)
arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
}
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(2400); // Sabertooths on modulus have been reprogrammed to 2400 baud! ST1.setBaudrate(2400); from another pico
Sabertooth::autobaud(Serial2);
swivel[0].setTimeout(100);
swivel[1].setTimeout(100);
actuators.setTimeout(100);
// Servo & Talon SRX init
// talon0, talon1, talon2, talon3, arm1, arm2, arm3
talon1.attach(TALON_PIN_1);
talon2.attach(TALON_PIN_2);
talon3.attach(TALON_PIN_3);
talon4.attach(TALON_PIN_4);
for (int i = 5; i < 9; i++)
set_motor(i, 0);
arm1.attach(ARM_SERVO_PIN_1);
arm2.attach(ARM_SERVO_PIN_2);
arm3.attach(ARM_SERVO_PIN_3);
for (int i = 15; i < 18; i++)
set_motor(i, 0);
Serial.println(" done");
delay(20);
// Initialize encoders
Serial.print("Initializing encoders..");
set_hex(0x5);
enc1.begin();
//enc1.flip();
enc2.begin();
enc3.begin();
enc4.begin();
Serial.println(" done");
delay(20);
// Radio initialization
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.stickRX = 127;
safe.stickRY = 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
// Initialize swerve_drive struct, load with initial encoder values
int front_left_encoder = enc1.getCount(), front_right_encoder = enc2.getCount(), back_left_encoder = enc3.getCount(), back_right_encoder = enc4.getCount();
swrv = initializeSwerveDrive(front_left_encoder, front_right_encoder, back_left_encoder, back_right_encoder);
// Initialize manipulator
clawarm = initializeManipulatorArm(clawarm);
clawarm = setArmSpeedLimit(clawarm, 1.0f);
clawarm = setArmSpeedCoefficient(clawarm, 1.0f);
// Setup
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 telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_float, float zeroed_ry_float, int drive_mode, \
float left_joystick_angle, float left_joystick_magnitude, float right_joystick_angle, float right_joystick_magnitude, \
uint64_t processing_time) { // Print encoder positions for steering motors
char buffer[300] = "\0";
// Create a new line to show the start of this telemetry cycle
//Serial.println(buffer);
//SerComm.println(buffer);
// Encoder data and loop number
sprintf(buffer, "Loop = %llu E1 = %i E2 = %i E3 = %i E4 = %i z_lx = %f z_ly = %f z_rx = %f z_ry = %f mode = %i mag-L = %f ang-L = %f mag-R = %f ang-R = %f proctime = %llu", \
loop_counter_core_0, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount(), \
zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, drive_mode, \
left_joystick_magnitude, left_joystick_angle, right_joystick_magnitude, right_joystick_angle, \
processing_time);
Serial.println(buffer);
SerComm.println(buffer);
}
void loop() {
previous_loop_start_time_core_0 = millis(); // Track the start time of this loop to determine how long to sleep before the next loop
rp2040.wdt_reset();
comm_parse();
//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 = ((int)(astate->stickX) - 127);
int zeroed_ly = -1 * ((int)(astate->stickY) - 127);
int zeroed_rx = ((int)(astate->stickRX) - 127);
int zeroed_ry = -1 * ((int)(astate->stickRY) - 127);
// Modulus swerve drive
// Modify controller joystick inputs to be within a range of -1.0 to 1.0
float zeroed_lx_float = ((float) zeroed_lx) / CONTROLLER_JOYSTICK_MAX;
float zeroed_ly_float = ((float) zeroed_ly) / CONTROLLER_JOYSTICK_MAX;
float zeroed_rx_float = ((float) zeroed_rx) / CONTROLLER_JOYSTICK_MAX;
float zeroed_ry_float = ((float) zeroed_ry) / CONTROLLER_JOYSTICK_MAX;
// Joystick calculations
float left_joystick_magnitude = sqrt(pow(zeroed_lx_float,2) + pow(zeroed_ly_float,2));
float left_joystick_angle = fmod(360.0 - fmod((DEGREES_PER_RADIAN * atan2(zeroed_ly_float, zeroed_lx_float)) + 270.0, 360.0) , 360.0);
float right_joystick_magnitude = sqrt(pow(zeroed_rx_float,2) + pow(zeroed_ry_float,2));
float right_joystick_angle = fmod(360.0 - fmod((DEGREES_PER_RADIAN * atan2(zeroed_ry_float, zeroed_rx_float)) + 270.0, 360.0) , 360.0);
// add 270, fmod 360, 360 - n, fmod 360
// Drive mode calculation
int loop_drive_mode = DRIVE_STOP; // Temporary drive mode specific to this loop, is DRIVE_STOP (0) until it is changed
if (getButton(SHOULDER_TOP_RIGHT))
drive_mode = DRIVE_TRANSLATION;
else if(getButton(SHOULDER_TOP_LEFT))
drive_mode = DRIVE_BASIC;
if (fabs(left_joystick_magnitude) > 0.1 ) { // if left stick is touched
loop_drive_mode = drive_mode;
}
if (fabs(right_joystick_magnitude) > 0.1 ) { // if right stick is touched - override translation
loop_drive_mode = DRIVE_ROTATION;
}
// Select operation based on current drive mode
switch(loop_drive_mode) {
case DRIVE_STOP:
swrv = stopSwerve(swrv);
swrv.enable_steering = false;
break;
case DRIVE_BASIC:
swrv = basicDrive(swrv, left_joystick_magnitude, left_joystick_angle);
swrv.enable_steering = true;
break;
case DRIVE_TRANSLATION:
swrv = translationDrive(swrv, left_joystick_magnitude, left_joystick_angle);
swrv.enable_steering = true;
break;
case DRIVE_ROTATION:
swrv = rotationDrive(swrv, zeroed_rx_float);
swrv.enable_steering = true;
break;
case DRIVE_TANK:
swrv = tankDrive(swrv, left_joystick_angle, left_joystick_magnitude);
swrv.enable_steering = false;
break;
}
swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct
swrv = updateSwerveCommand(swrv); // Calculate power for each drive and steering motor
// Arm motor control (stepper motors), DPAD_UP to move arm up, DPAD_DOWN to move arm down, both or neither being pressed stops the arm
float arm_speed = (float) (((int) getButton(DPAD_UP)) - ((int) getButton(DPAD_DOWN))); // TODO 20230929 confirm speed and polarity
clawarm = setArmSpeed(clawarm, arm_speed);
// Claw servo control
int new_claw_command = CLAW_COMMAND_UNSET;
int claw_direction = getButton(CLAW_OPEN_BUTTON) - getButton(CLAW_CLOSE_BUTTON);
switch(claw_direction) {
case 0:
new_claw_command = CLAW_COMMAND_STAY;
break;
case 1:
new_claw_command = CLAW_COMMAND_OPEN;
break;
case -1:
new_claw_command = CLAW_COMMAND_CLOSE;
break;
}
clawarm = updateClawCommand(clawarm, new_claw_command);
// Tilt servo control
int new_tilt_command = TILT_COMMAND_UNSET;
clawarm = updateTiltCommand(clawarm, new_tilt_command);
telemetry(zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, loop_drive_mode, left_joystick_angle, left_joystick_magnitude, right_joystick_angle, right_joystick_magnitude, previous_loop_processing_duration_core_0); // DEBUG ONLY, telemetry
// update motors after calculation
set_motor(FLDRIVE, swrv.front_left_power);
set_motor(BRDRIVE, swrv.back_right_power);
set_motor(FRDRIVE, swrv.front_right_power);
set_motor(BLDRIVE, swrv.back_left_power);
// Lock the spinlock and transfer the steering motor data to core 1, which will send the data to the sabertooth motor controllers
spinlock_lock_core_0(&drive_power_command_spinlock_flag);
power_data_transfer_fl = swrv.front_left_power;
power_data_transfer_fr = swrv.front_right_spin_power;
power_data_transfer_bl = swrv.back_left_spin_power;
power_data_transfer_br = swrv.back_right_spin_power;
spinlock_release(&drive_power_command_spinlock_flag);
// update stepper motors
set_motor(LIFTALL, clawarm.arm_set_motor_int);
// update servos
/*
// TODO: Figure out servo mapping
set_motor(SERVOTILT, clawarm.tilt_set_motor_int);
set_motor(SERVOCLAW, clawarm.claw_set_motor_int);
*/
previous_loop_processing_duration_core_0 = millis() - previous_loop_start_time_core_0;
int64_t delay_time_ms = LOOP_DELAY_MS_CORE_0 - (int64_t) previous_loop_processing_duration_core_0; // Dynamically calculate delay time
if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
delay(delay_time_ms);
}
loop_counter_core_0++;
//DDYield();
}
void drive_control_core_1() { // Control drive motors from core 1 from loop1() function
// Lock the steering motor power data to read it
spinlock_lock_core_1(&drive_power_command_spinlock_flag);
int local_fl = power_data_transfer_fl;
int local_fr = power_data_transfer_fr;
int local_bl = power_data_transfer_bl;
int local_br = power_data_transfer_br;
spinlock_release(&drive_power_command_spinlock_flag); // Release the spinlock
// Set motors if the requested power is different than the previously requested power
if(local_fl != power_data_transfer_prev_fl) {
set_motor(FLSTEER, local_fl);
}
if(local_fr != power_data_transfer_prev_fr) {
set_motor(FRSTEER, local_fr);
}
if(local_bl != power_data_transfer_prev_bl) {
set_motor(BLSTEER, local_bl);
}
if(local_br != power_data_transfer_prev_br) {
set_motor(BRSTEER, local_br);
}
// Set the previously requested power data to the current power data, will be read in the next loop
power_data_transfer_prev_fl = local_fl;
power_data_transfer_prev_fr = local_fr;
power_data_transfer_prev_bl = local_bl;
power_data_transfer_prev_br = local_br;
}
void loop1() {
previous_loop_start_time_core_1 = millis();
rp2040.wdt_reset();
drive_control_core_1();
previous_loop_processing_duration_core_1 = millis() - previous_loop_start_time_core_1;
int64_t delay_time_ms = LOOP_DELAY_MS_CORE_1 - (int64_t) previous_loop_processing_duration_core_1; // Dynamically calculate delay time
if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
delay(delay_time_ms);
}
loop_counter_core_1++;
}
Reference in New Issue View Git Blame Copy Permalink