Draft issue #396 arm module refactor.

rover-apps/arm_2021/include/Modules/ClawModule.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include <numeric>
+
+#include "ActuatorControllerManager.h"
+#include "Current.h"
+#include "DCMotor.h"
+#include "LookupTable.h"
+#include "OpenLoop.h"
+#include "PID.h"
+#include "Position.h"
+#include "Quadrature64CPR.h"
+#include "Velocity.h"
+#include "hw_bridge.h"
+
+class ClawModule final : public Module {
+ public:
+  ElbowModule();
+  HWBRIDGE::CANSignalValue_t clawSetPoint;
+
+  void periodic_1s(void) override;
+  void periodic_10s(void) override;
+  void periodic_100ms(void) override;
+  void periodic_10ms(void) override;
+  void periodic_1ms(CANInterface& can) override;
+
+  Encoder::Quadrature64CPR encoder;
+  Actuator::DCMotor motor;
+
+  // static Sensor::CurrentSensor currentSensor(CLAW_CRNT_SNS_SPI_CLK, CLAW_CRNT_SNS_SPI_MISO, CLAW_CRNT_SNS_SPI_CS);
+  // TODO: Add once current sensor driver works
+
+  PID::PID velPID;
+  PID::PID posPID;
+  PID::PID curPID;
+
+  constexpr float POLOLUMAXCURRENT = 3;
+  constexpr float POLOLUMADEGPERSEC =
+      std::numeric_limits<float>::infinity();  // TODO: figure out MAXDEGPERSEC of motors (1680?);
+
+  Controller::Velocity vel;
+  Controller::Position pos;
+  // static Controller::Current cur(motor, encoder, std::nullopt, curPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT,
+  // LIM_CLAW_OPEN, NC);
+  Controller::OpenLoop open;
+
+  const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel},
+                                      {HWBRIDGE::CONTROL::Mode::POSITION, &pos},
+                                      // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur},
+                                      {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}};
+
+  Controller::ActuatorControllerManager manager;
+
+ private:
+}

rover-apps/arm_2021/include/Modules/ElbowModule.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include "AEAT6012.h"
+#include "ActuatorControllerManager.h"
+#include "Current.h"
+#include "DCMotor.h"
+#include "LookupTable.h"
+#include "Module.h"
+#include "OpenLoop.h"
+#include "PID.h"
+#include "Position.h"
+#include "Velocity.h"
+#include "hw_bridge.h"
+#include "mbed.h"
+
+class ElbowModule final : public Module {
+ public:
+  ElbowModule();
+  HWBRIDGE::CANSignalValue_t elbowSetPoint;
+
+  void periodic_1s(void) override;
+  void periodic_10s(void) override;
+  void periodic_100ms(void) override;
+  void periodic_10ms(void) override;
+  void periodic_1ms(CANInterface& can) override;
+
+  Encoder::AEAT6012 encoder;
+  Actuator::DCMotor motor;
+
+  // static Sensor::CurrentSensor currentSensor(TRNTBL_CRNT_SNS_SPI_CLK, TRNTBL_CRNT_SNS_SPI_MISO,
+  // TRNTBL_CRNT_SNS_SPI_CS);
+  // TODO: Add once current sensor driver works
+
+  PID::PID velPID;
+  PID::PID posPID;
+  PID::PID curPID;
+
+  constexpr float ASSUNMAXCURRENT = 25.263;
+constexpr float ASSUNMAXDEGPERSEC =
+    std::numeric_limits<float>::infinity();  
+    // TODO: figure out MAXDEGPERSEC of motors (35580?);
+
+
+  Controller::Velocity vel;
+  Controller::Position pos;
+  // static Controller::Current cur(motor, encoder, std::nullopt, curPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS,
+  //                               LIM_TRNTBL_RHS);
+  Controller::OpenLoop open;
+
+  const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel},
+                                      {HWBRIDGE::CONTROL::Mode::POSITION, &pos},
+                                      //{HWBRIDGE::CONTROL::Mode::CURRENT, &cur},
+                                      {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}};
+
+  Controller::ActuatorControllerManager manager;
+
+ private:
+}

