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

9/22/23 start

Browse Source
This commit is contained in:
Cole Deck 2023-09-22 17:43:02 -05:00
parent 109b2c92c4
commit b4e999475e
4 changed files with 183 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/Sabertooth/src/Sabertooth.h
View File

@ -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

4
platformio.ini
View File

@ -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

226
src/main.cpp
View File

@ -12,7 +12,7 @@
//#include "wifidumbdisplay.h"
#include "zserio.h"
#include "SerialUART.h"
#include "Sabertooth.h"
#include <Sabertooth.h>
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);
}

17
src/swerve.cpp Normal file
View File

@ -0,0 +1,17 @@
template <typename T> 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;
}