Initial Stepper Motor Interface Implementation

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<uint16_t>((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;
+}
+

Node/Actuation/Actuation.h

@@ -0,0 +1,49 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <Adafruit_MotorShield.h>
+#include <mcp2515.h>
+
+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];
+};
+

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 <Wire.h>
+#include <inttypes.h>
+
+//#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