rover-apps/arm_2021/include/Modules/ShoulderModule.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include "AEAT6012.h"
+#include "ActuatorControllerManager.h"
+#include "Current.h"
+#include "DCMotor.h"
+#include "LookupTable.h"
+#include "Module.h"
+#include "OpenLoop.h"
+#include "PID.h"
+#include "Position.h"
+#include "Velocity.h"
+#include "hw_bridge.h"
+#include "mbed.h"
+
+class ShoulderModule final : public Module {
+ public:
+  TurntableModule();
+  HWBRIDGE::CANSignalValue_t shoulderSetPoint;
+
+  void periodic_1s(void) override;
+  void periodic_10s(void) override;
+  void periodic_100ms(void) override;
+  void periodic_10ms(void) override;
+  void periodic_1ms(CANInterface& can) override;
+
+  Encoder::AEAT6012 encoder;
+  Actuator::DCMotor motor;
+
+  // static Sensor::CurrentSensor currentSensor(TRNTBL_CRNT_SNS_SPI_CLK, TRNTBL_CRNT_SNS_SPI_MISO,
+  // TRNTBL_CRNT_SNS_SPI_CS);
+  // TODO: Add once current sensor driver works
+
+  PID::PID velPID;
+  PID::PID posPID;
+  PID::PID curPID;
+
+  constexpr uint8_t PA04MAXCURRENT = 6;
+  constexpr float PA04MAXDEGPERSEC = std::numeric_limits<float>::infinity();
+  // TODO: figure out MAXDEGPERSEC of motors
+
+  Controller::Velocity vel;
+  Controller::Position pos;
+  // static Controller::Current cur(motor, encoder, std::nullopt, curPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS,
+  //                               LIM_TRNTBL_RHS);
+  Controller::OpenLoop open;
+
+  const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel},
+                                      {HWBRIDGE::CONTROL::Mode::POSITION, &pos},
+                                      //{HWBRIDGE::CONTROL::Mode::CURRENT, &cur},
+                                      {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}};
+
+  Controller::ActuatorControllerManager manager;
+
+ private:
+}

rover-apps/arm_2021/include/Modules/TooltipModule.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <numeric>
+
+#include "LimServo.h"
+
+class ClawModule final : public Module {
+ public:
+  ElbowModule();
+  HWBRIDGE::CANSignalValue_t toolTipPosition;
+
+  Actuator::LimServo clawTooltipServo;
+
+  void periodic_1s(void) override;
+  void periodic_10s(void) override;
+  void periodic_100ms(void) override;
+  void periodic_10ms(void) override;
+  void periodic_1ms(CANInterface& can) override;
+
+  Encoder::Quadrature64CPR encoder;
+  Actuator::DCMotor motor;
+
+  // static Sensor::CurrentSensor currentSensor(CLAW_CRNT_SNS_SPI_CLK, CLAW_CRNT_SNS_SPI_MISO, CLAW_CRNT_SNS_SPI_CS);
+  // TODO: Add once current sensor driver works
+
+  PID::PID velPID;
+  PID::PID posPID;
+  PID::PID curPID;
+
+  constexpr float POLOLUMAXCURRENT = 3;
+  constexpr float POLOLUMADEGPERSEC =
+      std::numeric_limits<float>::infinity();  // TODO: figure out MAXDEGPERSEC of motors (1680?);
+
+  Controller::Velocity vel;
+  Controller::Position pos;
+  // static Controller::Current cur(motor, encoder, std::nullopt, curPID, POLOLUMADEGPERSEC, POLOLUMAXCURRENT,
+  // LIM_CLAW_OPEN, NC);
+  Controller::OpenLoop open;
+
+  const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel},
+                                      {HWBRIDGE::CONTROL::Mode::POSITION, &pos},
+                                      // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur},
+                                      {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}};
+
+  Controller::ActuatorControllerManager manager;
+
+ private:
+}

rover-apps/arm_2021/include/Modules/TurntableModule.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include "AEAT6012.h"
+#include "ActuatorControllerManager.h"
+#include "Current.h"
+#include "DCMotor.h"
+#include "LookupTable.h"
+#include "OpenLoop.h"
+#include "PID.h"
+#include "Position.h"
+#include "Velocity.h"
+#include "hw_bridge.h"
+
+class TurntableModule final : public Module {
+ public:
+  TurntableModule();
+  HWBRIDGE::CANSignalValue_t turntableSetPoint;
+
+  void periodic_1s(void) override;
+  void periodic_10s(void) override;
+  void periodic_100ms(void) override;
+  void periodic_10ms(void) override;
+  void periodic_1ms(CANInterface& can) override;
+
+  Encoder::AEAT6012 encoder;
+  Actuator::DCMotor motor;
+
+  // static Sensor::CurrentSensor currentSensor(TRNTBL_CRNT_SNS_SPI_CLK, TRNTBL_CRNT_SNS_SPI_MISO,
+  // TRNTBL_CRNT_SNS_SPI_CS);
+  // TODO: Add once current sensor driver works
+
+  PID::PID velPID;
+  PID::PID posPID;
+  PID::PID curPID;
+
+  constexpr uint8_t MAXCURRENT = 53;
+  constexpr float MAXDEGPERSEC = std::numeric_limits<float>::infinity();
+  // TODO: figure out MAXDEGPERSEC of motors (79080?)
+
+  Controller::Velocity vel;
+  Controller::Position pos;
+  // static Controller::Current cur(motor, encoder, std::nullopt, curPID, MAXDEGPERSEC, MAXCURRENT, LIM_TRNTBL_LHS,
+  //                               LIM_TRNTBL_RHS);
+  Controller::OpenLoop open;
+
+  const Controller::ControlMap lut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &vel},
+                                      {HWBRIDGE::CONTROL::Mode::POSITION, &pos},
+                                      // {HWBRIDGE::CONTROL::Mode::CURRENT, &cur},
+                                      {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &open}};
+
+  Controller::ActuatorControllerManager manager;
+
+ private:
+}

