#include #include #include #include #include "globals.h" #include #include "MCP3XXX.h" #include "PCF8574.h" #include "pio_encoder.h" //#include "dumbdisplay.h" //#include "wifidumbdisplay.h" #include "zserio.h" #include "SerialUART.h" #include #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++; }