From e78b7eb18bbbfed8b67ada005d2c85d993f093d9 Mon Sep 17 00:00:00 2001
From: Arnaud Cadot <arnaud.cadot@telecom-bretagne.eu>
Date: Thu, 24 Mar 2016 21:48:19 +0100
Subject: [PATCH] Added ramp controls to Benne class

---
 stm32/include/actionneurs/benne.h | 22 +++++++++++++++++++---
 stm32/src/actionneurs/benne.cpp   | 31 +++++++++++++++++++++++++++----
 2 files changed, 46 insertions(+), 7 deletions(-)

diff --git a/stm32/include/actionneurs/benne.h b/stm32/include/actionneurs/benne.h
index b12c6e8c..bed305e7 100644
--- a/stm32/include/actionneurs/benne.h
+++ b/stm32/include/actionneurs/benne.h
@@ -8,11 +8,23 @@ class Benne
     /**
       * The ID of the AX12 used to drive the belts
     */
-    static const int SERVO_ID = 0; // To update
+    static const int BELTS_SERVO_ID = 0; // To update
 
     static const int FORWARD_SPEED  = 1023;
     static const int BACKWARD_SPEED = 2046;
 
+    /**
+      * The ID of the AX12 used to deploy/retract the front ramp
+    */
+    static const int RAMP_LEFT_SERVO_ID     = 0; // To update
+    static const int RAMP_RIGHT_SERVO_ID    = 0; // To update
+
+    static const int RAMP_LEFT_DEPLOYED_ANGLE    = 0;
+    static const int RAMP_LEFT_RETRACTED_ANGLE   = 1023;
+
+    static const int RAMP_RIGHT_DEPLOYED_ANGLE   = 1023;
+    static const int RAMP_RIGHT_RETRACTED_ANGLE  = 0;
+
     public:
         struct Status
         {
@@ -43,11 +55,15 @@ class Benne
 
         void stop();
 
-        bool isFrontSwitchActive() const;
-        bool isBackSwitchActive() const;
+        void deployRamp();
+        void retractRamp();
 
         Benne::Status::Enum getStatus() const;
 
+    protected:
+        bool isFrontSwitchActive() const;
+        bool isBackSwitchActive() const;
+
     private:
         Benne();
 
diff --git a/stm32/src/actionneurs/benne.cpp b/stm32/src/actionneurs/benne.cpp
index f9831299..943a086b 100644
--- a/stm32/src/actionneurs/benne.cpp
+++ b/stm32/src/actionneurs/benne.cpp
@@ -31,7 +31,10 @@ Benne::Benne()
     m_status = Status::UNKNOWN;
 
     #ifdef ROBOTHW
-        ServosNumeriques::changeContinuousRotationMode(SERVO_ID,  true);
+        ServosNumeriques::changeContinuousRotationMode(BELTS_SERVO_ID, true);
+
+        ServosNumeriques::changeContinuousRotationMode(RAMP_LEFT_SERVO_ID,  false);
+        ServosNumeriques::changeContinuousRotationMode(RAMP_RIGHT_SERVO_ID, false);
     #endif
 }
 
@@ -74,7 +77,7 @@ void Benne::empty()
     if(getStatus() != Status::CLOSED && getStatus() != Status::CLOSING)
     {
         #ifdef ROBOTHW
-            ServosNumeriques::moveAtSpeed(FORWARD_SPEED, SERVO_ID);
+            ServosNumeriques::moveAtSpeed(FORWARD_SPEED, BELTS_SERVO_ID);
         #else
             qDebug() << "Bin is closing";
         #endif
@@ -87,7 +90,7 @@ void Benne::open()
     if(getStatus() != Status::OPEN && getStatus() != Status::OPENING)
     {
         #ifdef ROBOTHW
-            ServosNumeriques::moveAtSpeed(BACKWARD_SPEED, SERVO_ID);
+            ServosNumeriques::moveAtSpeed(BACKWARD_SPEED, BELTS_SERVO_ID);
         #else
             qDebug() << "Bin is opening";
         #endif
@@ -95,6 +98,26 @@ void Benne::open()
     }
 }
 
+void Benne::deployRamp()
+{
+    #ifdef ROBOTHW
+        ServosNumeriques::moveTo(RAMP_LEFT_DEPLOYED_ANGLE,  RAMP_LEFT_SERVO_ID);
+        ServosNumeriques::moveTo(RAMP_RIGHT_DEPLOYED_ANGLE, RAMP_RIGHT_SERVO_ID);
+    #else
+        qDebug() << "Deploying ramp";
+    #endif
+}
+
+void Benne::retractRamp()
+{
+    #ifdef ROBOTHW
+        ServosNumeriques::moveTo(RAMP_LEFT_RETRACTED_ANGLE,  RAMP_LEFT_SERVO_ID);
+        ServosNumeriques::moveTo(RAMP_RIGHT_RETRACTED_ANGLE, RAMP_RIGHT_SERVO_ID);
+    #else
+        qDebug() << "Retracting ramp";
+    #endif
+}
+
 void Benne::update()
 {
     if(getStatus() == Status::CLOSING && isFrontSwitchActive())
@@ -121,7 +144,7 @@ void Benne::update()
 void Benne::stop()
 {
     #ifdef ROBOTHW
-        ServosNumeriques::moveAtSpeed(0, SERVO_ID);
+        ServosNumeriques::moveAtSpeed(0, BELTS_SERVO_ID);
     #endif
 }
 
-- 
GitLab