rover-apps/arm_2021/include/Modules/WristModule.h

@@ -0,0 +1,77 @@
+#pragma once
+
+#include <numeric>
+
+#include "ActuatorControllerManager.h"
+#include "Current.h"
+#include "DCMotor.h"
+#include "LookupTable.h"
+#include "OpenLoop.h"
+#include "PID.h"
+#include "Position.h"
+#include "Quadrature64CPR.h"
+#include "Velocity.h"
+#include "hw_bridge.h"
+
+class WristModule final : public Module {
+ public:
+  WristModule();
+  HWBRIDGE::CANSignalValue_t leftWristSetPoint  = 0;
+  HWBRIDGE::CANSignalValue_t rightWristSetPoint = 0;
+
+  void periodic_1s(void) override;
+  void periodic_10s(void) override;
+  void periodic_100ms(void) override;
+  void periodic_10ms(void) override;
+  void periodic_1ms(CANInterface& can) override;
+
+  Encoder::Quadrature64CPR leftEncoder;
+  Encoder::Quadrature64CPR rightEncoder;
+
+  Actuator::DCMotor leftMotor;
+  Actuator::DCMotor rightMotor;
+
+  /* TODO: Can we use wrist limit switches if motors are individually addressable */
+
+  // static Sensor::CurrentSensor leftCurrentSensor(WRSTL_CRNT_SNS_SPI_CLK, WRSTL_CRNT_SNS_SPI_MISO,
+  // WRSTL_CRNT_SNS_SPI_CS),
+  //     rightCurrentSensor(WRSTR_CRNT_SNS_SPI_CLK, WRSTR_CRNT_SNS_SPI_MISO, WRSTR_CRNT_SNS_SPI_CS);
+  // TODO: Add once current sensor driver works
+
+  PID::PID leftVelPID({1, 0, 0, -1, 1, 0, false, false});
+  PID::PID leftPosPID({1, 0, 0, -1, 1, 0, false, false});
+  PID::PID leftCurPID({1, 0, 0, -1, 1, 0, false, false});
+
+  PID::PID rightVelPID({1, 0, 0, -1, 1, 0, false, false});
+  PID::PID rightPosPID({1, 0, 0, -1, 1, 0, false, false});
+  PID::PID rightCurPID({1, 0, 0, -1, 1, 0, false, false});
+
+  constexpr uint8_t POLOLUMAXCURRENT = 3;
+  constexpr float POLOLUMAXDEGPERSEC = std::numeric_limits<float>::infinity();
+  // TODO: figure out MAXDEGPERSEC of motors (1680?)
+
+  Controller::Velocity leftVel;
+  Controller::Position leftPos;
+  // Controller::Current leftCur(leftMotor, leftEncoder, std::nullopt, leftCurPID, POLOLUMAXDEGPERSEC,
+  //
+  Controller::OpenLoop leftOpen;
+
+  Controller::Velocity rightVel;
+  Controller::Position rightPos;
+  // Controller::Current rightCur(rightMotor, rightEncoder, std::nullopt, rightCurPID, POLOLUMAXDEGPERSEC,
+  //
+  Controller::OpenLoop rightOpen;
+
+  const Controller::ControlMap leftLut  = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &leftVel},
+                                          {HWBRIDGE::CONTROL::Mode::POSITION, &leftPos},
+                                          // {HWBRIDGE::CONTROL::Mode::CURRENT, &leftCur},
+                                          {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &leftOpen}};
+  const Controller::ControlMap rightLut = {{HWBRIDGE::CONTROL::Mode::VELOCITY, &rightVel},
+                                           {HWBRIDGE::CONTROL::Mode::POSITION, &rightPos},
+                                           // {HWBRIDGE::CONTROL::Mode::CURRENT, &rightCur},
+                                           {HWBRIDGE::CONTROL::Mode::OPEN_LOOP, &rightOpen}};
+  Controller::ActuatorControllerManager leftManager;
+  Controller::ActuatorControllerManager rightManager;
+
+ private:
+}