Diff: main.cpp

Revision:

4:babe09a69296

Parent:

3:2aaf54ce090b

Child:

5:ef77da99d0d1

--- a/main.cpp	Thu Apr 04 15:37:37 2019 +0000
+++ b/main.cpp	Fri Apr 05 12:28:22 2019 +0000
@@ -4,6 +4,7 @@
 #include "QEI.h"
 #include "BiQuad.h"
 #include "FastPWM.h"
+
 // Algemeen
 DigitalIn button3(SW3);  
 DigitalIn button2(SW2); 
@@ -19,8 +20,10 @@
 Timer t2;
 //Motoren
 DigitalOut direction1(D4);
-FastPWM pwmpin1(D5);
+FastPWM pwmpin1(D5);              
 FastPWM pwmpin2(D6);
+//PwmOut pwmpin1(D5);
+//PwmOut pwmpin2(D6);
 DigitalOut direction2(D7);
 volatile float pwm1;
 volatile float pwm2;
@@ -46,6 +49,13 @@
 Ticker PotRead;
 Ticker Kdc;
 
+//Servo
+FastPWM myservo1(D3);
+Ticker ServoTick;
+float servo_position;
+float Periodlength = 0.00006;
+int counts = 0;
+
 // EMG
 float EMG1;       // Rotatie
 float EMG2;       // Elleboog
@@ -272,6 +282,24 @@
 
 }
 
+//Servo functie
+void ServoPeriod()
+{
+  led1 = 1;
+  led2 = 1;
+  double Pulslength = 0.0005 + (servo_position * 0.002); //in seconden
+  if (counts <= (Pulslength/Periodlength)) {
+        myservo1.pulsewidth(0.00006);
+        counts++;
+    }
+  else if (counts <= (0.02/Periodlength)){
+        myservo1.pulsewidth(0);
+        counts++;
+    }
+    else {
+        counts = 0;
+    }
+}
 
 void ContinuousReader(void)
 {
@@ -324,12 +352,21 @@
       if (stateChanged)
       {
         // state initialization: oranje
-        led1 = 0;
-        led2 = 0;
-        led3 = 1;
-        wait (1);
-        
-        stateChanged = false;
+                led1 = 0;
+                led2 = 0;
+                led3 = 1;
+                
+                servo_position = 0.5;
+                ServoTick.attach(ServoPeriod, Periodlength);
+                wait(1);
+                servo_position = 0.9;
+                led2 = 1;
+                wait(1);
+                servo_position = 0.5;
+                wait(1);
+                servo_position = 0.1;
+                wait (1);
+                stateChanged = false;
       }
       
       // State transition logic: automatisch terug naar motors off.
@@ -557,10 +594,11 @@
     
     t2.start();
     int counter = 0;
-    pwmpin2.period_us(60);
-    PotRead.attach(ContinuousReader,Ts);
+    myservo1.period_us(60);
+    //PotRead.attach(ContinuousReader,Ts);
     pc.baud(115200);
     
+    
     while(true)
     {
     led1 = 1;