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

Add sabertooth lib, start enabling all motor drive

Browse Source
This commit is contained in:
Cole Deck
2023-09-13 11:30:59 -05:00
parent 8f59f0fb15
commit 109b2c92c4
19 changed files with 1158 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

346
src/main.cpp
View File

@ -1,22 +1,25 @@
#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 "dumbdisplay.h"
//#include "wifidumbdisplay.h"
#include "zserio.h"
#include "SerialUART.h"
#include "Sabertooth.h"
const char* ssid = "TEST";
const char* password = "pink4bubble";
DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
LcdDDLayer *optionsdisplay = NULL;
SevenSegmentRowDDLayer *sevenSeg;
JoystickDDLayer *appjoystick;
//DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
//LcdDDLayer *optionsdisplay = NULL;
//SevenSegmentRowDDLayer *sevenSeg;
//JoystickDDLayer *appjoystick;
packet_t pA, pB, safe;
packet_t *astate, *incoming;
@ -27,9 +30,57 @@ byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0
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
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
Sabertooth swivel[2] = { Sabertooth(128), Sabertooth(129) };
Sabertooth actuators(130);
#define MIN_MICROS 800
#define MAX_MICROS 2450
#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;
@ -46,7 +97,21 @@ int acceleration = 1;
bool turbo = false;
int left_cooldown = 0;
int right_cooldown = 0;
int olddisplay = 99999;
int olddisplay = 99999; // guarantee a change when first value comes in
#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
unsigned int getButton(unsigned int num) {
if (num <= 7) {
@ -63,7 +128,7 @@ unsigned int getDPad() {
}
void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
/*void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
if (pLayer == optionsdisplay) {
// clicked the "clear" button
if(turbo) {
@ -76,7 +141,7 @@ void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
}
//delay(100);
}
}
}*/
void osmc_init() {
digitalWrite(ALI1, LOW);
@ -238,29 +303,33 @@ void set_mosfet(bool pin, bool value) {
void set_digit(byte digit, byte value)
{
Wire.beginTransmission(0x38);
/*Wire.beginTransmission(0x38);
Wire.write(0x20 + digit);
Wire.write(d[value]);
Wire.endTransmission();
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.beginTransmission(0x38);
Wire.write(0x20 + digit);
Wire.write(value);
Wire.endTransmission();
Wire.endTransmission();*/
return;
}
void set_blank() {
Wire.beginTransmission(0x38);
/*Wire.beginTransmission(0x38);
Wire.write(0x20);
Wire.write((byte)0);
Wire.write((byte)0);
Wire.endTransmission();
Wire.endTransmission();*/
return;
}
void set_hex(byte num) {
byte digit1 = num;
@ -274,9 +343,8 @@ void set_hex(byte num) {
}
set_digit(0, digit1);
set_digit(1, digit2);
if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
if(num != olddisplay && millis() % 10 < 1) {
olddisplay = num;
sevenSeg->showHexNumber(num);
}
set_raw(4, 0x00); // clear second dot
}
@ -292,13 +360,54 @@ void set_dec(byte num) {
}
set_digit(0, digit1);
set_digit(1, digit2);
if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
if(num != olddisplay && millis() % 10 < 1) {
olddisplay = num;
sevenSeg->showNumber(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
// 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, MAX_MICROS);
RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 5].servoIndex, newspeed);
}
else if (motor <= 10) {
// actuator controls
actuators.motor((motor - 9) % 2, speed);
}
// set servos
// speed needs to be a high number to be reasonable
else if (motor == 11) {
stepperX.setSpeed((float)speed);
stepperX.runSpeedState();
}
else if (motor == 12) {
stepperY.setSpeed((float)speed);
stepperY.runSpeedState();
}
else if (motor == 13) {
stepperY.setSpeed((float)speed);
stepperY.runSpeedState();
}
}
void setup() {
WiFi.noLowPowerMode();
@ -355,17 +464,113 @@ void setup() {
Serial.println(" done");
delay(20);
Serial.print("Initializing OSMCs..");
set_hex(0x2);
osmc_init();
//Serial.print("Initializing OSMCs..");
//set_hex(0x2);
//osmc_init();
//Serial.println(" done");
//delay(20);
////delay(2000);
Serial.print("Initializing motor controllers..");
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
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
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);
// Sabertooth init
Serial2.setTX(4); // pin 5 of OSMC port 2
Serial2.begin(9600);
Sabertooth::autobaud(Serial2);
// Talon SRX init
for (int index = 0; index < NUM_SERVOS; index++)
{
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);
}
}
Serial.println(" done");
delay(20);
//delay(2000);
bool ioex1p, ioex2p = false;
Serial.print("Initializing I/O expanders..");
set_hex(0x3);
if(ioex1.begin()) {
delay(200);
Serial.print(" 1");
@ -374,6 +579,7 @@ void setup() {
delay(20);
}
set_hex(0x4);
if(ioex2.begin()) {
delay(20);
Serial.print(" 2");
@ -381,14 +587,13 @@ void setup() {
} else {
delay(20);
}
Serial.println(" done");
delay(20);
Serial.print("Initializing encoders..");
set_hex(0x5);
enc1.begin();
enc2.begin();
//enc1.begin();
//enc2.begin();
Serial.println(" done");
delay(20);
@ -450,28 +655,28 @@ void setup() {
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);
}
//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..");
@ -504,7 +709,7 @@ void setup() {
Serial.print("Self tests complete: ");
Serial.print(pass);
Serial.println("/6 tests passed.");
Serial.println("/4 tests passed.");
Serial.println("RIB is ready to go. Starting program.");
set_blank();
/*dumbdisplay.recordLayerSetupCommands();
@ -644,8 +849,8 @@ void loop() {
//SerComm.println();
set_hex(avg_speed);
drive_right(right_enabled, right_power);
drive_left(left_enabled, -left_power);
//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)
@ -730,9 +935,42 @@ void loop1() {
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));
//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;
}*/
for (int x = 1; x < 14; x++) {
set_motor(x, loopcount * 5);
}
delay(25);
}