Diff: main.cpp

Revision:

16:bf4c5affd3e9

Parent:

15:3a6074f3ceaf

Child:

17:0116e2f17d0d

--- a/main.cpp	Wed Oct 08 13:40:40 2014 +0000
+++ b/main.cpp	Fri Oct 10 07:56:24 2014 +0000
@@ -8,7 +8,7 @@
 #define K_D (0  /TSAMP)
 #define I_LIMIT 1.
 #define POT_AVG 50
-#define WACHT 1
+#define WACHTEN 1
 #define SLAAN 2
 #define TERUGKEREN 3
 
@@ -19,7 +19,7 @@
 AnalogIn potmeter(PTC2);
 volatile bool looptimerflag;
 float potsamples[POT_AVG];
-
+int EMG(float biceps, float triceps) // misschien niet float omdat input functies zijn
 
 void setlooptimerflag(void)
 {
@@ -28,13 +28,16 @@
 
 int main()
 {
-    int toestand = TERUGKEREN;
-    //----------------------------------------------------------------dit is de initiële toestand zodat hij begint met hoek 0--------------------------
+    //----------------------------------------------------------------dit is de initiële toestand zodat hij begint met hoek 0, moet misschien nog een wait achter
+    //-----------------------------------------------------en de snelheid van toestand 1,2,3 moeten bepaald worden in het script.
     float angle,v3=0.2,v2=0.1,v1=0.05,anglemax=1
     Encoder motor1(PTD0,PTC9);
     PwmOut pwm_motor(PTA5);
     pwm_motor.period_us(100);
     DigitalOut motordir(PTD1);
+    
+    int toestand = TERUGKEREN;
+    
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,TSAMP);
     while(1) {
@@ -42,10 +45,22 @@
         float new_pwm;
         while(!looptimerflag);
         looptimerflag = false;
-        //---------------------------------------------------------leest potentiometer (?), probably won't do what I want it to do ------------------
-        //angle = ???? Encoder
+        
+        if (angle<anglemin)
+        {
+            toestand = WACHTEN;
+        }
+        
+        if (toestand = WACHTEN && EMG(readbiceps, readtriceps) != 0)
+        {
+            toestand = SLAAN
+        }
+    // De toestand moet vanaf wacht naar slaan gaan bij aanspanning van spier(1) Hier zit waarschijnlijk een delay in 
+    // vanwege het middelen over de tijd van het signaal. van MK en Tanja stukje script met functie met return (0/1/2 of 3).
+    // angle = ???? Encoder
         if(toestand==SLAAN)
         {
+            // potmeter.read() wordt waarschijnlijk output EMG functie (0,1,2,3)
             if ( 0.75 < potmeter.read() && angle<anglemax) //angle<anglemax moet ergens anders staan (iets met toestand)
             {
                 setpoint = v3; //experimenteel bepaald, snelheid om bovenste goal te raken
@@ -89,7 +104,6 @@
 
 float pid(float setpoint, float measurement)
 {
-    //What I want: if getposition gives value beneath min v=0!, If above max: return to 0 position(from the way it came)
     float error;
     static float prev_error = 0;
     float           out_p = 0;