Gimbal grabber integration

Safety/AttitudeManager/Inc/AttitudeDatatypes.hpp

@@ -17,8 +17,10 @@ constexpr static uint8_t FRONT_LEFT_MOTOR_CHANNEL {0};
 constexpr static uint8_t FRONT_RIGHT_MOTOR_CHANNEL {1};
 constexpr static uint8_t BACK_LEFT_MOTOR_CHANNEL {2};
 constexpr static uint8_t BACK_RIGHT_MOTOR_CHANNEL {3};
+constexpr static uint8_t LEFT_GIMBAL {4};
+constexpr static uint8_t RIGHT_GIMBAL {5};
 
-constexpr static uint8_t NUM_PWM_CHANNELS {4}; // keeping them at 4 motors only for now, will def be changed as more elements added
+constexpr static uint8_t NUM_PWM_CHANNELS {6}; // 4 motors plus two actuators for gimbal
 
 /*
 Commented out for now to allow for just quadcopter channel mapping

Safety/AttitudeManager/Inc/attitudeStateClasses.hpp

@@ -29,6 +29,14 @@ class fetchInstructionsMode : public attitudeState
         static CommandsForAM *GetPMInstructions(void) {return &_PMInstructions;}
         static PPM_Instructions_t *GetTeleopInstructions(void) {return &_TeleopInstructions;}
         static bool isAutonomous(void) {return fetchInstructionsMode::_isAutonomous;}
+        static int leftGimbalPercentage;
+        static int rightGimbalPercentage;
+        static int grabberCraneState; // -1 - reverse, 0 - off, +1 - forward
+        static int grabberMouthState; // -1 - reverse, 0 - off, +1 - forward
+        static int getGimbalGrabberState(void) {return fetchInstructionsMode::gimbalGrabberState;}
+        static int updateGimbalGrabberState();
+        static void setGimbalGrabberState(int toSet);
+
     private:
         fetchInstructionsMode() {
             //CommFromAMToPMInit(); (To be implementetd)
@@ -41,6 +49,7 @@ class fetchInstructionsMode : public attitudeState
         static PPM_Instructions_t _TeleopInstructions;
         static Instructions_t _controlsInstructions;
         static bool _isAutonomous;
+        static int gimbalGrabberState;
         static uint8_t teleopTimeoutCount;
         static uint8_t PMTimeoutCount;
 };

Safety/AttitudeManager/Src/attitudeStateClasses.cpp

@@ -1,6 +1,10 @@
 #include "attitudeStateClasses.hpp"
+#include "AttitudeDatatypes.hpp"
 #include "Controls.hpp"
+#include "PPM.hpp"
+#include "main.h"
 #include "safetyConfig.hpp"
+#include "stm32f0xx_hal_gpio.h"
 #include "RSSI.hpp"
 #include "PID.hpp"
 // #include "CommFromPMToAM.hpp"
@@ -13,6 +17,7 @@
 float OutputMixingMode::_channelOut[4];
 SFOutput_t sensorFusionMode::_SFOutput;
 PID_Output_t *PIDloopMode::_PidOutput;
+int fetchInstructionsMode::gimbalGrabberState;
 Instructions_t *_ControlsInstructions = new Instructions_t();
 CommandsForAM fetchInstructionsMode::_PMInstructions;
 PPM_Instructions_t fetchInstructionsMode::_TeleopInstructions;
@@ -22,6 +27,9 @@ uint8_t fetchInstructionsMode::PMTimeoutCount;
 uint8_t DisarmMode:: _armDisarmPPMValue;
 uint8_t DisarmMode:: armDisarmTimeoutCount;
 const uint8_t MIN_ARM_VALUE = 50; //Minimum PPM value for ARM instruction
+const uint8_t GIMBAL_THRESHOLD = 33; // Maximum PPM value for drone to be in gimbal operations state
+const uint8_t NO_GIMBAL_GRABBER_THRESHOLD = 66; // Maximum PPM value for drone to be in neither gimbal nor grabber (between 33 and 66)
+const uint8_t GRABBER_THRESHOLD = 100; // Maximum PPM value for drone to be in grabber mode (higher than 66)
 
 /***********************************************************************************************************************
  * Code
@@ -78,6 +86,9 @@ void fetchInstructionsMode::execute(attitudeManager* attitudeMgr)
         return;
     }
 
+    // determine what actuator operation state the drone is (gimbal, grabber, or off)
+    setGimbalGrabberState(updateGimbalGrabberState());
+
     // The support is also here for sending stuff to Path manager, but there's nothing I need to send atm.
     attitudeMgr->setState(sensorFusionMode::getInstance());
 }
@@ -114,6 +125,25 @@ bool fetchInstructionsMode:: isArmed()
     return retVal;
 }
 
+int fetchInstructionsMode::updateGimbalGrabberState()
+{
+    int retVal;
+    if (_TeleopInstructions.PPMValues[GIMBAL_GRABBER_TOGGLE_INDEX] <= GIMBAL_THRESHOLD) {
+        retVal = 0; // the drone will be able to operate gimbal only in this mode
+    }
+    else if (_TeleopInstructions.PPMValues[GIMBAL_GRABBER_TOGGLE_INDEX] > NO_GIMBAL_GRABBER_THRESHOLD) {
+        retVal = 2; // the drone will be able to operate grabber only in this mode 
+    }
+    else {
+        retVal = 1; // the drone won't be able to operate gimbal or grabber in this mode
+    }
+    return retVal;
+}
+
+void fetchInstructionsMode::setGimbalGrabberState(int toSet) {
+    fetchInstructionsMode::gimbalGrabberState = toSet;
+}
+
 void sensorFusionMode::execute(attitudeManager* attitudeMgr)
 {
     SFError_t _SFError = SF_GetResult(&_SFOutput);
@@ -207,13 +237,94 @@ void OutputMixingMode::execute(attitudeManager* attitudeMgr)
 
     OutputMixing_error_t ErrorStruct = OutputMixing_Execute(PidOutput, _channelOut);
 
+    PPM_Instructions_t *teleopInstructions = fetchInstructionsMode::GetTeleopInstructions();
+
     if (ErrorStruct.errorCode == 0)
     {
         // setting PWM channel values
         attitudeMgr->pwm->set(FRONT_LEFT_MOTOR_CHANNEL, PidOutput -> frontLeftMotorPercent);
         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);
+
+        if (fetchInstructionsMode::getGimbalGrabberState() == 0) 
+        {
+        // set gimbal position according to PPM values of sliders on left and right side (channel 6 and 7)
+            attitudeMgr->pwm->set(LEFT_GIMBAL, teleopInstructions->PPMValues[LEFT_GIMBAL_GRABBER_CRANE]);
+            attitudeMgr->pwm->set(RIGHT_GIMBAL,teleopInstructions->PPMValues[RIGHT_GIMBAL_GRABBER_MOUTH]);   
+        }
+
+        else if (fetchInstructionsMode::getGimbalGrabberState() == 2)
+        {
+            // set grabber position according to PPM values of switches on transmitter and speed
+            int craneState = 0;
+            int grabberState = 0;
+            if (teleopInstructions->PPMValues[LEFT_GIMBAL_GRABBER_CRANE] < 33) {
+                craneState = -1; // crane lowering
+            }
+            else if (teleopInstructions->PPMValues[LEFT_GIMBAL_GRABBER_CRANE] <= 66) {
+                craneState = 0; // crane braked
+            }
+
+            else if (teleopInstructions->PPMValues[LEFT_GIMBAL_GRABBER_CRANE] > 66) {
+                craneState = 1; // crane raising
+            }
+
+
+            if (teleopInstructions->PPMValues[RIGHT_GIMBAL_GRABBER_MOUTH] < 33) {
+                grabberState = -1; // grabber closing
+            }
+            else if (teleopInstructions->PPMValues[RIGHT_GIMBAL_GRABBER_MOUTH] <= 66) {
+                grabberState = 0; // grabber braked
+            }
+
+            else if (teleopInstructions->PPMValues[RIGHT_GIMBAL_GRABBER_MOUTH] > 66) {
+                grabberState = 1; // grabber opening
+            }
+
+            if (craneState == -1) {
+                HAL_GPIO_WritePin(GRABBER_M2A_Port, GRABBER_Pin_9_M2A, GPIO_PIN_SET);
+                HAL_GPIO_WritePin(GRABBER_M2B_Port, GRABBER_Pin_5_M2B, GPIO_PIN_RESET);
+
+                HAL_GPIO_WritePin(GRABBER_M1A_Port, GRABBER_Pin_11_M1A, GPIO_PIN_SET);
+                HAL_GPIO_WritePin(GPIOC, GRABBER_Pin_12_M1B, GPIO_PIN_RESET);
+            }
+
+            else if (craneState == 0) {
+                HAL_GPIO_WritePin(GRABBER_M2A_Port, GRABBER_Pin_9_M2A, GPIO_PIN_RESET);
+                HAL_GPIO_WritePin(GPIOC, GRABBER_Pin_5_M2B, GPIO_PIN_RESET);
+
+                HAL_GPIO_WritePin(GRABBER_M1A_Port, GRABBER_Pin_11_M1A, GPIO_PIN_RESET);
+                HAL_GPIO_WritePin(GPIOC, GRABBER_Pin_12_M1B, GPIO_PIN_RESET);
+            }
+
+            else {
+                HAL_GPIO_WritePin(GRABBER_M2A_Port, GRABBER_Pin_9_M2A, GPIO_PIN_RESET);
+                HAL_GPIO_WritePin(GRABBER_M2B_Port, GRABBER_Pin_5_M2B, GPIO_PIN_SET);
+
+                HAL_GPIO_WritePin(GRABBER_M1A_Port, GRABBER_Pin_11_M1A, GPIO_PIN_RESET);
+                HAL_GPIO_WritePin(GPIOC, GRABBER_Pin_12_M1B, GPIO_PIN_SET);
+            }
+
+            if (grabberState == -1) {
+
+                HAL_GPIO_WritePin(CRANE_M1A_Port, CRANE_Pin_7_M1A, GPIO_PIN_SET);
+                HAL_GPIO_WritePin(CRANE_M1B_Port, CRANE_Pin_8_M1B, GPIO_PIN_RESET);
+            }
+
+            else if (grabberState == 0) {
+
+                HAL_GPIO_WritePin(CRANE_M1A_Port, CRANE_Pin_7_M1A, GPIO_PIN_RESET);
+                HAL_GPIO_WritePin(CRANE_M1B_Port, CRANE_Pin_8_M1B, GPIO_PIN_RESET);
+            }
+
+            else {
+                HAL_GPIO_WritePin(CRANE_M1A_Port, CRANE_Pin_7_M1A, GPIO_PIN_RESET);
+                HAL_GPIO_WritePin(CRANE_M1B_Port, CRANE_Pin_8_M1B, GPIO_PIN_SET);
+            }
+
+        }
+
         attitudeMgr->setState(fetchInstructionsMode::getInstance()); // returning to beginning of state machine
     }
     else

Safety/Inc/PPM.hpp

@@ -22,6 +22,9 @@
 static const int32_t MAX_PPM_CHANNELS = 12;
 
 static const int32_t ARM_DISARM_CHANNEL_INDEX = 5;
+static const uint8_t GIMBAL_GRABBER_TOGGLE_INDEX = 4; // three-way switch for gimbal, grabber, or none
+static const uint8_t LEFT_GIMBAL_GRABBER_CRANE = 6;
+static const uint8_t RIGHT_GIMBAL_GRABBER_MOUTH = 7;
 
 enum StatusCode{STATUS_CODE_OK, STATUS_CODE_FAILED, STATUS_CODE_INVALID_ARGS};
 

Safety/Src/PWM.cpp

@@ -30,8 +30,8 @@ static const PWMPinConfig PWM_CONFIG[MAX_CHANNELS]
     {PWM4_Pin, PWM4_GPIO_Port, &htim1, TIM_CHANNEL_4, true, DMAMotorBuffer4, TIM_DMA_ID_CC4, TIM_DMA_CC4}, 
     {PWM6_Pin, PWM6_GPIO_Port, &htim3, TIM_CHANNEL_1, false, {0}, 0, 0},
     {PWM10_Pin, PWM10_GPIO_Port, &htim3, TIM_CHANNEL_2, false, {0}, 0, 0},
-    {PWM11_Pin, PWM11_GPIO_Port, &htim3, TIM_CHANNEL_3, false, {0}, 0, 0},
-    {PWM12_Pin, PWM12_GPIO_Port, &htim3, TIM_CHANNEL_4, false, {0}, 0, 0}
+    // {PWM11_Pin, PWM11_GPIO_Port, &htim3, TIM_CHANNEL_3, false, {0}, 0, 0},
+    // {PWM12_Pin, PWM12_GPIO_Port, &htim3, TIM_CHANNEL_4, false, {0}, 0, 0}
 };
 
 PWMChannel::PWMChannel()

Safety/boardfiles/Inc/main.h

@@ -70,6 +70,7 @@ void Error_Handler(void);
 #define RSSI_GPIO_Port GPIOB
 #define PWM6_Pin GPIO_PIN_6
 #define PWM6_GPIO_Port GPIOC
+
 #define PWM10_Pin GPIO_PIN_7
 #define PWM10_GPIO_Port GPIOC
 #define PWM11_Pin GPIO_PIN_8
@@ -90,6 +91,22 @@ void Error_Handler(void);
 #define LED2_GPIO_Port GPIOC
 #define LED3_Pin GPIO_PIN_12
 #define LED3_GPIO_Port GPIOC
+
+#define GRABBER_Pin_9_M2A GPIO_PIN_11
+#define GRABBER_M2A_Port GPIOB
+#define GRABBER_Pin_5_M2B GPIO_PIN_10
+#define GRABBER_M2B_Port GPIOB
+
+#define GRABBER_Pin_11_M1A GPIO_PIN_2
+#define GRABBER_M1A_Port GPIOC
+#define GRABBER_Pin_12_M1B GPIO_PIN_3
+#define GRABBER_M1B_Port GPIOC
+
+#define CRANE_Pin_7_M1A GPIO_PIN_15
+#define CRANE_M1A_Port GPIOA
+#define CRANE_Pin_8_M1B GPIO_PIN_3
+#define CRANE_M1B_Port GPIOB
+
 #define DBG_UART_TX_Pin GPIO_PIN_6
 #define DBG_UART_TX_GPIO_Port GPIOB
 #define DBG_UART_RX_Pin GPIO_PIN_7