Grabber

Safety/AttitudeManager/Inc/attitudeManager.hpp

@@ -1,6 +1,7 @@
 #pragma once
 #include "attitudeStateManager.hpp"
 #include "PPM.hpp"
+#include "grabber.hpp"
 class attitudeState;
 
 namespace AttMan
@@ -12,13 +13,14 @@ enum _Attitude_Manager_Cycle_Status {COMPLETED_CYCLE = 0, IN_CYCLE, FAILURE_MODE
 class attitudeManager
 {
     public:
-        attitudeManager(PPMChannel *ppm, PWMChannel *pwm);
+        attitudeManager(PPMChannel *ppm, PWMChannel *pwm, Grabber *grabber);
         inline attitudeState* getCurrentState() const {return currentState;}
         void execute();
         void setState(attitudeState& newState);
         AttMan::_Attitude_Manager_Cycle_Status getStatus() {return status;}
         PPMChannel *ppm;
         PWMChannel *pwm;
+        Grabber *grabber;
     private:
         attitudeState* currentState;
         AttMan::_Attitude_Manager_Cycle_Status status;

Safety/AttitudeManager/Inc/attitudeStateClasses.hpp

@@ -123,3 +123,16 @@ class DisarmMode : public attitudeState
         static uint8_t _armDisarmPPMValue;
         static uint8_t armDisarmTimeoutCount;
 };
+
+class PeripheralControlMode : public attitudeState
+{
+    public:
+        void enter(attitudeManager* attitudeMgr) {(void) attitudeMgr;}
+        void execute(attitudeManager* attitudeMgr);
+        void exit(attitudeManager* attitudeMgr) {(void) attitudeMgr;}
+        static attitudeState& getInstance();
+    private:
+        PeripheralControlMode() {}
+        PeripheralControlMode(const PeripheralControlMode& other);
+        PeripheralControlMode& operator =(const PeripheralControlMode& other);
+};

Safety/AttitudeManager/Src/attitudeManager.cpp

@@ -1,12 +1,13 @@
 #include "attitudeManager.hpp"
 #include "attitudeStateClasses.hpp"
 
-attitudeManager::attitudeManager(PPMChannel *ppmChannel, PWMChannel *pwmChannel)
+attitudeManager::attitudeManager(PPMChannel *ppmChannel, PWMChannel *pwmChannel, Grabber *grabber)
 {
     currentState = &fetchInstructionsMode::getInstance();
     status = AttMan::COMPLETED_CYCLE;
     ppm = ppmChannel;
     pwm = pwmChannel;
+    this->grabber = grabber;
 }
 
 void attitudeManager::setState(attitudeState& newState)

Safety/AttitudeManager/Src/attitudeManagerInterface.cpp

@@ -3,13 +3,15 @@
 #include "attitudeManager.hpp"
 #include "PPM.hpp"
 #include "PWM.hpp"
+#include "grabber.hpp"
 
 attitudeManager *attMng;
 
 void AttitudeManagerInterfaceInit(void) {
     PPMChannel *ppm = new PPMChannel(MAX_PPM_CHANNELS);
     PWMChannel *pwm = new PWMChannel();
-    attMng = new attitudeManager(ppm, pwm);
+    Grabber *grabber = &Grabber::getInstance(4,5,6,7,pwm); //TODO: What output channel?
+    attMng = new attitudeManager(ppm, pwm, grabber);
 }
 
 void AttitudeManagerInterfaceExecute(void)

Safety/AttitudeManager/Src/attitudeStateClasses.cpp

@@ -214,7 +214,7 @@ void OutputMixingMode::execute(attitudeManager* attitudeMgr)
         attitudeMgr->pwm->set(FRONT_RIGHT_MOTOR_CHANNEL, PidOutput -> frontRightMotorPercent);
         attitudeMgr->pwm->set(BACK_LEFT_MOTOR_CHANNEL, PidOutput -> backLeftMotorPercent);
         attitudeMgr->pwm->set(BACK_RIGHT_MOTOR_CHANNEL, PidOutput -> backRightMotorPercent);
-        attitudeMgr->setState(fetchInstructionsMode::getInstance()); // returning to beginning of state machine
+        attitudeMgr->setState(PeripheralControlMode::getInstance()); // returning to beginning of state machine
     }
     else
     {
@@ -318,4 +318,35 @@ bool DisarmMode:: isArmed()
     }
 
     return retVal;
-}
+}
+
+void PeripheralControlMode::execute(attitudeManager* attitudeMgr)
+{
+    const uint8_t speed = 10;
+    PPM_Instructions_t *teleopInstructions = fetchInstructionsMode::GetTeleopInstructions();
+    const uint8_t &clawInput = teleopInstructions->PPMValues[attitudeMgr->grabber->ppmClawChannel];
+    const uint8_t &craneInput = teleopInstructions->PPMValues[attitudeMgr->grabber->ppmCraneChannel];
+
+    if (clawInput < 30)
+        attitudeMgr->grabber->close(speed);
+    else if (clawInput < 70)
+        attitudeMgr->grabber->brake();
+    else
+        attitudeMgr->grabber->open(speed);
+
+    if (craneInput < 30)
+        attitudeMgr->grabber->lower(speed);
+    else if (craneInput < 70)
+        attitudeMgr->grabber->crane_brake();
+    else
+        attitudeMgr->grabber->raise(speed);
+
+
+    attitudeMgr->setState(fetchInstructionsMode::getInstance());
+}
+
+attitudeState& PeripheralControlMode::getInstance()
+{
+    static PeripheralControlMode singleton;
+    return singleton;
+}

