Diff: main.cpp

Revision:

8:3990a8c4ccea

Parent:

7:464fb83c8cdf

Child:

10:1754b6220c7a

--- a/main.cpp	Fri Oct 25 14:32:41 2019 +0000
+++ b/main.cpp	Mon Oct 28 16:50:36 2019 +0000
@@ -2,79 +2,30 @@
 //#include "Servo.h"
 //#include "FastPWM.h"
 #include <math.h>
+#include "Servo.h"
 
 Serial pc(USBTX, USBRX);
-PwmOut myservo(D5);
-DigitalIn servo_button_pressed(D0);
+//PwmOut myservo(D5);
+//DigitalIn servo_button_pressed(D0);
 
+Servo servo1 (D7); // kan nog aangepast worden de pin
+//servo1.Enable(1500,20000);
+int pos(); 
 int main()
 {
-  myservo.period(0.02f);
-  float t = 0;
-  while (true)
-    {
-     float ref = sin(t);
-     float pwm = 0.05 + 0.03 * ref;  // maximale bereik: allebei 0.05
-     myservo.write(pwm);
-     t+=0.02;                       // nooit groter dan periode servomotor, zelfde geldt voor regel hieronder
-     wait(0.02); 
-    }  
-}
-float theta_s_out;
 
 
-void servo_horizontal()
-theta_s=theta_2_h+f;         //f is de beginwaarde en theta_2_h is de waarde van INVERSE KINEM
-        if(0<=theta_s<=60)
-        {
-            theta_s_out=0.3  //f krijgt een vaste waarde tussen de range van 0-1. Dit getal is de rangewaarde voor f   
-        }    
-        if (60<theta_s<180)
-        {
-            theta_s_out= theta_s*0.0055555556
-        }
-        if (theta_s=>180)
-        {
-            theta_s_out=1
-        }
-            
-  
-void servo_flippen()           //voor het flippen is een waarde 0.2 gekozen dit kan veranderd worden 
-theta_s=theta_2_h+f;         //f is de beginwaarde en theta_2_h is de waarde van INVERSE KINEM
-        if(0<=theta_s<=60)
-        {
-            theta_s_out=0.3-0.2  //f krijgt een vaste waarde tussen de range van 0-1. Dit getal is de rangewaarde voor f   
-        }    
-        if (60<theta_s<180)
-        {
-            theta_s_out= (theta_s*0.0055555556)-0.2
-            
-        }
-        if (theta_s=>180)
-        {
-            theta_s_out=1-0.2
-        }
+while(1) {
+     for (int pos = 1000; pos < 2000; pos += 25) {
+         servo1.SetPosition(pos);  
+         wait_ms(20);
+     }
+     for (int pos = 2000; pos > 1000; pos -= 25) {
+         servo1.SetPosition(pos); 
+         wait_ms(20); 
+     }
+ }
+}
 
 
-
-
-case controlling_position:
-            if (stateChanged) 
-                {
-                 servo_horizontal();
-                 // functie waarbij de hoek gelijk blijft
-                stateChanged = false;
-                pc.printf("Servo hoek gelijk\r\n");
-                }          
-            if  (servo_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
-                {      
-                servo_flippen();
-               //currentState = flipping_spatula;
-                //stateChanged = true;
-                pc.printf("Moving to flipping spatula\r\n");
-                }
-            if  (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
-                { 
-                emergency();
-                } 
-            break;
+    
\ No newline at end of file