Diff: Servo/servo_bewegung.cpp

Revision:

26:caa4fab7023e

Parent:

22:f71d32309307

--- a/Servo/servo_bewegung.cpp	Tue Apr 27 20:04:51 2021 +0000
+++ b/Servo/servo_bewegung.cpp	Wed Apr 28 14:58:37 2021 +0000
@@ -3,37 +3,104 @@
 
 #include "servo_bewegung.h"
 
-#define MID         1500
-#define MIN         1000
-#define MAX         2000
-#define MIT          800
+#define RIGHT         1200
+#define MID           1000
+#define MAX           2000
+#define LEFT          800
 #define STEP          25
 //Time delay between steps in milliseconds
-#define TIME         100
+#define TIME          100
 
 
-void bewegung(){
-    Servo servo1(PA_0);
-    servo1.Enable(MIN,MAX);
+void bewegung(int servo_id){
+    switch(servo_id) {
+    case 1:{
+        
+        Servo servo1(PA_0/*PB_2*/);
+        servo1.Enable(MID,MAX);
     
-    //Servo Bewegung
-    for (int pos = MIN; pos > MIT; pos -= STEP) {
-        servo1.SetPosition(pos);
-        thread_sleep_for(TIME);
-    }
-    for(int j=0; j<2; j++){
-        for (int pos = MIT; pos < MID; pos += STEP) {
+        //Servo Bewegung
+        for (int pos = MID; pos > LEFT; pos -= STEP) {
+            servo1.SetPosition(pos);
+            thread_sleep_for(TIME);
+        }
+        for(int j=0; j<2; j++){
+            for (int pos = LEFT; pos < RIGHT; pos += STEP) {
+                servo1.SetPosition(pos);
+                thread_sleep_for(TIME);
+            }
+            for (int pos = RIGHT; pos > LEFT; pos -= STEP) {
+                servo1.SetPosition(pos);
+                thread_sleep_for(TIME);
+            }
+        }
+        for (int pos = LEFT; pos < MID; pos += STEP) {
             servo1.SetPosition(pos);
             thread_sleep_for(TIME);
         }
-        for (int pos = MID; pos > MIT; pos -= STEP) {
-            servo1.SetPosition(pos);
+        thread_sleep_for(TIME);
+        servo1.Disable();
+        break;
+        }
+    case 2:{
+        
+        Servo servo2(PA_1/*PC_8*/);
+        servo2.Enable(MID,MAX);
+        
+        //Servo Bewegung
+        for (int pos = MID; pos > LEFT; pos -= STEP) {
+            servo2.SetPosition(pos);
+            thread_sleep_for(TIME);
+        }
+        for(int j=0; j<2; j++){
+            for (int pos = LEFT; pos < RIGHT; pos += STEP) {
+                servo2.SetPosition(pos);
+                thread_sleep_for(TIME);
+            }
+            for (int pos = RIGHT; pos > LEFT; pos -= STEP) {
+                servo2.SetPosition(pos);
+                thread_sleep_for(TIME);
+            }
+        }
+        for (int pos = LEFT; pos < MID; pos += STEP) {
+            servo2.SetPosition(pos);
             thread_sleep_for(TIME);
         }
+        thread_sleep_for(TIME);
+        servo2.Disable();
+        break;
+        }
+    case 3:{
+        
+        Servo servo3(PA_4/*PC_6*/);
+        servo3.Enable(MID,MAX);
+
+        //Servo Bewegung
+        for (int pos = MID; pos > LEFT; pos -= STEP) {
+            servo3.SetPosition(pos);
+            thread_sleep_for(TIME);
+        }
+        for(int j=0; j<2; j++){
+            for (int pos = LEFT; pos < RIGHT; pos += STEP) {
+                servo3.SetPosition(pos);
+                thread_sleep_for(TIME);
+            }
+            for (int pos = RIGHT; pos > LEFT; pos -= STEP) {
+                servo3.SetPosition(pos);
+                thread_sleep_for(TIME);
+            }
+        }
+        for (int pos = LEFT; pos < MID; pos += STEP) {
+            servo3.SetPosition(pos);
+            thread_sleep_for(TIME);
+        }
+        thread_sleep_for(TIME);
+        servo3.Disable();
+        break;
+        }
+    default:{
+        printf("servo not found");
+        break;
+        }
     }
-    for (int pos = MIT; pos < MIN; pos += STEP) {
-        servo1.SetPosition(pos);
-        thread_sleep_for(TIME);
-    }
-    servo1.Disable();
 }