Safety/Inc/grabber.hpp

@@ -0,0 +1,58 @@
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef GRABBER_HPP
+#define GRABBER_HPP
+
+
+/* Includes ------------------------------------------------------------------*/
+
+#include "stm32f0xx.h"
+#include "PWM.hpp"
+// #include <stdint.h>
+// #include "../boardfiles/Inc/i2c.h"
+
+/* Exported functions prototypes ---------------------------------------------*/
+
+class Grabber {
+public:
+  static Grabber& getInstance(uint8_t grabber_pwm_channel_1,
+                              uint8_t grabber_pwm_channel_2,
+                              uint8_t crane_pwm_channel_1,
+                              uint8_t crane_pwm_channel_2,
+                              PWMChannel *pwm);
+  Grabber(const Grabber*)=delete; 
+
+  void brake();
+  void open(uint8_t speed);
+  void close(uint8_t speed);
+
+  void crane_brake();
+  void crane_coast();
+  void lower(uint8_t speed);
+  void raise(uint8_t speed);
+
+  const uint8_t ppmCraneChannel = 6; // TODO: What channel?
+  const uint8_t ppmClawChannel = 7; // TODO: What channel?
+
+private:
+  Grabber() {};
+  Grabber(uint8_t grabber_pwm_channel_1,
+          uint8_t grabber_pwm_channel_2,
+          uint8_t crane_pwm_channel_1,
+          uint8_t crane_pwm_channel_2,
+          PWMChannel *pwm) : 
+    grabber_channel_1(grabber_pwm_channel_1),
+    grabber_channel_2(grabber_pwm_channel_2),
+    crane_channel_1(grabber_pwm_channel_1),
+    crane_channel_2(grabber_pwm_channel_2),
+    pwm(*pwm) {}
+
+  const uint8_t grabber_channel_1 = 0;
+  const uint8_t grabber_channel_2 = 0;
+  const uint8_t crane_channel_1 = 0;
+  const uint8_t crane_channel_2 = 0;
+  PWMChannel pwm;
+};
+#endif /* GRABBER_HPP */
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

Safety/Src/grabber.cpp

@@ -0,0 +1,76 @@
+
+/* Includes ------------------------------------------------------------------*/
+#include "grabber.hpp"
+#include "stm32f0xx_hal_gpio.h"
+
+/* Private define ------------------------------------------------------------*/
+
+
+/* Private variables ---------------------------------------------------------*/
+
+
+/* Public Methods  ---------------------------------------------------------*/
+
+Grabber& Grabber::getInstance(	uint8_t grabber_pwm_channel_1,
+								uint8_t grabber_pwm_channel_2,
+								uint8_t crane_pwm_channel_1,
+								uint8_t crane_pwm_channel_2,
+								PWMChannel *pwm) {
+  static Grabber singleton(	grabber_pwm_channel_1,
+							grabber_pwm_channel_2,
+							crane_pwm_channel_1,
+							crane_pwm_channel_2,
+							pwm); 
+  return singleton; 
+}
+
+// Grabber lowering is controlled based on this table:
+// https://cdn-shop.adafruit.com/product-files/3190/drv8871.pdf
+// DRV8871 - Section 7.3.1
+// === H-Bridge Control ===
+// | IN1 | IN2 | OUT1   | OUT2   | DESCRIPTION
+// |-----|-----|--------|--------|-------------
+// |  0  |  0  | High-Z | High-Z | Coast; H-bridge disabled to High-Z (sleep entered after 1 ms)
+// |  0  |  1  | L      | H      | Reverse (Current OUT2 --> OUT1)
+// |  1  |  0  | H      | L      | Forward (Current OUT1 --> OUT2)
+// |  1  |  1  | L      | L      | Brake; low-side slow decay
+
+void Grabber::crane_brake() {
+	pwm.set(this->crane_channel_1, 100);
+	pwm.set(this->crane_channel_2, 100);
+}
+void Grabber::crane_coast() {
+	pwm.set(this->crane_channel_1, 0);
+	pwm.set(this->crane_channel_2, 0);
+}
+// obviously this is not instant, and we probably need a callback once the device
+// is finished lowering to set the motor into brake mode and update its state
+// Unfortunately I dont know what that will look like... It could very well be time based
+// need more details from Mech/Electrical
+// Update: This will just rely on pilot visual unless otherwise required
+void Grabber::lower(uint8_t speed) {
+	pwm.set(this->crane_channel_1, speed);
+	pwm.set(this->crane_channel_2, 0);
+}
+void Grabber::raise(uint8_t speed) {
+	pwm.set(this->crane_channel_1, 0);
+	pwm.set(this->crane_channel_2, speed);
+}
+
+
+
+// Using https://media.digikey.com/pdf/Data%20Sheets/Seeed%20Technology/105090004_Web.pdf
+void Grabber::brake() {
+	pwm.set(this->grabber_channel_1, 0);
+	pwm.set(this->grabber_channel_2, 0);
+}
+void Grabber::close(uint8_t speed) {
+	pwm.set(this->grabber_channel_1, speed);
+	pwm.set(this->grabber_channel_2, 0);
+}
+void Grabber::open(uint8_t speed) {
+	pwm.set(this->grabber_channel_1, 0);
+	pwm.set(this->grabber_channel_2, speed);
+}
+
+/* Private methods  ---------------------------------------------------------*/