Diff: servo.cpp

Revision:

1:be03cac9fe74

Child:

2:7dc265489818

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo.cpp	Mon Jun 08 21:57:59 2020 +0000
@@ -0,0 +1,63 @@
+#include "servo.h"
+Servo myServo;
+
+Servo::Servo(unsigned int uiServoFrequency) {
+    LedInit();
+    sServo.eState = CALLIB;
+    Timer0Interrupts_Init((1000000/uiServoFrequency), &ServoAutomat);
+    while(sServo.eState != IDLE) {}
+}
+
+void ServoGoTo(unsigned int uiPosition) {
+    sServo.uiDesiredPosition = uiPosition;
+    sServo.eState = IN_PROGRESS;
+    while(sServo.eState != IDLE) {}
+}
+
+void ServoCallib() {
+    sServo.eState = CALLIB;
+    while(sServo.eState != IDLE) {}
+}
+
+void Servo::Automat() {
+    switch(myServo::eState) {
+        case IN_PROGRESS:
+            if(uiCurrentPosition < uiDesiredPosition) {
+                LedStepRight();
+                uiCurrentPosition++;
+            }
+            else if(uiCurrentPosition > uiDesiredPosition){
+                LedStepLeft();
+                uiCurrentPosition--;
+            }
+            else {
+                eState = IDLE;
+            }
+        break;
+        case IDLE:
+            if (uiCurrentPosition != uiDesiredPosition) {
+                eState = IN_PROGRESS;
+            }
+            else {}
+        break;
+        case CALLIB:
+            if(eReadDetector() == ACTIVE) {
+                eState = IDLE;
+                uiCurrentPosition = 0;
+                uiDesiredPosition = 0;
+            }
+            else {
+                LedStepLeft();
+            }
+        break;
+    }
+}
+
+enum DetectorState eReadDetector() {
+    if ((IO0PIN & DETECTOR_bm) == 0) {
+        return ACTIVE;
+    }
+    else {
+        return INACTIVE;
+    }
+}