Diff: Gobeur.cpp

Revision:

3:44a200543540

Child:

5:64a88f154e65

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Gobeur.cpp	Thu Jul 08 14:09:26 2021 +0000
@@ -0,0 +1,71 @@
+#include "Gobeur.h"
+
+
+Gobeur::Gobeur(PinName pin_turbine, PinName pin_servo, float turbine_on, float turbine_off, float servo_open, float servo_close): 
+    _order(release), _turbine_on(turbine_on), _turbine_off(turbine_off), _servo_open(servo_open), _servo_close(servo_close)
+{
+    _turbine = new Servo(pin_turbine);
+    _servo = new Servo(pin_servo);
+    *_turbine = _turbine_off;
+    *_servo = _servo_close;
+    
+}
+
+void Gobeur::update_order(int order)
+{
+    if(order == 0) _order = suck;
+    if(order == 1) _order = release;
+}
+void Gobeur::automate(void)
+{
+    switch(_state)
+    {
+        case released:
+            if( (_order == suck ) || ((clock_s() - _time) > 2*GOBEUR_PAUSE_TIMEOUT)&&1 )
+            {
+                 _state = sucking;
+                 //                                                     *_servo = GOBEUR_SERVO_POS_OPEN;
+                 *_turbine = _turbine_on;
+                 _time = clock_s();
+            }
+            break;
+        case sucking:
+            if(  (clock_s() - _time) > GOBEUR_SUCKING_TIMEOUT) //TODO add switch
+            {
+                _state = closing;
+                *_servo = _servo_close;
+                //                                                      *_turbine = GOBEUR_TURBINE_TH_ON;
+                _time = clock_s();
+            }
+            break;
+        case closing:
+            if( (clock_s() - _time) > GOBEUR_CLOSING_TIMEOUT) //TODO add switch
+            {
+                _state = sucked;
+                //                                                      *_servo = GOBEUR_SERVO_POS_CLOSE;
+                *_turbine = _turbine_off;
+                _time = clock_s();
+                // TODO send ack
+            }
+            break;
+        case sucked:
+            if( (_order == release ) || ((clock_s() - _time) > GOBEUR_PAUSE_TIMEOUT)&&1 )
+            {
+                 _state = releasing;
+                 *_servo = _servo_open;
+                 //                                                       *_turbine = GOBEUR_TURBINE_TH_OFF;
+                 _time = clock_s();
+            }
+            break;
+        case releasing:
+            if((clock_s() - _time) > GOBEUR_OPENING_TIMEOUT) //TODO add switch
+            {
+                _state = released;
+                //                                                         *_servo = GOBEUR_SERVO_POS_OPEN;
+                //                                                         *_turbine = GOBEUR_TURBINE_TH_OFF;
+                _time = clock_s();
+                // TODO send ack
+            }
+            break;
+    }
+}
\ No newline at end of file