Diff: main.cpp

Revision:

11:bb036c288cd7

Parent:

10:1754b6220c7a

Child:

12:515bacd56d07

diff -r 1754b6220c7a -r bb036c288cd7 main.cpp
--- a/main.cpp	Mon Oct 28 17:18:32 2019 +0000
+++ b/main.cpp	Tue Oct 29 09:50:16 2019 +0000
@@ -5,25 +5,36 @@
 
 Serial pc(USBTX, USBRX);
 Servo mijnServo(D7);
-double pos = 0;
-double target;
+DigitalIn servo_button_pressed(D1);
+//int Position= mijnServo.read();
+
+
+void servo()
+{
+    double theta_s= 1450;       // dit zijn allemaal waarden tussen de 500-2400
+   double theta_sout;          // dit zijn allemaal waarden tussen de 500-2400
+    double q1;                  // dit is in rad uit I.K
+    double q2;                  // dit is in rad uit I.K
+    double q1_ser;              // dit is 500-2400
+    double q2_ser;              // dit is 500-2400
+
+    q1_ser= q1*604.7887837;
+    q2_ser= q2*604.7887837;
+
+    if  (servo_button_pressed.read() == false) 
+        { 
+            theta_sout=theta_s-q1_ser+q2_ser+400;
+            mijnServo.SetPosition(theta_sout);
+            pc.printf("De servo staat op %f graden.\n\r", theta_sout);
+        }
+    else 
+        {
+            theta_sout=theta_s-q1_ser+q2_ser;
+            mijnServo.SetPosition(theta_sout);
+            pc.printf("De servo staat op %f graden.\n\r", theta_sout);
+        }
+}
 int main(void)
-{mijnServo.Enable(1500,20000);
-    wait(1);
-    for(pos = 2400; pos >=500; pos-=50)
-    {
-        target = pos/180;
-        mijnServo.SetPosition(pos);
-        pc.printf("De servo staat op %f graden.\n\r", pos);
-        wait(0.1);
-    }
-    for(pos = 500; pos <= 2400; pos += 50)
-    {
-        target = pos/180;
-        mijnServo.SetPosition(pos);
-        pc.printf("De servo staat op %f graden.\n\r", pos);
-        wait(0.1);
-    } 
-    while(true);
-   
-}
\ No newline at end of file
+    {   servo();
+        mijnServo.Enable(1500,20000);
+    }
\ No newline at end of file