Revision 6:e6125ef7705e, committed 2019-10-21

Comitter:

s1923196

Date:

Mon Oct 21 15:13:14 2019 +0000

Parent:

5:74962b191242

Child:

7:464fb83c8cdf

Commit message:

servo met hoeken

Changed in this revision

--- a/main.cpp	Fri Oct 11 11:06:43 2019 +0000
+++ b/main.cpp	Mon Oct 21 15:13:14 2019 +0000
@@ -18,4 +18,62 @@
      t+=0.02;                       // nooit groter dan periode servomotor, zelfde geldt voor regel hieronder
      wait(0.02); 
     }  
-}
\ No newline at end of file
+}
+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
+        }
+
+
+
+
+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;