Diff: servo.cpp

Revision:

0:33ff53112cc9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo.cpp	Tue Jun 09 08:24:40 2020 +0000
@@ -0,0 +1,67 @@
+#include "servo.h"
+
+InterruptIn user_button(USER_BUTTON);
+
+void Servo::Automat(){
+
+    switch(eState){
+            
+        case CALLIB:
+            if(user_button == 0){
+                myLed.StepLeft(); 
+            }
+            else{  
+                uiCurrentPosition = 0;
+                uiDesiredPosition = 0;
+                eState = IDLE;
+            }
+            break;
+            
+        case IDLE:
+
+            if(uiCurrentPosition != uiDesiredPosition){
+                eState = IN_PROGRESS; 
+            }
+            break;
+            
+        case IN_PROGRESS:
+            if(uiCurrentPosition < uiDesiredPosition){
+                uiCurrentPosition++;
+                myLed.StepRight();
+            }
+            else if(uiCurrentPosition > uiDesiredPosition){
+                uiCurrentPosition--;
+                myLed.StepLeft();
+            }
+            else{
+                eState = IDLE;
+            }
+            break;
+    }
+}
+
+void Servo::Callib(void){
+    
+    eState = CALLIB;
+    while(eState != IDLE){ 
+        wait_ms(1);
+    }
+}
+
+void Servo::GoTo(unsigned int uiPosition){
+    
+    uiDesiredPosition = uiPosition;
+    eState = IN_PROGRESS;
+    while(eState != IDLE){ 
+        wait_ms(1);
+    }
+}
+
+void Servo::StepRight(unsigned int uiSteps){
+    
+    uiDesiredPosition += uiSteps;
+    eState = IN_PROGRESS;
+    while(eState != IDLE){
+        wait_ms(1);
+    }
+}