diff --git a/Node/Actuation/Actuation.cpp b/Node/Actuation/Actuation.cpp new file mode 100644 index 0000000..5fe5f35 --- /dev/null +++ b/Node/Actuation/Actuation.cpp @@ -0,0 +1,141 @@ +#include "Actuation.h" + +ActuationManager::ActuationManager(TwoWire* pI2C, MCP2515* can) + : m_pI2C(pI2C), m_can(can), m_motorShield() +{} + +MCP2515::ERROR ActuationManager::receiveCommand(can_frame* frame) +{ + return m_can->readMessage(frame); +} + +MCP2515::ERROR ActuationManager::handleCommand(can_frame* frame) +{ + MCP2515::ERROR status; + switch (frame->can_id) + { + case (eInitCommand): + { + 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]; + status = setStepperSpeed(u16Data, u8Address); + break; + } + case (eStepCommand): + { + StepperCommand command = { + static_cast((frame->data[0] << 8) | frame->data[1]), + frame->data[2], + frame->data[3] + }; + status = stepperCommand(command, frame->data[4]); + break; + } + } + + return status; +} + +MCP2515::ERROR ActuationManager::InitStepper(uint16_t u16Step, uint8_t u8Address) +{ + if (u8Address != 1 || u8Address != 2) + { + return MCP2515::ERROR_FAIL; + } + if (m_apSteppers[u8Address - 1] == nullptr) + { + m_apSteppers[u8Address - 1] = m_motorShield.getStepper(u16Step, u8Address); + } + return sendInitStepperResponse(u16Step, 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]; + m_can->prepareId(au8IdBuf, false, eInitResponse); + + canid_t* pIdBuf = (canid_t*)au8IdBuf; + can_frame frame = { *pIdBuf, 8, *au8Data }; + + return m_can->sendMessage(&frame); +} + +MCP2515::ERROR ActuationManager::setStepperSpeed(uint16_t u16Speed, uint8_t u8Address) +{ + if (m_apSteppers[u8Address - 1] == nullptr) + { + return MCP2515::ERROR_FAIL; + } + m_apSteppers[u8Address - 1]->setSpeed(u16Speed); + + return sendStepperSpeedResponse(u16Speed, 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]; + m_can->prepareId(au8IdBuf, false, eSpeedResponse); + + canid_t* pIdBuf = (canid_t*)au8IdBuf; + can_frame frame = { *pIdBuf, 8, *au8Data }; + + return m_can->sendMessage(&frame); +} + +MCP2515::ERROR ActuationManager::stepperCommand(StepperCommand command, uint8_t u8Address) +{ + if (m_apSteppers[u8Address - 1] == nullptr) + { + return MCP2515::ERROR_FAIL; + } + m_apSteppers[u8Address - 1]->step(command.nSteps, command.u8Direction, command.u8Style); + + return stepperResponse(command, u8Address); +} + +MCP2515::ERROR 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 + }; + uint8_t au8IdBuf[4]; + m_can->prepareId(au8IdBuf, false, eStepResponse); + + canid_t* pIdBuf = (canid_t*)au8IdBuf; + can_frame frame = { *pIdBuf, 8, *au8Data }; + + return m_can->sendMessage(&frame); +} + +MCP2515::ERROR ActuationManager::sendHeartbeat() +{ + uint8_t au8IdBuf[4]; + m_can->prepareId(au8IdBuf, false, eHeartbeat); + can_frame frame = {0}; + + canid_t* pIdBuf = (canid_t*)au8IdBuf; + frame.can_id = *pIdBuf; + + return m_can->sendMessage(&frame); +} + +int main(void) +{ + MCP2515 can{15}; + ActuationManager manager{&Wire, &can}; + return 0; +} + diff --git a/Node/Actuation/Actuation.h b/Node/Actuation/Actuation.h new file mode 100644 index 0000000..aaa97cd --- /dev/null +++ b/Node/Actuation/Actuation.h @@ -0,0 +1,49 @@ +#include +#include +#include +#include + +class ActuationManager +{ +public: + ActuationManager(TwoWire*, MCP2515*); + + MCP2515::ERROR receiveCommand(can_frame*); + MCP2515::ERROR handleCommand(can_frame*); + +private: + enum ECommandResponse : uint8_t + { + eHeartbeat = 10, + eInitCommand = 12, + eInitResponse = 14, + eSpeedCommand = 16, + eSpeedResponse = 18, + eStepCommand = 20, + eStepResponse = 22 + }; + + struct StepperCommand + { + uint16_t nSteps; + uint8_t u8Direction; + uint8_t u8Style; + }; + + MCP2515::ERROR InitStepper(uint16_t, uint8_t); + MCP2515::ERROR sendInitStepperResponse(uint16_t, uint8_t); + + MCP2515::ERROR setStepperSpeed(uint16_t, uint8_t); + MCP2515::ERROR sendStepperSpeedResponse(uint16_t, uint8_t); + + MCP2515::ERROR stepperCommand(StepperCommand, uint8_t); + MCP2515::ERROR stepperResponse(StepperCommand, uint8_t); + + MCP2515::ERROR sendHeartbeat(); + + TwoWire* m_pI2C; + MCP2515* m_can; + Adafruit_MotorShield m_motorShield; + Adafruit_StepperMotor* m_apSteppers[2]; +}; + diff --git a/Node/Common/actuators/inc/Adafruit_MotorShield.h b/Node/Common/actuators/inc/Adafruit_MotorShield.h new file mode 100644 index 0000000..7922cb9 --- /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 "../../drivers/inc/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/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 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(); +} diff --git a/Node/platformio.ini b/Node/platformio.ini index 1c3ff31..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 @@ -73,10 +88,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