From 849721bdd33707c46663e3c8d7179d1b54d66bcb Mon Sep 17 00:00:00 2001
From: Aidan Bowers <abowers@uwaterloo.ca>
Date: Mon, 25 Apr 2022 12:04:27 -0400
Subject: [PATCH] Update grabber controls & add use output for purchased driver
 board

---
 .../Src/attitudeStateClasses.cpp              | 23 ++++++++++--
 Safety/Inc/grabber.hpp                        |  6 +--
 Safety/Src/grabber.cpp                        | 37 ++++++++-----------
 3 files changed, 37 insertions(+), 29 deletions(-)

diff --git a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp
index d525a446..e50f3a79 100644
--- a/Safety/AttitudeManager/Src/attitudeStateClasses.cpp
+++ b/Safety/AttitudeManager/Src/attitudeStateClasses.cpp
@@ -322,11 +322,26 @@ bool DisarmMode:: isArmed()
 
 void PeripheralControlMode::execute(attitudeManager* attitudeMgr)
 {
+    const uint8_t speed = 10;
     PPM_Instructions_t *teleopInstructions = fetchInstructionsMode::GetTeleopInstructions();
-    if (teleopInstructions->PPMValues[attitudeMgr->grabber->ppmChannel] > 50)
-        attitudeMgr->grabber->close(50);
-    else 
-        attitudeMgr->grabber->open(50);
+    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());
 }
 
diff --git a/Safety/Inc/grabber.hpp b/Safety/Inc/grabber.hpp
index 264d7a34..dfea5deb 100644
--- a/Safety/Inc/grabber.hpp
+++ b/Safety/Inc/grabber.hpp
@@ -22,6 +22,7 @@ class Grabber {
                               PWMChannel *pwm);
   Grabber(const Grabber*)=delete; 
   
+  void brake();
   void open(uint8_t speed);
   void close(uint8_t speed);
 
@@ -30,7 +31,8 @@ class Grabber {
   void lower(uint8_t speed);
   void raise(uint8_t speed);
 
-  const uint8_t ppmChannel = 5; // TODO: What channel?
+  const uint8_t ppmCraneChannel = 6; // TODO: What channel?
+  const uint8_t ppmClawChannel = 7; // TODO: What channel?
 
 private:
   Grabber() {};
@@ -45,8 +47,6 @@ class Grabber {
     crane_channel_2(grabber_pwm_channel_2),
     pwm(*pwm) {}
   
-  bool isOpen = false;
-  bool isLowered = false;
   const uint8_t grabber_channel_1 = 0;
   const uint8_t grabber_channel_2 = 0;
   const uint8_t crane_channel_1 = 0;
diff --git a/Safety/Src/grabber.cpp b/Safety/Src/grabber.cpp
index be7da5c9..b3556312 100644
--- a/Safety/Src/grabber.cpp
+++ b/Safety/Src/grabber.cpp
@@ -47,37 +47,30 @@ void Grabber::crane_coast() {
 // 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) {
-	if (!this->isLowered) {
-		pwm.set(this->crane_channel_1, speed);
-		pwm.set(this->crane_channel_2, 0);
-		this->isLowered = true;
-	}
+	pwm.set(this->crane_channel_1, speed);
+	pwm.set(this->crane_channel_2, 0);
 }
 void Grabber::raise(uint8_t speed) {
-	if (this->isLowered) {
-		pwm.set(this->crane_channel_1, 0);
-		pwm.set(this->crane_channel_2, speed);
-		this->isLowered = false;
-	}
+	pwm.set(this->crane_channel_1, 0);
+	pwm.set(this->crane_channel_2, speed);
 }
 
 
-//TODO: Still need data from electrical on the input to the motor driver, 
-//		hopefully it is similar to the above
+
+// 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) {
-	if (!this->isOpen) {
-		pwm.set(this->grabber_channel_1, speed);
-		pwm.set(this->grabber_channel_2, 0);
-		this->isOpen = true;
-	}
+	pwm.set(this->grabber_channel_1, speed);
+	pwm.set(this->grabber_channel_2, 0);
 }
 void Grabber::open(uint8_t speed) {
-	if (this->isOpen) {
-		pwm.set(this->grabber_channel_1, 0);
-		pwm.set(this->grabber_channel_2, speed);
-		this->isOpen = false;
-	}
+	pwm.set(this->grabber_channel_1, 0);
+	pwm.set(this->grabber_channel_2, speed);
 }
 
 /* Private methods  ---------------------------------------------------------*/