Diff: main.cpp

Revision:

26:02170c78ac2f

Parent:

25:9ab3123c2b15

Child:

27:fb73c8b5321a

--- a/main.cpp	Fri Oct 17 07:09:41 2014 +0000
+++ b/main.cpp	Fri Oct 17 11:05:47 2014 +0000
@@ -1,123 +1,111 @@
 #include "mbed.h"
 #include "encoder.h"
-#include "MODSERIAL.h"
 
-#define TSAMP 0.01
-#define K_P (0.01) //aanpassen Ki Kp en Kd vóór motortest!
+#define K_P (0.01)
 #define K_I (0  *TSAMP)
 #define K_D (0  /TSAMP)
 #define I_LIMIT 1.
-#define POT_AVG 50
+
+#define TSAMP 0.01
 #define WACHTEN 1
 #define SLAAN 2
 #define TERUGKEREN 3
-#define ANGLEMAX 244
+#define ANGLEMAX 1000
 #define ANGLEMIN 0
 
-MODSERIAL pc(USBTX,USBRX);
-void clamp(float * in, float min, float max);
-float pid(float setpoint, float measurement);
-AnalogIn potmeter(PTC2);
+float pid(float setspeed, float measurement);
+float new_pwm;
+DigitalOut motordir2(PTA4);
+PwmOut pwm_motor2(PTA5);
+Encoder motor2(PTD0,PTD2);
+
 volatile bool looptimerflag;
-float potsamples[POT_AVG];
-int EMG(float biceps, float triceps); // waarschijnlijk niet float omdat input functies zijn
 
 void setlooptimerflag(void)
 {
     looptimerflag = true;
 }
 
-int main()
+int main ()
 {
-    float v3=0.2,v2=0.1,v1=0.05, readbiceps=0, readtriceps=0;
-    // zie schrift voor berekeningen ANGLEMAX. Is float ANGLEMAX nog nodig als ANGLEMAX = defined
-    Encoder motor1(PTD0,PTC9);
-    PwmOut pwm_motor(PTA5);
-    pwm_motor.period_us(100);
-    DigitalOut motordir(PTD1);
+    pwm_motor2.write(0.3); 
+    motordir2 = 0;
     
-    int toestand = TERUGKEREN;
-    //moet hier wel int voor staan? want terugkeren is al gedifined alszijnde 3 (waarschijnlijk wel want toestand is hier voor het eerst benoemd...)
+    int toestand = TERUGKEREN; //initial condition
+    int EMG = 1;
+    float setspeed = 0, V3=40, V2=30, V1 =20, Vreturn= 15;//V in rad/s
     
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,TSAMP);
     while(1) 
     {
-        float setpoint;
-        float new_pwm;
         while(!looptimerflag);
         looptimerflag = false;
         
-        if (motor1.getPosition()<= ANGLEMIN && toestand != SLAAN)
+        if (motor2.getPosition()<= ANGLEMIN && toestand != SLAAN)
         {
             toestand = WACHTEN;
         }
         
-        if (EMG(readbiceps, readtriceps) != 0 && toestand == WACHTEN) //door de ifs uit elkaar te halen en && condities toe te voegen kan het minder snel fout gaan!
+        if (EMG != 0 && toestand == WACHTEN) // iets vinden om te testen... ipv 1
         {
             toestand = SLAAN;
-            if ( EMG(readbiceps, readtriceps)==3)
+            if ( EMG==3)
             {
-                setpoint = v3; //experimenteel bepaald, snelheid(?)>>>=pulsewidthmodulation! geclampt tussen -1/1. om bovenste goal te raken 
-            }
-            else if ( EMG(readbiceps, readtriceps)==2)
-            {
-                setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
+                setspeed = V3;
             }
-            else if (EMG(readbiceps, readtriceps)==1)
+             if ( EMG==2)
             {
-                setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
+                setspeed = V2;
             }
-            else //waarschijnlijk niet nodig want hij kan hier niet komen als readEMG nul is
+             if ( EMG==1)
             {
-                toestand = TERUGKEREN;
+               setspeed = V1;
             }
         }
         
-    // 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).
-    //new_pwm = getal, opletten dat deze tijdens het slaan niet kan veranderen! 
-    //kan niet want hij gaat pas naar terugkeren als ANGLEMAX is overschreden 
-    //en hij kan niet bij veranderingen van setpoints komen als de toestand = SLAAN
-        if (toestand != SLAAN)//is dit de beste manier en plaats voor dit?
+        if (toestand == SLAAN && motor2.getPosition() >= ANGLEMAX)
         {
-            setpoint = 0;
+            toestand = TERUGKEREN;
+        }        
+        if (toestand == TERUGKEREN)
+        {
+            new_pwm = pid(Vreturn, motor2.getSpeed());
+            pwm_motor2.write(new_pwm);  
+            motordir2 = 1;
         }
-        new_pwm = pid(setpoint, motor1.getPosition());
-        clamp(&new_pwm, -1,1);
-    //-------------------------------------------------------------------------------------output motor richting-------------------------------------
-        if(new_pwm > 0)
-            motordir = 0;
-        else
-            motordir = 1;
-        pwm_motor.write(abs(new_pwm)); 
-        if(motor1.getPosition()>= ANGLEMAX && toestand==SLAAN)
-        { 
-            toestand = TERUGKEREN;
-            //setpoint = 0 //misschien -5 oid... (hij moet 0 wel echt berijken)
-            // pc.printf ("Toestand = terugkeren!");
+         if (toestand == WACHTEN)
+        {
+            pwm_motor2.write(0); 
         }
-    } // eind while (1)
-} //eind int main
+        if (toestand == SLAAN)
+        {
+            new_pwm = pid(setspeed, motor2.getSpeed());
+            motordir2 = 0; 
+            pwm_motor2.write(new_pwm);
+        }
+            
+    }//end while
+}//end main
 
-void clamp(float * in, float min, float max)
+
+void clamp(float * in, float min, float max)//nodig??
 {
     *in > min ? *in < max? : *in = max: *in = min;
 }
 
-
-float pid(float setpoint, float measurement)
+float pid(float setspeed, float measurement)
 {
     float error;
     static float prev_error = 0;
     float           out_p = 0;
     static float    out_i = 0;
     float           out_d = 0;
-    error  = setpoint-measurement;
+    error  = setspeed-measurement;
     out_p  = error*K_P;
     out_i += error*K_I;
     out_d  = (error-prev_error)*K_D;
     clamp(&out_i,-I_LIMIT,I_LIMIT);
     prev_error = error;
     return out_p + out_i + out_d;
-}
+}
\ No newline at end of file