diff --git a/lib/Sabertooth/src/Sabertooth.h b/lib/Sabertooth/src/Sabertooth.h index 79d3915..e981aff 100644 --- a/lib/Sabertooth/src/Sabertooth.h +++ b/lib/Sabertooth/src/Sabertooth.h @@ -30,7 +30,7 @@ typedef Print SabertoothStream; #if defined(USBCON) #define SabertoothTXPinSerial Serial1 // Arduino Leonardo has TX->1 on Serial1, not Serial. #else -#define SabertoothTXPinSerial Serial +#define SabertoothTXPinSerial Serial2 #endif #define SyRenTXPinSerial SabertoothTXPinSerial diff --git a/platformio.ini b/platformio.ini index a1861e9..49a873f 100644 --- a/platformio.ini +++ b/platformio.ini @@ -13,7 +13,7 @@ platform = https://github.com/maxgerhardt/platform-raspberrypi.git board = rpipicow framework = arduino platform_packages = - framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master + framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master ; use master branch for setXXX fix upload_port = /run/media/amelia/RPI-RP2/ board_build.core = earlephilhower lib_deps = @@ -25,4 +25,4 @@ lib_deps = olikraus/Ucglib@^1.5.2 khoih-prog/RP2040_ISR_Servo@^1.1.2 debug_tool = cmsis-dap -build_flags = -O3 +build_flags = -O3 ; optimize build level 3 diff --git a/src/main.cpp b/src/main.cpp index c6ae3b8..f158c63 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,7 +12,7 @@ //#include "wifidumbdisplay.h" #include "zserio.h" #include "SerialUART.h" -#include "Sabertooth.h" +#include const char* ssid = "TEST"; const char* password = "pink4bubble"; @@ -49,18 +49,19 @@ I2Cwrapper wrapper(i2cAddress); AccelStepperI2C stepperX(&wrapper); // Stepper Motor 1 AccelStepperI2C stepperY(&wrapper); // Stepper Motor 2 AccelStepperI2C stepperZ(&wrapper); // Stepper Motor 3 -bool stepperXdir = true; // true=CW, false=CCW -bool stepperYdir = true; -bool stepperZdir = true; -#define defMaxSpeed 1000000 -#define defAcceleration 1000000 -#define stepsToGo 1000 // 200 steps/rev w/o microstepping +int stepperX_pos = 0; +int stepperY_pos = 0; +int stepperZ_pos = 0; -Sabertooth swivel[2] = { Sabertooth(128), Sabertooth(129) }; -Sabertooth actuators(130); +#define defMaxSpeed 8000 +#define defAcceleration 8000 -#define MIN_MICROS 800 -#define MAX_MICROS 2450 +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 @@ -112,6 +113,7 @@ int olddisplay = 99999; // guarantee a change when first value comes in #define LIFT1 11 #define LIFT2 12 #define LIFT3 13 +#define LIFTALL 14 unsigned int getButton(unsigned int num) { if (num <= 7) { @@ -170,7 +172,7 @@ void osmc_init() { digitalWrite(22, LOW); pinMode(22, OUTPUT); - analogWriteFreq(4000); // set PWM frequency to 16kHz + analogWriteFreq(4000); // set PWM frequency to 4kHz } char try_enable_osmc(char enabled, char enablepin, float vbatt, @@ -371,6 +373,7 @@ void set_motor(byte motor, int speed) { // 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); @@ -386,26 +389,67 @@ void set_motor(byte motor, int speed) { // 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, MAX_MICROS); + 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) % 2, speed); + 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); - stepperX.runSpeedState(); + //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); - stepperY.runSpeedState(); + //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); - stepperY.runSpeedState(); + //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 + } } } @@ -419,7 +463,7 @@ void setup() { digitalWrite(LED_BUILTIN, HIGH); Serial.begin(115200); //Serial.println("hello!"); - delay(2000); + //delay(2000); Serial.println("Initializing RIB subsystems.."); @@ -438,29 +482,50 @@ void setup() { 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"); + + 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(0x1); + set_hex(0x3); 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); @@ -470,7 +535,22 @@ void setup() { //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); @@ -493,18 +573,19 @@ void setup() { 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 - // attach() replaces the AccelStepper constructor, so it could also be called like this: - // stepper.attach(AccelStepper::DRIVER, 5, 6); - // stepper.attach(); // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5 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 @@ -533,16 +614,23 @@ void setup() { 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); // pin 5 of OSMC port 2 + 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); } @@ -562,33 +650,14 @@ void setup() { 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); - 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); @@ -680,9 +749,9 @@ void setup() { set_hex(0xB); Serial.print("Checking I/O expander 1.."); - for (int i = 0; i < 8; i++) { - ioex1.pinMode(i,OUTPUT, LOW); - } + // 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); @@ -694,9 +763,9 @@ void setup() { } set_hex(0xC); Serial.print("Checking I/O expander 2.."); - for (int i = 0; i < 8; i++) { - ioex2.pinMode(i,OUTPUT, LOW); - } + // 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); @@ -969,8 +1038,39 @@ void loop1() { stepperZ.runState(); stepperZdir = !stepperZdir; }*/ - for (int x = 1; x < 14; x++) { - set_motor(x, loopcount * 5); + 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); } \ No newline at end of file diff --git a/src/swerve.cpp b/src/swerve.cpp new file mode 100644 index 0000000..8ed6a6c --- /dev/null +++ b/src/swerve.cpp @@ -0,0 +1,17 @@ + +template int signum(T val) { + return (T(0) < val) - (val < T(0)); +} + +double closestAngle(double current, double target) +{ + // get direction + double dir = target % 360.0 - current % 360.0; + + // convert from -360 to 360 to -180 to 180 + if (abs(dir) > 180.0) + { + dir = -(signum(dir) * 360.0) + dir; + } + return dir; +}