Diff: main.cpp

Revision:

3:611fd72c9d46

Parent:

2:f3e8a27d376c

Child:

4:c22f3095b130

--- a/main.cpp	Fri Oct 31 13:50:14 2014 +0000
+++ b/main.cpp	Sat Nov 01 19:40:46 2014 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
-#include "HIDScope.h"
+
 
 #include "encoder.h"
 #define K_P (0.1)
@@ -8,29 +8,13 @@
 #define K_D (0.0005 /TSAMP)
 #define TSAMP 0.001
 #define I_LIMIT 1.
-
-// Outputs van de motor
-Encoder encoderA(PTD5,PTA13);
-Encoder encoder1(PTD0,PTD2);
-
-
-// Outputs naar de motor
-PwmOut pwm(PTC8);
-DigitalOut dir(PTC9);
-PwmOut pwm1(PTA5);
-DigitalOut dir1(PTA4);
+#define THRESHOLD 0.02
 
-// Vaststellen van het datatype van de outputs naar de motor
-float s_speed1;
-float s_dir1;
-float s_speed2;
-float s_dir2;
-
-//lampjes
-
-DigitalOut rood(LED1);
-DigitalOut blauw(LED3);
-DigitalOut groen(LED2);
+#define K_P1 (0.004)
+#define K_I1 (0.00001*TSAMP1)
+#define K_D1 (0.0001/TSAMP1)
+#define TSAMP1 0.01
+#define I_LIMIT1 1.
 // Alle constantes voor de filters definiëren
 // Constantes voor de Low Pass filter
 #define A1LP    0.018180963222803
@@ -53,17 +37,34 @@
 
 // Sample Time
 #define TSAMP 0.001
+// Outputs van de motor
+Encoder encoderA(PTD5,PTA13);
+Encoder encoder1(PTD0,PTD2);
 
-// EMG Threshold waarde
-#define THRESHOLD 0.05
+#include <iostream>
 
-// EMG inputs
+// Outputs naar de motor
+PwmOut pwm(PTC8);
+DigitalOut dir(PTC9);
+PwmOut pwm1(PTA5);
+DigitalOut dir1(PTA4);
+
+//lampjes
+
+DigitalOut rood(LED1);
+DigitalOut blauw(LED3);
+DigitalOut groen(LED2);
 AnalogIn    emg1(PTB1);
 AnalogIn    emg2(PTB2);
+// Vaststellen van het datatype van de outputs naar de motor
+float s_speed1;
+float s_dir1;
+float s_speed2;
+float s_dir2;
 
 // PC communicatie
 MODSERIAL pc(USBTX,USBRX);
-HIDScope scope(5);
+
 int set;
 
 // Ticker voor de meetgegevens
@@ -71,49 +72,41 @@
 volatile bool looptimerflag;
 
 // Waardes voor de filters reserveren en als float vaststellen
-float emg_value2, ylp2, yhp2, yn2;
-float emg_value1, ylp1, yhp1, yn1;
+float emg_value2, ylp2, yhp2, yn2,ysum1 = 0, yave1=0;
+float emg_value1, ylp1, yhp1, yn1,ysum2 = 0, yave2=0 ;
+
 
-float ysum1 = 0, yave1=0 ;
-float ysum2 = 0, yave2=0 ;
+//variabvelen voor positie motor1
+float v1=0,out1 =0;
+int pos1 =0,zero =0, a=0;
+float out_i1 = 0;
+
 
 // 0 of 1 waardes gedefinieerd uit het EMG, met 0 is te lage activiteit, 1 is hoog genoeg, voor de zekerheid toch als int.
 uint8_t y1, y2;
 int check = 0;
 
 // integers reserveren voor het deel van het regelsysteem als we de boolean emg waardes hebben: hoek van het badje (0,1, of 2) en de gewenste snelheid (0,1, of 2)
-int badjestand;
-int badjes=1;
-int badjedone=0;
-int speeding;
-int armstand=0;
-int armspeed=0;
+int badjestand, badjes=1,badjedone=0,speeding,armstand=0,armspeed=0;
 bool speeddone=0;
-int a; //wordt gebruikt in gotopos
-float out1; //pid voor armpositie
+
 // Teller voor hoeveel metingen er zijn gedaan
 uint16_t teller=0;
 
 // Random meuk die ik uit batjespositie haal, nog ff opruimen
 int32_t enc = 0,enca2 =0,enca1=0, encp=0, counts =0;
 float speed = 0.1, out =0;
-int pos =0,zero =0, fout;
+int pos =0;
 float v=0;
 float out_i = 0;
 int y;
 
-
-// Scopes vaststellen
-void viewer()
+void clamp(float * in, float min, float max)
 {
-    scope.set(0,y1);
-    scope.set(1,y2);
-    scope.set(2,badjes);
-    scope.set(3,armstand);
-    scope.set(4,speeddone);
-    scope.send();
+*in > min ? *in < max? : *in = max: *in = min;
 }
 
+
 // EMG 1 uitlezen
 float readEMG1()
 {
@@ -275,58 +268,10 @@
 
 // Hoek van het badje vaststelen: begint met waarde 1, als EMG 1 alleen gebruikt wordt, gaat er 1 vanaf (maximaal 0), wordt EMG 2 alleen gebruikt, dan gaat er 1 bij (maximaal 2).
 // Als beide tegelijkertijd 2 maal achter elkaar gebruikt worden, dan wordt de hoek vastgezet (kan niet meer verandert worden tot de reset).
-uint8_t badje (uint8_t y1, uint8_t y2)
-{
-    if (y1>0 && y2>0 && check>0) {
-        badjedone=1;
-        check=0;
-        rood = 1;
-        groen = 0;
-    } else if (y1>0 && y2>0) {
-        check=1;
-    } else if (y1>0) {
-        badjes=0;
-        check=0;
-    } else if (y2>0 ) {
-        badjes=1;
-        check=0;
-    }
-    else {
-        check=0;
-        badjes = 2;
-    }
-    return badjes;
-}
+
 
 // Stukje dat hetzelfde als bij de hoek van het badje regelt, maar dan voor de snelheid waarmee de arm moet bewegen. Heeft ook de waardes 0,1, of 2.
 // Bij 2 maal achter elkaar EMG 1 en EMG 2 gebruikt, wordt de snelheid vastgezet.
-uint8_t velocity (uint8_t y1, uint8_t y2)
-{
-    if (y1>0 && y2>0 && check>0) {
-        speeddone=1;
-        check=0;
-        rood = 0;
-        groen = 1;
-    } else if (y1>0 && y2>0) {
-        check=1;
-    } else if (y1>0) {
-        if (speed>0) {
-            speed+=1;
-        } else {
-            check=0;
-        }
-    } else if (y2>0 ) {
-        if (speed<2) {
-            speed-=1;
-        } else {
-            check=0;
-        }
-    } else {
-        check=0;
-        
-    }
-    return speed;
-}
 
 void batposition(int y)
 {
@@ -335,14 +280,14 @@
           
             dir = 1;
             pwm.write(0.4);
-            wait(0.02);
+            wait(0.04);
             pwm.write(0);
             break;
         case 1:
             
             dir = 0;
             pwm.write(0.4);
-            wait(0.02);
+            wait(0.04);
             pwm.write(0);
             break;
         case 2:
@@ -353,35 +298,30 @@
     }
 
 }
-
-void clamp(float * in, float min, float max)
-{
-*in > min ? *in < max? : *in = max: *in = min;
-}
-
 float pid1(float setpoint, float measurement)
 {
     float error;
     static float prev_error = 0;
     float           out_p = 0;
-
     float           out_d = 0;
     error  = setpoint-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);
+    out_p  = error*K_P1;
+    out_i1 += error*K_I1;
+    out_d  = (error-prev_error)*K_D1;
+    clamp(&out_i1,-I_LIMIT1,I_LIMIT1);
     prev_error = error;
-    out1  = out_i+out_p+out_d;
+    out1  = out_i1+out_p+out_d;
     return out1;
 }
 
-float getv(float delta_t)
+float getv1(float delta_t)
 {
+    int enca1,enca2,counts,enc;
+    float v;
     int n =0 ;
     while(n<3) {
         wait(delta_t);
-        enc = encoderA.getPosition();
+        enc = encoder1.getPosition();
         enca2 = enca1;
         enca1 = enc;
         n++;
@@ -392,66 +332,120 @@
     v = (counts)*((2*3.14159265359)/1550);
     return v;
 }
-
-
-float gotopos(float pos)
+float reset()
+{
+    v1 = 1;
+    while(v1 !=0) {
+        dir1 = 0;
+        pwm1.write(0.1);
+        v1 =getv1(0.1);
+    }
+    pwm1 = 0;
+    dir1 =1;
+    encoder1.setPosition(0);
+   
+    return pwm1;
+}
+uint8_t velocity (uint8_t y1, uint8_t y2)
 {
-    switch(a) {
+    static int stand=0;
+    if (y1>0 && y2>0 && check>0) {
+        speeddone=1;
+        check=0;
+        rood = 0;
+        groen = 1;
+       cout<<"ga naar mode 1"<<endl;
+    } else if (y1>0 && y2>0) {
+        check=1;
+    } else if (y1>0) {
+        if (speed>0) {
+            stand=stand+1;
+            check=0;
+            cout<<"stand "<<stand<<endl;
+        }
+    } else if (y2>0 ) {
+        stand=stand-1;
+        check=0;
+        cout<<"stand "<<stand<<endl;
+        }
+     else{
+         stand = 3;
+         check =0;
+         }
+   
+    return stand;
+}
+
+uint8_t badje (uint8_t y1, uint8_t y2)
+{
+    if (y1>0 && y2>0 && check>0) {
+        badjedone=1;
+        check=0;
+        rood = 1;
+        groen = 0;
+        cout<<"ga naar mode 2"<<endl;
+        
+    } else if (y1>0 && y2>0) {
+        check=1;
+    } else if (y1>0) {
+        badjes=0;
+        check=0;
+    } else if (y2>0 ) {
+        badjes=1;
+        check=0;
+    }
+
+    else {
+        check=0;
+        badjes = 2;
+    }
+    return badjes;
+}
+
+void gotopos(int pos1)
+{
+    int place;
+    switch(pos1) {
+        case 0:
+           place = pos1;
+            break;
         case 1:
-            pos = 100;
+            place = 100;
             break;
 
         case 2:
-            pos = 200;
+            place = 200;
             break;
 
         case 3:
-            pos = 250;
+            place = 250;
             break;
     }
-    enc = encoder1.getPosition();
-    while((abs(pos-encoder1.getPosition()) >6)|| (v!= 0)) {
+    
+    while((abs(place-encoder1.getPosition()) >6)|| (v1!= 0)) {
 
         while(!looptimerflag);
         looptimerflag = false;
-        out1 = pid1(pos,encoder1.getPosition());
+        out1 = pid1(place,encoder1.getPosition());
 
-        if(out>0) {
+        if(out1>0) {
             dir1 = 1;
 
-        } else if(out<0) {
+        } else if(out1<0) {
             dir1 = 0;
         }
-        pwm1 = fabs(out);
-        v = getv(0.001);
+        pwm1 = fabs(out1);
+        v1 = getv1(0.001);
     }
     pwm1 =0;
 
-    return pwm1;
-}
-
-float reset()
-{
-    v = 1;
-    while(v !=0) {
-
-        dir = 0;
-        speed = 0.1;
-        pwm = speed;
-        v =getv(0.1);
-    }
-    pwm = 0;
-    dir =1;
-    encoderA.setPosition(0);
-    zero = encoderA.getPosition();
-
-    return pwm;
+    cout<<"place done"<<endl;
 }
 
 // Main code
 int main()
 {
-     v = 1;
+    reset();
     rood = 0;
     blauw = 1;
     groen = 1;
@@ -460,7 +454,7 @@
     timer.attach(setlooptimerflag,TSAMP);
     // 20 secondes wachten, zodat er geen effecten van het opstarten van het bordje, alvast waardes voor het badje of de arm vastzetten.
     wait(2);
-
+    cout<<"fase1"<<endl;
     while(1) {
 
         // Ticker
@@ -497,7 +491,7 @@
             } else if (badjedone==1) {
                 // Als de snelheid van de arm ook al klaar is, zorg ervoor dat de rest iet meer gebeurt
                 if (speeddone==1) {
-                    
+                    armspeed=armstand+1;
                     badjedone=0;
                     speeddone=0;
                     badjestand=1;
@@ -505,6 +499,8 @@
                     // Anders, pas de hoek van de arm aan naar de stand die hoort bij de huidig geselecteerde snelheid
                 } else {
                     armstand=velocity(y1,y2);
+        
+                    cout<<"armstand "<<armstand<<endl;
                     gotopos(armstand);
                 }
 
@@ -512,8 +508,6 @@
         }
         // Verzend data naar de scopes
 
-        viewer();
-        reset();
         
 
     }