From 349fba12a7b2347b39adde22892e6bc493691242 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Thu, 5 Nov 2020 17:59:51 -0330 Subject: [PATCH 01/12] Add stepper motor hat arduino interface library Should contain all the code needed to interact with the steppers through the stepper motor hat. May need to be wrapped up nicer --- .../actuators/inc/Adafruit_MotorShield.h | 110 ++++ .../actuators/src/Adafruit_MotorShield.cpp | 505 ++++++++++++++++++ .../drivers/inc/Adafruit_MS_PWMServoDriver.h | 59 ++ .../src/Adafruit_MS_PWMServoDriver.cpp | 114 ++++ 4 files changed, 788 insertions(+) create mode 100644 Node/Common/actuators/inc/Adafruit_MotorShield.h create mode 100644 Node/Common/actuators/src/Adafruit_MotorShield.cpp create mode 100644 Node/Common/drivers/inc/Adafruit_MS_PWMServoDriver.h create mode 100644 Node/Common/drivers/src/Adafruit_MS_PWMServoDriver.cpp diff --git a/Node/Common/actuators/inc/Adafruit_MotorShield.h b/Node/Common/actuators/inc/Adafruit_MotorShield.h new file mode 100644 index 0000000..b99e397 --- /dev/null +++ b/Node/Common/actuators/inc/Adafruit_MotorShield.h @@ -0,0 +1,110 @@ +/****************************************************************** + This is the library for the Adafruit Motor Shield V2 for Arduino. + It supports DC motors & Stepper motors with microstepping as well + as stacking-support. It is *not* compatible with the V1 library! + + It will only work with https://www.adafruit.com/products/1483 + + Adafruit invests time and resources providing this open + source code, please support Adafruit and open-source hardware + by purchasing products from Adafruit! + + Written by Limor Fried/Ladyada for Adafruit Industries. + BSD license, check license.txt for more information. + All text above must be included in any redistribution. + ******************************************************************/ + +#ifndef _Adafruit_MotorShield_h_ +#define _Adafruit_MotorShield_h_ + +#include "utility/Adafruit_MS_PWMServoDriver.h" +#include +#include + +//#define MOTORDEBUG + +#define MICROSTEPS 16 // 8 or 16 + +#define MOTOR1_A 2 +#define MOTOR1_B 3 +#define MOTOR2_A 1 +#define MOTOR2_B 4 +#define MOTOR4_A 0 +#define MOTOR4_B 6 +#define MOTOR3_A 5 +#define MOTOR3_B 7 + +#define FORWARD 1 +#define BACKWARD 2 +#define BRAKE 3 +#define RELEASE 4 + +#define SINGLE 1 +#define DOUBLE 2 +#define INTERLEAVE 3 +#define MICROSTEP 4 + +class Adafruit_MotorShield; + +/** Object that controls and keeps state for a single DC motor */ +class Adafruit_DCMotor { +public: + Adafruit_DCMotor(void); + friend class Adafruit_MotorShield; ///< Let MotorShield create DCMotors + void run(uint8_t); + void setSpeed(uint8_t); + +private: + uint8_t PWMpin, IN1pin, IN2pin; + Adafruit_MotorShield *MC; + uint8_t motornum; +}; + +/** Object that controls and keeps state for a single stepper motor */ +class Adafruit_StepperMotor { +public: + Adafruit_StepperMotor(void); + void setSpeed(uint16_t); + + void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); + uint8_t onestep(uint8_t dir, uint8_t style); + void release(void); + + friend class Adafruit_MotorShield; ///< Let MotorShield create StepperMotors + +private: + uint32_t usperstep; + + uint8_t PWMApin, AIN1pin, AIN2pin; + uint8_t PWMBpin, BIN1pin, BIN2pin; + uint16_t revsteps; // # steps per revolution + uint8_t currentstep; + Adafruit_MotorShield *MC; + uint8_t steppernum; +}; + +/** Object that controls and keeps state for the whole motor shield. + Use it to create DC and Stepper motor objects! */ +class Adafruit_MotorShield { +public: + Adafruit_MotorShield(uint8_t addr = 0x60); + + void begin(uint16_t freq = 1600, TwoWire *theWire = NULL); + Adafruit_DCMotor *getMotor(uint8_t n); + Adafruit_StepperMotor *getStepper(uint16_t steps, uint8_t n); + + friend class Adafruit_DCMotor; ///< Let DCMotors control the Shield + + void setPWM(uint8_t pin, uint16_t val); + void setPin(uint8_t pin, boolean val); + +private: + TwoWire *_i2c; + uint8_t _addr; + uint16_t _freq; + Adafruit_DCMotor dcmotors[4]; + Adafruit_StepperMotor steppers[2]; + Adafruit_MS_PWMServoDriver _pwm; +}; + +#endif diff --git a/Node/Common/actuators/src/Adafruit_MotorShield.cpp b/Node/Common/actuators/src/Adafruit_MotorShield.cpp new file mode 100644 index 0000000..15f941b --- /dev/null +++ b/Node/Common/actuators/src/Adafruit_MotorShield.cpp @@ -0,0 +1,505 @@ +/*! + * @file Adafruit_MotorShield.cpp + * + * @mainpage Adafruit FXOS8700 accel/mag sensor driver + * + * @section intro_sec Introduction + * + * This is the library for the Adafruit Motor Shield V2 for Arduino. + * It supports DC motors & Stepper motors with microstepping as well + * as stacking-support. It is *not* compatible with the V1 library! + * For use with the Motor Shield https://www.adafruit.com/products/1483 + * and Motor FeatherWing https://www.adafruit.com/product/2927 + * + * This shield/wing uses I2C to communicate, 2 pins (SCL+SDA) are required + * to interface. + * + * Adafruit invests time and resources providing this open source code, + * please support Adafruit and open-source hardware by purchasing + * products from Adafruit! + * + * @section author Author + * + * Written by Limor Fried/Ladyada for Adafruit Industries. + * + * @section license License + * + * BSD license, all text here must be included in any redistribution. + * + */ + +#include "Adafruit_MotorShield.h" +#include "Arduino.h" +#include +#include + +#if (MICROSTEPS == 8) +///! A sinusoial microstepping curve for the PWM output (8-bit range) with 9 +/// points - last one is start of next step. +static uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; +#elif (MICROSTEPS == 16) +///! A sinusoial microstepping curve for the PWM output (8-bit range) with 17 +/// points - last one is start of next step. +static uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, + 197, 212, 225, 236, 244, 250, 253, 255}; +#endif + +/**************************************************************************/ +/*! + @brief Create the Motor Shield object at an I2C address, default is 0x60 + @param addr Optional I2C address if you've changed it +*/ +/**************************************************************************/ +Adafruit_MotorShield::Adafruit_MotorShield(uint8_t addr) { + _addr = addr; + _pwm = Adafruit_MS_PWMServoDriver(_addr); +} + +/**************************************************************************/ +/*! + @brief Initialize the I2C hardware and PWM driver, then turn off all pins. + @param freq + The PWM frequency for the driver, used for speed control and microstepping. + By default we use 1600 Hz which is a little audible but efficient. + @param theWire + A pointer to an optional I2C interface. If not provided, we use Wire or + Wire1 (on Due) +*/ +/**************************************************************************/ +void Adafruit_MotorShield::begin(uint16_t freq, TwoWire *theWire) { + if (!theWire) { +#if defined(ARDUINO_SAM_DUE) + _i2c = &Wire1; +#else + _i2c = &Wire; +#endif + } else { + _i2c = theWire; + } + + // init PWM w/_freq + _i2c->begin(); + _pwm.begin(); + _freq = freq; + _pwm.setPWMFreq(_freq); // This is the maximum PWM frequency + for (uint8_t i = 0; i < 16; i++) + _pwm.setPWM(i, 0, 0); +} + +/**************************************************************************/ +/*! + @brief Helper that sets the PWM output on a pin and manages 'all on or off' + @param pin The PWM output on the driver that we want to control (0-15) + @param value The 12-bit PWM value we want to set (0-4095) - 4096 is a + special 'all on' value +*/ +/**************************************************************************/ +void Adafruit_MotorShield::setPWM(uint8_t pin, uint16_t value) { + if (value > 4095) { + _pwm.setPWM(pin, 4096, 0); + } else + _pwm.setPWM(pin, 0, value); +} + +/**************************************************************************/ +/*! + @brief Helper that sets the PWM output on a pin as if it were a GPIO + @param pin The PWM output on the driver that we want to control (0-15) + @param value HIGH or LOW depending on the value you want! +*/ +/**************************************************************************/ +void Adafruit_MotorShield::setPin(uint8_t pin, boolean value) { + if (value == LOW) + _pwm.setPWM(pin, 0, 0); + else + _pwm.setPWM(pin, 4096, 0); +} + +/**************************************************************************/ +/*! + @brief Mini factory that will return a pointer to an already-allocated + Adafruit_DCMotor object. Initializes the DC motor and turns off all pins + @param num The DC motor port we want to use: 1 thru 4 are valid + @returns NULL if something went wrong, or a pointer to a Adafruit_DCMotor +*/ +/**************************************************************************/ +Adafruit_DCMotor *Adafruit_MotorShield::getMotor(uint8_t num) { + if (num > 4) + return NULL; + + num--; + + if (dcmotors[num].motornum == 0) { + // not init'd yet! + dcmotors[num].motornum = num; + dcmotors[num].MC = this; + uint8_t pwm, in1, in2; + if (num == 0) { + pwm = 8; + in2 = 9; + in1 = 10; + } else if (num == 1) { + pwm = 13; + in2 = 12; + in1 = 11; + } else if (num == 2) { + pwm = 2; + in2 = 3; + in1 = 4; + } else if (num == 3) { + pwm = 7; + in2 = 6; + in1 = 5; + } + dcmotors[num].PWMpin = pwm; + dcmotors[num].IN1pin = in1; + dcmotors[num].IN2pin = in2; + } + return &dcmotors[num]; +} + +/**************************************************************************/ +/*! + @brief Mini factory that will return a pointer to an already-allocated + Adafruit_StepperMotor object with a given 'steps per rotation. + Then initializes the stepper motor and turns off all pins. + @param steps How many steps per revolution (used for RPM calculation) + @param num The stepper motor port we want to use: only 1 or 2 are valid + @returns NULL if something went wrong, or a pointer to a + Adafruit_StepperMotor +*/ +/**************************************************************************/ +Adafruit_StepperMotor *Adafruit_MotorShield::getStepper(uint16_t steps, + uint8_t num) { + if (num > 2) + return NULL; + + num--; + + if (steppers[num].steppernum == 0) { + // not init'd yet! + steppers[num].steppernum = num; + steppers[num].revsteps = steps; + steppers[num].MC = this; + uint8_t pwma, pwmb, ain1, ain2, bin1, bin2; + if (num == 0) { + pwma = 8; + ain2 = 9; + ain1 = 10; + pwmb = 13; + bin2 = 12; + bin1 = 11; + } else if (num == 1) { + pwma = 2; + ain2 = 3; + ain1 = 4; + pwmb = 7; + bin2 = 6; + bin1 = 5; + } + steppers[num].PWMApin = pwma; + steppers[num].PWMBpin = pwmb; + steppers[num].AIN1pin = ain1; + steppers[num].AIN2pin = ain2; + steppers[num].BIN1pin = bin1; + steppers[num].BIN2pin = bin2; + } + return &steppers[num]; +} + +/****************************************** + MOTORS +******************************************/ + +/**************************************************************************/ +/*! + @brief Create a DCMotor object, un-initialized! + You should never call this, instead have the {@link Adafruit_MotorShield} + give you a DCMotor object with {@link Adafruit_MotorShield.getMotor} +*/ +/**************************************************************************/ +Adafruit_DCMotor::Adafruit_DCMotor(void) { + MC = NULL; + motornum = 0; + PWMpin = IN1pin = IN2pin = 0; +} + +/**************************************************************************/ +/*! + @brief Control the DC Motor direction and action + @param cmd The action to perform, can be FORWARD, BACKWARD or RELEASE +*/ +/**************************************************************************/ +void Adafruit_DCMotor::run(uint8_t cmd) { + switch (cmd) { + case FORWARD: + MC->setPin(IN2pin, LOW); // take low first to avoid 'break' + MC->setPin(IN1pin, HIGH); + break; + case BACKWARD: + MC->setPin(IN1pin, LOW); // take low first to avoid 'break' + MC->setPin(IN2pin, HIGH); + break; + case RELEASE: + MC->setPin(IN1pin, LOW); + MC->setPin(IN2pin, LOW); + break; + } +} + +/**************************************************************************/ +/*! + @brief Control the DC Motor speed/throttle + @param speed The 8-bit PWM value, 0 is off, 255 is on +*/ +/**************************************************************************/ +void Adafruit_DCMotor::setSpeed(uint8_t speed) { + MC->setPWM(PWMpin, speed * 16); +} + +/****************************************** + STEPPERS +******************************************/ + +/**************************************************************************/ +/*! + @brief Create a StepperMotor object, un-initialized! + You should never call this, instead have the {@link Adafruit_MotorShield} + give you a StepperMotor object with {@link Adafruit_MotorShield.getStepper} +*/ +/**************************************************************************/ +Adafruit_StepperMotor::Adafruit_StepperMotor(void) { + revsteps = steppernum = currentstep = 0; +} + +/**************************************************************************/ +/*! + @brief Set the delay for the Stepper Motor speed in RPM + @param rpm The desired RPM, we will do our best to reach it! +*/ +/**************************************************************************/ +void Adafruit_StepperMotor::setSpeed(uint16_t rpm) { + // Serial.println("steps per rev: "); Serial.println(revsteps); + // Serial.println("RPM: "); Serial.println(rpm); + + usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); +} + +/**************************************************************************/ +/*! + @brief Release all pins of the stepper motor so it free-spins +*/ +/**************************************************************************/ +void Adafruit_StepperMotor::release(void) { + MC->setPin(AIN1pin, LOW); + MC->setPin(AIN2pin, LOW); + MC->setPin(BIN1pin, LOW); + MC->setPin(BIN2pin, LOW); + MC->setPWM(PWMApin, 0); + MC->setPWM(PWMBpin, 0); +} + +/**************************************************************************/ +/*! + @brief Move the stepper motor with the given RPM speed, don't forget to + call + {@link Adafruit_StepperMotor.setSpeed} to set the speed! + @param steps The number of steps we want to move + @param dir The direction to go, can be FORWARD or BACKWARD + @param style How to perform each step, can be SINGLE, DOUBLE, INTERLEAVE or + MICROSTEP +*/ +/**************************************************************************/ +void Adafruit_StepperMotor::step(uint16_t steps, uint8_t dir, uint8_t style) { + uint32_t uspers = usperstep; + + if (style == INTERLEAVE) { + uspers /= 2; + } else if (style == MICROSTEP) { + uspers /= MICROSTEPS; + steps *= MICROSTEPS; +#ifdef MOTORDEBUG + Serial.print("steps = "); + Serial.println(steps, DEC); +#endif + } + + while (steps--) { + // Serial.println("step!"); Serial.println(uspers); + onestep(dir, style); + delayMicroseconds(uspers); +#ifdef ESP8266 + yield(); // required for ESP8266 +#endif + } +} + +/**************************************************************************/ +/*! + @brief Move the stepper motor one step only, with no delays + @param dir The direction to go, can be FORWARD or BACKWARD + @param style How to perform each step, can be SINGLE, DOUBLE, INTERLEAVE or + MICROSTEP + @returns The current step/microstep index, useful for + Adafruit_StepperMotor.step to keep track of the current location, especially + when microstepping +*/ +/**************************************************************************/ +uint8_t Adafruit_StepperMotor::onestep(uint8_t dir, uint8_t style) { + uint8_t ocrb, ocra; + + ocra = ocrb = 255; + + // next determine what sort of stepping procedure we're up to + if (style == SINGLE) { + if ((currentstep / (MICROSTEPS / 2)) % 2) { // we're at an odd step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS / 2; + } else { + currentstep -= MICROSTEPS / 2; + } + } else { // go to the next even step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } else { + currentstep -= MICROSTEPS; + } + } + } else if (style == DOUBLE) { + if (!(currentstep / (MICROSTEPS / 2) % 2)) { // we're at an even step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS / 2; + } else { + currentstep -= MICROSTEPS / 2; + } + } else { // go to the next odd step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } else { + currentstep -= MICROSTEPS; + } + } + } else if (style == INTERLEAVE) { + if (dir == FORWARD) { + currentstep += MICROSTEPS / 2; + } else { + currentstep -= MICROSTEPS / 2; + } + } + + if (style == MICROSTEP) { + if (dir == FORWARD) { + currentstep++; + } else { + // BACKWARDS + currentstep--; + } + + currentstep += MICROSTEPS * 4; + currentstep %= MICROSTEPS * 4; + + ocra = ocrb = 0; + if (currentstep < MICROSTEPS) { + ocra = microstepcurve[MICROSTEPS - currentstep]; + ocrb = microstepcurve[currentstep]; + } else if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS * 2)) { + ocra = microstepcurve[currentstep - MICROSTEPS]; + ocrb = microstepcurve[MICROSTEPS * 2 - currentstep]; + } else if ((currentstep >= MICROSTEPS * 2) && + (currentstep < MICROSTEPS * 3)) { + ocra = microstepcurve[MICROSTEPS * 3 - currentstep]; + ocrb = microstepcurve[currentstep - MICROSTEPS * 2]; + } else if ((currentstep >= MICROSTEPS * 3) && + (currentstep < MICROSTEPS * 4)) { + ocra = microstepcurve[currentstep - MICROSTEPS * 3]; + ocrb = microstepcurve[MICROSTEPS * 4 - currentstep]; + } + } + + currentstep += MICROSTEPS * 4; + currentstep %= MICROSTEPS * 4; + +#ifdef MOTORDEBUG + Serial.print("current step: "); + Serial.println(currentstep, DEC); + Serial.print(" pwmA = "); + Serial.print(ocra, DEC); + Serial.print(" pwmB = "); + Serial.println(ocrb, DEC); +#endif + MC->setPWM(PWMApin, ocra * 16); + MC->setPWM(PWMBpin, ocrb * 16); + + // release all + uint8_t latch_state = 0; // all motor pins to 0 + + // Serial.println(step, DEC); + if (style == MICROSTEP) { + if (currentstep < MICROSTEPS) + latch_state |= 0x03; + if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS * 2)) + latch_state |= 0x06; + if ((currentstep >= MICROSTEPS * 2) && (currentstep < MICROSTEPS * 3)) + latch_state |= 0x0C; + if ((currentstep >= MICROSTEPS * 3) && (currentstep < MICROSTEPS * 4)) + latch_state |= 0x09; + } else { + switch (currentstep / (MICROSTEPS / 2)) { + case 0: + latch_state |= 0x1; // energize coil 1 only + break; + case 1: + latch_state |= 0x3; // energize coil 1+2 + break; + case 2: + latch_state |= 0x2; // energize coil 2 only + break; + case 3: + latch_state |= 0x6; // energize coil 2+3 + break; + case 4: + latch_state |= 0x4; // energize coil 3 only + break; + case 5: + latch_state |= 0xC; // energize coil 3+4 + break; + case 6: + latch_state |= 0x8; // energize coil 4 only + break; + case 7: + latch_state |= 0x9; // energize coil 1+4 + break; + } + } +#ifdef MOTORDEBUG + Serial.print("Latch: 0x"); + Serial.println(latch_state, HEX); +#endif + + if (latch_state & 0x1) { + // Serial.println(AIN2pin); + MC->setPin(AIN2pin, HIGH); + } else { + MC->setPin(AIN2pin, LOW); + } + if (latch_state & 0x2) { + MC->setPin(BIN1pin, HIGH); + // Serial.println(BIN1pin); + } else { + MC->setPin(BIN1pin, LOW); + } + if (latch_state & 0x4) { + MC->setPin(AIN1pin, HIGH); + // Serial.println(AIN1pin); + } else { + MC->setPin(AIN1pin, LOW); + } + if (latch_state & 0x8) { + MC->setPin(BIN2pin, HIGH); + // Serial.println(BIN2pin); + } else { + MC->setPin(BIN2pin, LOW); + } + + return currentstep; +} diff --git a/Node/Common/drivers/inc/Adafruit_MS_PWMServoDriver.h b/Node/Common/drivers/inc/Adafruit_MS_PWMServoDriver.h new file mode 100644 index 0000000..135cf74 --- /dev/null +++ b/Node/Common/drivers/inc/Adafruit_MS_PWMServoDriver.h @@ -0,0 +1,59 @@ +/*************************************************** + This is a library for our Adafruit 16-channel PWM & Servo driver + + Pick one up today in the adafruit shop! + ------> http://www.adafruit.com/products/815 + + These displays use I2C to communicate, 2 pins are required to + interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4 + + Adafruit invests time and resources providing this open source code, + please support Adafruit and open-source hardware by purchasing + products from Adafruit! + + Written by Limor Fried/Ladyada for Adafruit Industries. + BSD license, all text above must be included in any redistribution + ****************************************************/ + +#ifndef _Adafruit_MS_PWMServoDriver_H +#define _Adafruit_MS_PWMServoDriver_H + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#define PCA9685_SUBADR1 0x2 +#define PCA9685_SUBADR2 0x3 +#define PCA9685_SUBADR3 0x4 + +#define PCA9685_MODE1 0x0 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x6 +#define LED0_ON_H 0x7 +#define LED0_OFF_L 0x8 +#define LED0_OFF_H 0x9 + +#define ALLLED_ON_L 0xFA +#define ALLLED_ON_H 0xFB +#define ALLLED_OFF_L 0xFC +#define ALLLED_OFF_H 0xFD + +class Adafruit_MS_PWMServoDriver { +public: + Adafruit_MS_PWMServoDriver(uint8_t addr = 0x40); + void begin(void); + void reset(void); + void setPWMFreq(float freq); + void setPWM(uint8_t num, uint16_t on, uint16_t off); + +private: + uint8_t _i2caddr; + + uint8_t read8(uint8_t addr); + void write8(uint8_t addr, uint8_t d); +}; + +#endif diff --git a/Node/Common/drivers/src/Adafruit_MS_PWMServoDriver.cpp b/Node/Common/drivers/src/Adafruit_MS_PWMServoDriver.cpp new file mode 100644 index 0000000..9afb470 --- /dev/null +++ b/Node/Common/drivers/src/Adafruit_MS_PWMServoDriver.cpp @@ -0,0 +1,114 @@ +/*************************************************** + This is a library for our Adafruit 16-channel PWM & Servo driver + + Pick one up today in the adafruit shop! + ------> http://www.adafruit.com/products/815 + + These displays use I2C to communicate, 2 pins are required to + interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4 + + Adafruit invests time and resources providing this open source code, + please support Adafruit and open-source hardware by purchasing + products from Adafruit! + + Written by Limor Fried/Ladyada for Adafruit Industries. + BSD license, all text above must be included in any redistribution + ****************************************************/ + +#include +#include +#if defined(ARDUINO_SAM_DUE) +#define WIRE Wire1 +#else +#define WIRE Wire +#endif + +Adafruit_MS_PWMServoDriver::Adafruit_MS_PWMServoDriver(uint8_t addr) { + _i2caddr = addr; +} + +void Adafruit_MS_PWMServoDriver::begin(void) { + WIRE.begin(); + reset(); +} + +void Adafruit_MS_PWMServoDriver::reset(void) { write8(PCA9685_MODE1, 0x0); } + +void Adafruit_MS_PWMServoDriver::setPWMFreq(float freq) { + // Serial.print("Attempting to set freq "); + // Serial.println(freq); + + freq *= + 0.9; // Correct for overshoot in the frequency setting (see issue #11). + + float prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + // Serial.print("Estimated pre-scale: "); Serial.println(prescaleval); + uint8_t prescale = floor(prescaleval + 0.5); + // Serial.print("Final pre-scale: "); Serial.println(prescale); + + uint8_t oldmode = read8(PCA9685_MODE1); + uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep + write8(PCA9685_MODE1, newmode); // go to sleep + write8(PCA9685_PRESCALE, prescale); // set the prescaler + write8(PCA9685_MODE1, oldmode); + delay(5); + write8(PCA9685_MODE1, + oldmode | + 0xa1); // This sets the MODE1 register to turn on auto increment. + // This is why the beginTransmission below was not working. + // Serial.print("Mode now 0x"); Serial.println(read8(PCA9685_MODE1), HEX); +} + +void Adafruit_MS_PWMServoDriver::setPWM(uint8_t num, uint16_t on, + uint16_t off) { + // Serial.print("Setting PWM "); Serial.print(num); Serial.print(": "); + // Serial.print(on); Serial.print("->"); Serial.println(off); + + WIRE.beginTransmission(_i2caddr); +#if ARDUINO >= 100 + WIRE.write(LED0_ON_L + 4 * num); + WIRE.write(on); + WIRE.write(on >> 8); + WIRE.write(off); + WIRE.write(off >> 8); +#else + WIRE.send(LED0_ON_L + 4 * num); + WIRE.send((uint8_t)on); + WIRE.send((uint8_t)(on >> 8)); + WIRE.send((uint8_t)off); + WIRE.send((uint8_t)(off >> 8)); +#endif + WIRE.endTransmission(); +} + +uint8_t Adafruit_MS_PWMServoDriver::read8(uint8_t addr) { + WIRE.beginTransmission(_i2caddr); +#if ARDUINO >= 100 + WIRE.write(addr); +#else + WIRE.send(addr); +#endif + WIRE.endTransmission(); + + WIRE.requestFrom((uint8_t)_i2caddr, (uint8_t)1); +#if ARDUINO >= 100 + return WIRE.read(); +#else + return WIRE.receive(); +#endif +} + +void Adafruit_MS_PWMServoDriver::write8(uint8_t addr, uint8_t d) { + WIRE.beginTransmission(_i2caddr); +#if ARDUINO >= 100 + WIRE.write(addr); + WIRE.write(d); +#else + WIRE.send(addr); + WIRE.send(d); +#endif + WIRE.endTransmission(); +} From 9bf42bf99837d5194a67fae4db28baaa52e22a2d Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Thu, 5 Nov 2020 18:06:43 -0330 Subject: [PATCH 02/12] Fix build issues --- Node/Common/actuators/inc/Adafruit_MotorShield.h | 2 +- Node/platformio.ini | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/Node/Common/actuators/inc/Adafruit_MotorShield.h b/Node/Common/actuators/inc/Adafruit_MotorShield.h index b99e397..7922cb9 100644 --- a/Node/Common/actuators/inc/Adafruit_MotorShield.h +++ b/Node/Common/actuators/inc/Adafruit_MotorShield.h @@ -17,7 +17,7 @@ #ifndef _Adafruit_MotorShield_h_ #define _Adafruit_MotorShield_h_ -#include "utility/Adafruit_MS_PWMServoDriver.h" +#include "../../drivers/inc/Adafruit_MS_PWMServoDriver.h" #include #include diff --git a/Node/platformio.ini b/Node/platformio.ini index 1c3ff31..70ca6aa 100644 --- a/Node/platformio.ini +++ b/Node/platformio.ini @@ -73,10 +73,14 @@ board = uno framework = arduino src_filter = + + + + + + + build_flags = -I./WindTunnel/ -I./Common/drivers/inc -I./Common/sensors/inc + -I./Common/actuators/inc [env:Examples] platform = atmelavr From 47356e632b26eaf2fdaaadc8f69353397651d0bb Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Sat, 7 Nov 2020 16:06:57 -0330 Subject: [PATCH 03/12] Begin creating actuation board interfacing code --- Node/Actuation/Actuation.cpp | 48 ++++++++++++++++++++++++++++++++++++ Node/Actuation/Actuation.h | 35 ++++++++++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 Node/Actuation/Actuation.cpp create mode 100644 Node/Actuation/Actuation.h diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp new file mode 100644 index 0000000..c49aee1 --- /dev/null +++ b/Node/Actuation/Actuation.cpp @@ -0,0 +1,48 @@ +#include "Actuation.h" + +ActuationManager::ActuationManager(TwoWire* pI2C, const uint8_t u8Cs) + : m_pI2C(pI2C), m_can(u8Cs), m_motorShield() +{} + +bool ActuationManager::receiveCommand() +{ + can_frame frame = {0}; + MCP2515::ERROR response = m_can.readMessage(&frame); + if (response != MCP2515::ERROR_OK) + { + // TODO handle error here + return false; + } + switch (frame.can_id) + { + case (eInitCommand): + { + uint16_t u16Data = (frame.data[0] << 8) | frame.data[1]; + uint8_t u8Address = frame.data[2]; + InitializeStepper(u16Data, u8Address); + break; + } + case (eSpeedCommand): + { + break; + } + case (eStepCommand): + { + break; + } + } + + return true; +} + +void AcutationManager::InitializeStepper(uint16_t u16Step, uint8_t u8Address) +{ + +} + +int main(void) +{ + ActuationManager manager{&Wire, 15}; + return 0; +} + diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h new file mode 100644 index 0000000..8a5f26d --- /dev/null +++ b/Node/Actuation/Actuation.h @@ -0,0 +1,35 @@ +#include + +#include +#include +#include +#include + + +class ActuationManager +{ +public: + ActuationManager(TwoWire*, const uint8_t); + + bool receiveCommand(); + +private: + enum ECommandResponse : uint8_t + { + eHeartbeat = 10, + eInitCommand = 12, + eInitResponse = 14, + eSpeedCommand = 16, + eSpeedResponse = 18, + eStepCommand = 20, + eStepResponse = 22 + }; + + void InitializeStepper(uint16_t, uint8_t); + + TwoWire* m_pI2C; + MCP2515 m_can; + Adafruit_MotorShield m_motorShield; + std::array m_apSteppers; +}; + From d53508628e1736c4680c207d7f3e2867620425ce Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Sat, 7 Nov 2020 16:09:47 -0330 Subject: [PATCH 04/12] Create actuation environment, add includes and sources to build --- Node/platformio.ini | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/Node/platformio.ini b/Node/platformio.ini index 70ca6aa..a433319 100644 --- a/Node/platformio.ini +++ b/Node/platformio.ini @@ -12,6 +12,21 @@ src_dir = ./ include_dir = Common +[env:Actuation-Uno] +platform = atmelavr +board = uno +framework = arduino +src_filter = + + + + + + + + +build_flags = + -I./Actuation/ + -I./Common/drivers/inc + -I./Common/sensors/inc + -I./Common/actuators/inc + [env:DTS-SAMD] platform = atmelsam board = sodaq_autonomo From 1ec62ce1bd32bcf5785048d84784d6e68b7d4c2a Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Sat, 7 Nov 2020 16:44:44 -0330 Subject: [PATCH 05/12] Continue defining command response methods --- Node/Actuation/Actuation.cpp | 52 ++++++++++++++++++++++++++++++++++-- Node/Actuation/Actuation.h | 10 ++++--- 2 files changed, 56 insertions(+), 6 deletions(-) diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp index c49aee1..3d9a922 100644 --- a/Node/Actuation/Actuation.cpp +++ b/Node/Actuation/Actuation.cpp @@ -19,11 +19,14 @@ bool ActuationManager::receiveCommand() { uint16_t u16Data = (frame.data[0] << 8) | frame.data[1]; uint8_t u8Address = frame.data[2]; - InitializeStepper(u16Data, u8Address); + InitStepper(u16Data, u8Address); break; } case (eSpeedCommand): { + uint16_t u16Data = (frame.data[0] << 8) | frame.data[1]; + uint8_t u8Address = frame.data[2]; + setStepperSpeed(u16Data, u8Address); break; } case (eStepCommand): @@ -35,9 +38,54 @@ bool ActuationManager::receiveCommand() return true; } -void AcutationManager::InitializeStepper(uint16_t u16Step, uint8_t u8Address) +void ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address) +{ + if (u8Address != 1 || u8Address != 2) + { + // TODO error handling + return; + } + if (m_apSteppers[u8Address - 1] == nullptr) + { + m_apSteppers[u8Address - 1] = m_motorShield.getStepper(u16Step, u8Address); + } + sendInitStepperResponse(u16Step, u8Address); +} + +void ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Address) { + uint8_t au8Data[] = { (uint8_t)((u16Step & 0xFF00) >> 8), (uint8_t)(u16Step & 0x00FF), u8Address }; + can_frame frame = { eInitResponse, 8, au8Data }; + MCP2515::ERROR status = m_can.sendMessage(&frame); + if (status != MCP2515::ERROR_OK) + { + // TODO error handling + } +} + +void ActuationManager::setStepperSpeed(uint16_t u16Speed, uint8_t u8Address) +{ + if (m_apSteppers[u8Address - 1] == nullptr) + { + // TODO error handling + return; + } + m_apSteppers[u8Address - 1]->setSpeed(u16Speed); + + sendStepperSpeedResponse(u16Speed, u8Address); +} + +void ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Address) +{ + uint8_t au8Data[] = { (uint8_t)((u16Speed & 0xFF00) >> 8), (uint8_t)(u16Speed & 0x00FF), u8Address }; + can_frame frame = { eSpeedResponse, 8, au8Data }; + + MCP2515::ERROR status = m_can.sendMessage(&frame); + if (status != MCP2515::ERROR_OK) + { + // TODO error handling + } } int main(void) diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h index 8a5f26d..908e9da 100644 --- a/Node/Actuation/Actuation.h +++ b/Node/Actuation/Actuation.h @@ -1,5 +1,3 @@ -#include - #include #include #include @@ -25,11 +23,15 @@ class ActuationManager eStepResponse = 22 }; - void InitializeStepper(uint16_t, uint8_t); + void InitStepper(uint16_t, uint8_t); + void sendInitStepperResponse(uint16_t, uint8_t); + + void setStepperSpeed(uint16_t, uint8_t); + void sendStepperSpeedResponse(uint16_t, uint8_t); TwoWire* m_pI2C; MCP2515 m_can; Adafruit_MotorShield m_motorShield; - std::array m_apSteppers; + Adafruit_StepperMotor* m_apSteppers[2]; }; From 3d679a7ef9438a36e69f166a3f4aa3f8903b68d3 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Sat, 7 Nov 2020 20:12:49 -0330 Subject: [PATCH 06/12] Finish first implementation of command and response methods --- Node/Actuation/Actuation.cpp | 36 ++++++++++++++++++++++++++++++++++++ Node/Actuation/Actuation.h | 10 ++++++++++ 2 files changed, 46 insertions(+) diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp index 3d9a922..b4b8546 100644 --- a/Node/Actuation/Actuation.cpp +++ b/Node/Actuation/Actuation.cpp @@ -31,6 +31,12 @@ bool ActuationManager::receiveCommand() } case (eStepCommand): { + StepperCommand command = { + (frame.data[0] << 8) | frame.data[1], + frame.data[2], + frame.data[3] + }; + stepperCommand(command, frame.data[4]); break; } } @@ -88,6 +94,36 @@ void ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Add } } +void ActuationManager::stepperCommand(StepperCommand command, uint8_t u8Address) +{ + if (m_apSteppers[u8Address - 1] == nullptr) + { + // TODO error handling + return; + } + m_apSteppers[u8Address - 1]->step(command.nSteps, command.u8Direction, command.u8Style); + + stepperResponse(command, u8Address); +} + +void ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address) +{ + uint8_t au8Data[] = { + (uint8_t)((command.nSteps & 0xFF00) >> 8), + (uint8_t)(command.nSteps & 0x00FF), + command.u8Direction, + command.u8Style, + u8Address + }; + can_frame frame = { eStepResponse, 8, au8Data }; + + MCP2515::ERROR status = m_can.sendMessage(&frame); + if (status != MCP2515::ERROR_OK) + { + // TODO error handling + } +} + int main(void) { ActuationManager manager{&Wire, 15}; diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h index 908e9da..e358c6c 100644 --- a/Node/Actuation/Actuation.h +++ b/Node/Actuation/Actuation.h @@ -23,12 +23,22 @@ class ActuationManager eStepResponse = 22 }; + struct StepperCommand + { + uint16_t nSteps; + uint8_t u8Direction; + uint8_t u8Style; + }; + void InitStepper(uint16_t, uint8_t); void sendInitStepperResponse(uint16_t, uint8_t); void setStepperSpeed(uint16_t, uint8_t); void sendStepperSpeedResponse(uint16_t, uint8_t); + void stepperCommand(StepperCommand, uint8_t); + void stepperResponse(StepperCommand, uint8_t); + TwoWire* m_pI2C; MCP2515 m_can; Adafruit_MotorShield m_motorShield; From a7a65a1d16339daf83b8359b234330f57ecd9206 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Mon, 9 Nov 2020 19:02:13 -0330 Subject: [PATCH 07/12] Add error handling, and finish defining methods Error handling is very premature, and just returns true or false based on whether or not actuation/configuration/sending a message was sucessful. Will look at creating an ERROR enum or something down the line --- Node/Actuation/Actuation.cpp | 63 +++++++++++++++++++++++------------- Node/Actuation/Actuation.h | 17 ++++++---- 2 files changed, 51 insertions(+), 29 deletions(-) diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp index b4b8546..25578cd 100644 --- a/Node/Actuation/Actuation.cpp +++ b/Node/Actuation/Actuation.cpp @@ -6,11 +6,11 @@ ActuationManager::ActuationManager(TwoWire* pI2C, const uint8_t u8Cs) bool ActuationManager::receiveCommand() { + bool bStatus; can_frame frame = {0}; MCP2515::ERROR response = m_can.readMessage(&frame); if (response != MCP2515::ERROR_OK) { - // TODO handle error here return false; } switch (frame.can_id) @@ -19,14 +19,14 @@ bool ActuationManager::receiveCommand() { uint16_t u16Data = (frame.data[0] << 8) | frame.data[1]; uint8_t u8Address = frame.data[2]; - InitStepper(u16Data, u8Address); + bStatus = InitStepper(u16Data, u8Address); break; } case (eSpeedCommand): { uint16_t u16Data = (frame.data[0] << 8) | frame.data[1]; uint8_t u8Address = frame.data[2]; - setStepperSpeed(u16Data, u8Address); + bStatus = setStepperSpeed(u16Data, u8Address); break; } case (eStepCommand): @@ -36,29 +36,28 @@ bool ActuationManager::receiveCommand() frame.data[2], frame.data[3] }; - stepperCommand(command, frame.data[4]); + bStatus = stepperCommand(command, frame.data[4]); break; } } - return true; + return bStatus; } -void ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address) +bool ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address) { if (u8Address != 1 || u8Address != 2) { - // TODO error handling - return; + return false; } if (m_apSteppers[u8Address - 1] == nullptr) { m_apSteppers[u8Address - 1] = m_motorShield.getStepper(u16Step, u8Address); } - sendInitStepperResponse(u16Step, u8Address); + return sendInitStepperResponse(u16Step, u8Address); } -void ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Address) +bool ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Address) { uint8_t au8Data[] = { (uint8_t)((u16Step & 0xFF00) >> 8), (uint8_t)(u16Step & 0x00FF), u8Address }; can_frame frame = { eInitResponse, 8, au8Data }; @@ -66,23 +65,24 @@ void ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Addre MCP2515::ERROR status = m_can.sendMessage(&frame); if (status != MCP2515::ERROR_OK) { - // TODO error handling + return false; } + + return true; } -void ActuationManager::setStepperSpeed(uint16_t u16Speed, uint8_t u8Address) +bool ActuationManager::setStepperSpeed(uint16_t u16Speed, uint8_t u8Address) { if (m_apSteppers[u8Address - 1] == nullptr) { - // TODO error handling - return; + return false; } m_apSteppers[u8Address - 1]->setSpeed(u16Speed); - sendStepperSpeedResponse(u16Speed, u8Address); + return sendStepperSpeedResponse(u16Speed, u8Address); } -void ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Address) +bool ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Address) { uint8_t au8Data[] = { (uint8_t)((u16Speed & 0xFF00) >> 8), (uint8_t)(u16Speed & 0x00FF), u8Address }; can_frame frame = { eSpeedResponse, 8, au8Data }; @@ -90,23 +90,24 @@ void ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Add MCP2515::ERROR status = m_can.sendMessage(&frame); if (status != MCP2515::ERROR_OK) { - // TODO error handling + return false; } + + return true; } -void ActuationManager::stepperCommand(StepperCommand command, uint8_t u8Address) +bool ActuationManager::stepperCommand(StepperCommand command, uint8_t u8Address) { if (m_apSteppers[u8Address - 1] == nullptr) { - // TODO error handling - return; + return false; } m_apSteppers[u8Address - 1]->step(command.nSteps, command.u8Direction, command.u8Style); - stepperResponse(command, u8Address); + return stepperResponse(command, u8Address); } -void ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address) +bool ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address) { uint8_t au8Data[] = { (uint8_t)((command.nSteps & 0xFF00) >> 8), @@ -120,8 +121,24 @@ void ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address MCP2515::ERROR status = m_can.sendMessage(&frame); if (status != MCP2515::ERROR_OK) { - // TODO error handling + return false; + } + + return true; +} + +bool ActuationManager::sendHeartbeat() +{ + can_frame frame = {0}; + frame.can_id = eHeartbeat; + + MCP2515::ERROR status = m_can.sendMessage(&frame); + if (status != MCP2515::ERROR_OK) + { + return false; } + + return true; } int main(void) diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h index e358c6c..7e096ca 100644 --- a/Node/Actuation/Actuation.h +++ b/Node/Actuation/Actuation.h @@ -3,6 +3,7 @@ #include #include +// TODO class wrapping watchdog timer and heartbeat can message class ActuationManager { @@ -30,14 +31,18 @@ class ActuationManager uint8_t u8Style; }; - void InitStepper(uint16_t, uint8_t); - void sendInitStepperResponse(uint16_t, uint8_t); + bool InitStepper(uint16_t, uint8_t); + bool sendInitStepperResponse(uint16_t, uint8_t); - void setStepperSpeed(uint16_t, uint8_t); - void sendStepperSpeedResponse(uint16_t, uint8_t); + bool setStepperSpeed(uint16_t, uint8_t); + bool sendStepperSpeedResponse(uint16_t, uint8_t); - void stepperCommand(StepperCommand, uint8_t); - void stepperResponse(StepperCommand, uint8_t); + bool stepperCommand(StepperCommand, uint8_t); + bool stepperResponse(StepperCommand, uint8_t); + + bool sendHeartbeat(); + + bool sendResponse(); TwoWire* m_pI2C; MCP2515 m_can; From 73fc5f58498baa41a254e5dc2ccd368696226ec9 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Mon, 9 Nov 2020 19:31:16 -0330 Subject: [PATCH 08/12] Properly construct can_id using prepareId method --- Node/Actuation/Actuation.cpp | 20 ++++++++++++++++---- Node/Common/drivers/inc/mcp2515.h | 2 +- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp index 25578cd..54179f8 100644 --- a/Node/Actuation/Actuation.cpp +++ b/Node/Actuation/Actuation.cpp @@ -60,7 +60,10 @@ bool ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address) bool ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Address) { uint8_t au8Data[] = { (uint8_t)((u16Step & 0xFF00) >> 8), (uint8_t)(u16Step & 0x00FF), u8Address }; - can_frame frame = { eInitResponse, 8, au8Data }; + uint8_t au8IdBuf[4]; + m_can.prepareId(au8IdBuf, false, eInitResponse); + + can_frame frame = { &au8IdBuf, 8, au8Data }; MCP2515::ERROR status = m_can.sendMessage(&frame); if (status != MCP2515::ERROR_OK) @@ -85,7 +88,10 @@ bool ActuationManager::setStepperSpeed(uint16_t u16Speed, uint8_t u8Address) bool ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Address) { uint8_t au8Data[] = { (uint8_t)((u16Speed & 0xFF00) >> 8), (uint8_t)(u16Speed & 0x00FF), u8Address }; - can_frame frame = { eSpeedResponse, 8, au8Data }; + uint8_t au8IdBuf[4]; + m_can.prepareId(au8IdBuf, false, eSpeedResponse); + + can_frame frame = { &au8IdBuf, 8, au8Data }; MCP2515::ERROR status = m_can.sendMessage(&frame); if (status != MCP2515::ERROR_OK) @@ -116,7 +122,10 @@ bool ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address command.u8Style, u8Address }; - can_frame frame = { eStepResponse, 8, au8Data }; + uint8_t au8IdBuf[4]; + m_can.prepareId(au8IdBuf, false, eStepResponse); + + can_frame frame = { &au8IdBuf, 8, au8Data }; MCP2515::ERROR status = m_can.sendMessage(&frame); if (status != MCP2515::ERROR_OK) @@ -129,8 +138,11 @@ bool ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address bool ActuationManager::sendHeartbeat() { + uint8_t au8IdBuf[4]; + m_can.prepareId(au8IdBuf, false, eHeartbeat); can_frame frame = {0}; - frame.can_id = eHeartbeat; + + frame.can_id = &au8IdBuf; MCP2515::ERROR status = m_can.sendMessage(&frame); if (status != MCP2515::ERROR_OK) diff --git a/Node/Common/drivers/inc/mcp2515.h b/Node/Common/drivers/inc/mcp2515.h index 7e38d18..1288524 100644 --- a/Node/Common/drivers/inc/mcp2515.h +++ b/Node/Common/drivers/inc/mcp2515.h @@ -453,7 +453,6 @@ class MCP2515 void setRegisters(const REGISTER reg, const uint8_t values[], const uint8_t n); void modifyRegister(const REGISTER reg, const uint8_t mask, const uint8_t data); - void prepareId(uint8_t *buffer, const bool ext, const uint32_t id); public: MCP2515(const uint8_t _CS); @@ -484,6 +483,7 @@ class MCP2515 void clearRXnOVR(void); void clearMERR(); void clearERRIF(); + void prepareId(uint8_t *buffer, const bool ext, const uint32_t id); }; #endif From 86ee8d49be2fdeb0144ea047e511230ac43cf7b2 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Mon, 9 Nov 2020 19:45:32 -0330 Subject: [PATCH 09/12] change m_can to be a pointer to MCP2515 object --- Node/Actuation/Actuation.cpp | 25 +++++++++++++------------ Node/Actuation/Actuation.h | 4 ++-- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp index 54179f8..e6daf36 100644 --- a/Node/Actuation/Actuation.cpp +++ b/Node/Actuation/Actuation.cpp @@ -1,14 +1,14 @@ #include "Actuation.h" -ActuationManager::ActuationManager(TwoWire* pI2C, const uint8_t u8Cs) - : m_pI2C(pI2C), m_can(u8Cs), m_motorShield() +ActuationManager::ActuationManager(TwoWire* pI2C, MCP2515* can) + : m_pI2C(pI2C), m_can(can), m_motorShield() {} bool ActuationManager::receiveCommand() { bool bStatus; can_frame frame = {0}; - MCP2515::ERROR response = m_can.readMessage(&frame); + MCP2515::ERROR response = m_can->readMessage(&frame); if (response != MCP2515::ERROR_OK) { return false; @@ -61,11 +61,11 @@ bool ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Addre { uint8_t au8Data[] = { (uint8_t)((u16Step & 0xFF00) >> 8), (uint8_t)(u16Step & 0x00FF), u8Address }; uint8_t au8IdBuf[4]; - m_can.prepareId(au8IdBuf, false, eInitResponse); + m_can->prepareId(au8IdBuf, false, eInitResponse); can_frame frame = { &au8IdBuf, 8, au8Data }; - MCP2515::ERROR status = m_can.sendMessage(&frame); + MCP2515::ERROR status = m_can->sendMessage(&frame); if (status != MCP2515::ERROR_OK) { return false; @@ -89,11 +89,11 @@ bool ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Add { uint8_t au8Data[] = { (uint8_t)((u16Speed & 0xFF00) >> 8), (uint8_t)(u16Speed & 0x00FF), u8Address }; uint8_t au8IdBuf[4]; - m_can.prepareId(au8IdBuf, false, eSpeedResponse); + m_can->prepareId(au8IdBuf, false, eSpeedResponse); can_frame frame = { &au8IdBuf, 8, au8Data }; - MCP2515::ERROR status = m_can.sendMessage(&frame); + MCP2515::ERROR status = m_can->sendMessage(&frame); if (status != MCP2515::ERROR_OK) { return false; @@ -123,11 +123,11 @@ bool ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address u8Address }; uint8_t au8IdBuf[4]; - m_can.prepareId(au8IdBuf, false, eStepResponse); + m_can->prepareId(au8IdBuf, false, eStepResponse); can_frame frame = { &au8IdBuf, 8, au8Data }; - MCP2515::ERROR status = m_can.sendMessage(&frame); + MCP2515::ERROR status = m_can->sendMessage(&frame); if (status != MCP2515::ERROR_OK) { return false; @@ -139,12 +139,12 @@ bool ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address bool ActuationManager::sendHeartbeat() { uint8_t au8IdBuf[4]; - m_can.prepareId(au8IdBuf, false, eHeartbeat); + m_can->prepareId(au8IdBuf, false, eHeartbeat); can_frame frame = {0}; frame.can_id = &au8IdBuf; - MCP2515::ERROR status = m_can.sendMessage(&frame); + MCP2515::ERROR status = m_can->sendMessage(&frame); if (status != MCP2515::ERROR_OK) { return false; @@ -155,7 +155,8 @@ bool ActuationManager::sendHeartbeat() int main(void) { - ActuationManager manager{&Wire, 15}; + MCP2515 can{15}; + ActuationManager manager{&Wire, &can}; return 0; } diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h index 7e096ca..8e2a006 100644 --- a/Node/Actuation/Actuation.h +++ b/Node/Actuation/Actuation.h @@ -8,7 +8,7 @@ class ActuationManager { public: - ActuationManager(TwoWire*, const uint8_t); + ActuationManager(TwoWire*, MCP2515*); bool receiveCommand(); @@ -45,7 +45,7 @@ class ActuationManager bool sendResponse(); TwoWire* m_pI2C; - MCP2515 m_can; + MCP2515* m_can; Adafruit_MotorShield m_motorShield; Adafruit_StepperMotor* m_apSteppers[2]; }; From c623fe628c39fb4befac5efa9ad43aaf2c45fc10 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Mon, 9 Nov 2020 19:57:14 -0330 Subject: [PATCH 10/12] Remove sendResponse declaration for now --- Node/Actuation/Actuation.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h index 8e2a006..6ffcce7 100644 --- a/Node/Actuation/Actuation.h +++ b/Node/Actuation/Actuation.h @@ -3,8 +3,6 @@ #include #include -// TODO class wrapping watchdog timer and heartbeat can message - class ActuationManager { public: @@ -42,8 +40,6 @@ class ActuationManager bool sendHeartbeat(); - bool sendResponse(); - TwoWire* m_pI2C; MCP2515* m_can; Adafruit_MotorShield m_motorShield; From bfb3214be5a656d7cb2dffce5a9685a8e36d636b Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Thu, 12 Nov 2020 18:55:54 -0330 Subject: [PATCH 11/12] Add error handling using MCP2515::ERROR enum --- Node/Actuation/Actuation.cpp | 88 +++++++++++++----------------------- Node/Actuation/Actuation.h | 17 +++---- 2 files changed, 40 insertions(+), 65 deletions(-) diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp index e6daf36..c5b1253 100644 --- a/Node/Actuation/Actuation.cpp +++ b/Node/Actuation/Actuation.cpp @@ -4,47 +4,45 @@ ActuationManager::ActuationManager(TwoWire* pI2C, MCP2515* can) : m_pI2C(pI2C), m_can(can), m_motorShield() {} -bool ActuationManager::receiveCommand() +MCP2515::ERROR ActuationManager::receiveCommand(can_frame* frame) { - bool bStatus; - can_frame frame = {0}; - MCP2515::ERROR response = m_can->readMessage(&frame); - if (response != MCP2515::ERROR_OK) - { - return false; - } - switch (frame.can_id) + return m_can->readMessage(&frame); +} + +MCP2515::ERROR handleCommand(can_frame* frame) +{ + switch (frame->can_id) { case (eInitCommand): { - uint16_t u16Data = (frame.data[0] << 8) | frame.data[1]; - uint8_t u8Address = frame.data[2]; - bStatus = InitStepper(u16Data, u8Address); + uint16_t u16Data = (frame->data[0] << 8) | frame->data[1]; + uint8_t u8Address = frame->data[2]; + status = InitStepper(u16Data, u8Address); break; } case (eSpeedCommand): { - uint16_t u16Data = (frame.data[0] << 8) | frame.data[1]; - uint8_t u8Address = frame.data[2]; - bStatus = setStepperSpeed(u16Data, u8Address); + uint16_t u16Data = (frame->data[0] << 8) | frame->data[1]; + uint8_t u8Address = frame->data[2]; + status = setStepperSpeed(u16Data, u8Address); break; } case (eStepCommand): { StepperCommand command = { - (frame.data[0] << 8) | frame.data[1], - frame.data[2], - frame.data[3] + (frame->data[0] << 8) | frame->data[1], + frame->data[2], + frame->data[3] }; - bStatus = stepperCommand(command, frame.data[4]); + status = stepperCommand(command, frame->data[4]); break; } } - return bStatus; + return status; } -bool ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address) +MCP2515::ERROR ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address) { if (u8Address != 1 || u8Address != 2) { @@ -57,7 +55,7 @@ bool ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address) return sendInitStepperResponse(u16Step, u8Address); } -bool ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Address) +MCP2515::ERROR ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Address) { uint8_t au8Data[] = { (uint8_t)((u16Step & 0xFF00) >> 8), (uint8_t)(u16Step & 0x00FF), u8Address }; uint8_t au8IdBuf[4]; @@ -65,27 +63,21 @@ bool ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8_t u8Addre can_frame frame = { &au8IdBuf, 8, au8Data }; - MCP2515::ERROR status = m_can->sendMessage(&frame); - if (status != MCP2515::ERROR_OK) - { - return false; - } - - return true; + return m_can->sendMessage(&frame); } -bool ActuationManager::setStepperSpeed(uint16_t u16Speed, uint8_t u8Address) +MCP2515::ERROR ActuationManager::setStepperSpeed(uint16_t u16Speed, uint8_t u8Address) { if (m_apSteppers[u8Address - 1] == nullptr) { - return false; + return MCP2515::ERROR_FAIL; } m_apSteppers[u8Address - 1]->setSpeed(u16Speed); return sendStepperSpeedResponse(u16Speed, u8Address); } -bool ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Address) +MCP2515::ERROR ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Address) { uint8_t au8Data[] = { (uint8_t)((u16Speed & 0xFF00) >> 8), (uint8_t)(u16Speed & 0x00FF), u8Address }; uint8_t au8IdBuf[4]; @@ -93,27 +85,21 @@ bool ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uint8_t u8Add can_frame frame = { &au8IdBuf, 8, au8Data }; - MCP2515::ERROR status = m_can->sendMessage(&frame); - if (status != MCP2515::ERROR_OK) - { - return false; - } - - return true; + return m_can->sendMessage(&frame); } -bool ActuationManager::stepperCommand(StepperCommand command, uint8_t u8Address) +MCP2515::ERROR ActuationManager::stepperCommand(StepperCommand command, uint8_t u8Address) { if (m_apSteppers[u8Address - 1] == nullptr) { - return false; + return MCP2515::ERROR_FAIL; } m_apSteppers[u8Address - 1]->step(command.nSteps, command.u8Direction, command.u8Style); return stepperResponse(command, u8Address); } -bool ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address) +MCP2515::ERROR ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address) { uint8_t au8Data[] = { (uint8_t)((command.nSteps & 0xFF00) >> 8), @@ -127,16 +113,10 @@ bool ActuationManager::stepperResponse(StepperCommand command, uint8_t u8Address can_frame frame = { &au8IdBuf, 8, au8Data }; - MCP2515::ERROR status = m_can->sendMessage(&frame); - if (status != MCP2515::ERROR_OK) - { - return false; - } - - return true; + return m_can->sendMessage(&frame); } -bool ActuationManager::sendHeartbeat() +MCP2515::ERROR ActuationManager::sendHeartbeat() { uint8_t au8IdBuf[4]; m_can->prepareId(au8IdBuf, false, eHeartbeat); @@ -144,13 +124,7 @@ bool ActuationManager::sendHeartbeat() frame.can_id = &au8IdBuf; - MCP2515::ERROR status = m_can->sendMessage(&frame); - if (status != MCP2515::ERROR_OK) - { - return false; - } - - return true; + return m_can->sendMessage(&frame); } int main(void) diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h index 6ffcce7..20d0c60 100644 --- a/Node/Actuation/Actuation.h +++ b/Node/Actuation/Actuation.h @@ -8,7 +8,8 @@ class ActuationManager public: ActuationManager(TwoWire*, MCP2515*); - bool receiveCommand(); + MCP2515::ERROR receiveCommand(); + MCP2515::ERROR handleCommand(can_frame*); private: enum ECommandResponse : uint8_t @@ -29,16 +30,16 @@ class ActuationManager uint8_t u8Style; }; - bool InitStepper(uint16_t, uint8_t); - bool sendInitStepperResponse(uint16_t, uint8_t); + MCP2515::ERROR InitStepper(uint16_t, uint8_t); + MCP2515::ERROR sendInitStepperResponse(uint16_t, uint8_t); - bool setStepperSpeed(uint16_t, uint8_t); - bool sendStepperSpeedResponse(uint16_t, uint8_t); + MCP2515::ERROR setStepperSpeed(uint16_t, uint8_t); + MCP2515::ERROR sendStepperSpeedResponse(uint16_t, uint8_t); - bool stepperCommand(StepperCommand, uint8_t); - bool stepperResponse(StepperCommand, uint8_t); + MCP2515::ERROR stepperCommand(StepperCommand, uint8_t); + MCP2515::ERROR stepperResponse(StepperCommand, uint8_t); - bool sendHeartbeat(); + MCP2515::ERROR sendHeartbeat(); TwoWire* m_pI2C; MCP2515* m_can; From 052797cb9580728205d2f7c3068176d7a0555c80 Mon Sep 17 00:00:00 2001 From: Burke-Daniel <52763491+Burke-Daniel@users.noreply.github.com> Date: Sun, 15 Nov 2020 12:46:26 -0330 Subject: [PATCH 12/12] Bug fixes in methods when constructing can frame --- Node/Actuation/Actuation.cpp | 23 ++++++++++++++--------- Node/Actuation/Actuation.h | 2 +- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp index c5b1253..5fe5f35 100644 --- a/Node/Actuation/Actuation.cpp +++ b/Node/Actuation/Actuation.cpp @@ -6,11 +6,12 @@ ActuationManager::ActuationManager(TwoWire* pI2C, MCP2515* can) MCP2515::ERROR ActuationManager::receiveCommand(can_frame* frame) { - return m_can->readMessage(&frame); + return m_can->readMessage(frame); } -MCP2515::ERROR handleCommand(can_frame* frame) +MCP2515::ERROR ActuationManager::handleCommand(can_frame* frame) { + MCP2515::ERROR status; switch (frame->can_id) { case (eInitCommand): @@ -30,7 +31,7 @@ MCP2515::ERROR handleCommand(can_frame* frame) case (eStepCommand): { StepperCommand command = { - (frame->data[0] << 8) | frame->data[1], + static_cast((frame->data[0] << 8) | frame->data[1]), frame->data[2], frame->data[3] }; @@ -46,7 +47,7 @@ MCP2515::ERROR ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address { if (u8Address != 1 || u8Address != 2) { - return false; + return MCP2515::ERROR_FAIL; } if (m_apSteppers[u8Address - 1] == nullptr) { @@ -61,7 +62,8 @@ MCP2515::ERROR ActuationManager::sendInitStepperResponse(uint16_t u16Step, uint8 uint8_t au8IdBuf[4]; m_can->prepareId(au8IdBuf, false, eInitResponse); - can_frame frame = { &au8IdBuf, 8, au8Data }; + canid_t* pIdBuf = (canid_t*)au8IdBuf; + can_frame frame = { *pIdBuf, 8, *au8Data }; return m_can->sendMessage(&frame); } @@ -82,8 +84,9 @@ MCP2515::ERROR ActuationManager::sendStepperSpeedResponse(uint16_t u16Speed, uin uint8_t au8Data[] = { (uint8_t)((u16Speed & 0xFF00) >> 8), (uint8_t)(u16Speed & 0x00FF), u8Address }; uint8_t au8IdBuf[4]; m_can->prepareId(au8IdBuf, false, eSpeedResponse); - - can_frame frame = { &au8IdBuf, 8, au8Data }; + + canid_t* pIdBuf = (canid_t*)au8IdBuf; + can_frame frame = { *pIdBuf, 8, *au8Data }; return m_can->sendMessage(&frame); } @@ -111,7 +114,8 @@ MCP2515::ERROR ActuationManager::stepperResponse(StepperCommand command, uint8_t uint8_t au8IdBuf[4]; m_can->prepareId(au8IdBuf, false, eStepResponse); - can_frame frame = { &au8IdBuf, 8, au8Data }; + canid_t* pIdBuf = (canid_t*)au8IdBuf; + can_frame frame = { *pIdBuf, 8, *au8Data }; return m_can->sendMessage(&frame); } @@ -122,7 +126,8 @@ MCP2515::ERROR ActuationManager::sendHeartbeat() m_can->prepareId(au8IdBuf, false, eHeartbeat); can_frame frame = {0}; - frame.can_id = &au8IdBuf; + canid_t* pIdBuf = (canid_t*)au8IdBuf; + frame.can_id = *pIdBuf; return m_can->sendMessage(&frame); } diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h index 20d0c60..aaa97cd 100644 --- a/Node/Actuation/Actuation.h +++ b/Node/Actuation/Actuation.h @@ -8,7 +8,7 @@ class ActuationManager public: ActuationManager(TwoWire*, MCP2515*); - MCP2515::ERROR receiveCommand(); + MCP2515::ERROR receiveCommand(can_frame*); MCP2515::ERROR handleCommand(can_frame*); private: