Diff: main.cpp

Revision:

12:515bacd56d07

Parent:

11:bb036c288cd7

Child:

13:1a014f5b01df

--- a/main.cpp	Tue Oct 29 09:50:16 2019 +0000
+++ b/main.cpp	Tue Oct 29 11:00:26 2019 +0000
@@ -2,39 +2,63 @@
 #include "Servo.h"
 #include "mbed.h"
 #include "FastPWM.h"
-
+#define M_PI 3.14159265358979323846  /* pi */
+#include <math.h>
 Serial pc(USBTX, USBRX);
+Ticker servo_ticker;
 Servo mijnServo(D7);
-DigitalIn servo_button_pressed(D1);
+DigitalIn servo_button_pressed(SW3);
+//InterruptIn button(SW3);
 //int Position= mijnServo.read();
+//bool servo_button_pressed;
 
+//void flag(void){
+ //   servo_button_pressed=!servo_button_pressed;
+//}
 
 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 theta_s= 500;       // dit zijn allemaal waarden tussen de 500-2400
+    double theta_sout;          // dit zijn allemaal waarden tussen de 500-2400
+    double q1=0; //     0.2*M_PI;               // dit is in rad uit I.K
+    double q2=0;      //0.5*M_PI;               // 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) 
+    if  (servo_button_pressed.read()== false) 
         { 
             theta_sout=theta_s-q1_ser+q2_ser+400;
+            if (theta_sout>=2400)
+            {   theta_sout=2400;}
+            if (theta_sout<=500)
+            {   theta_sout=500;}
+            else 
+            {   theta_sout;
+            }
             mijnServo.SetPosition(theta_sout);
-            pc.printf("De servo staat op %f graden.\n\r", theta_sout);
+            pc.printf("De servo staat op %f graden. in de klapstand\n\r", theta_sout);
         }
     else 
         {
             theta_sout=theta_s-q1_ser+q2_ser;
+             if (theta_sout>=2400)
+            {   theta_sout=2400;}
+            if (theta_sout<=500)
+            {   theta_sout=500;}
+            else {   theta_sout;
+            }
             mijnServo.SetPosition(theta_sout);
             pc.printf("De servo staat op %f graden.\n\r", theta_sout);
         }
 }
 int main(void)
-    {   servo();
-        mijnServo.Enable(1500,20000);
+    {   
+    mijnServo.Enable(1500,20000);
+    servo();
+    servo_ticker.attach(servo,0.5);
+    //button.fall(flag);
+    while(1){}
     }
\ No newline at end of file