Revision 0:58c11abe4785, committed 2013-11-03

Comitter:

bouvdberg

Date:

Sun Nov 03 10:55:53 2013 +0000

Commit message:

Versie 3

Changed in this revision

diff -r 000000000000 -r 58c11abe4785 MODSERIAL.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Sun Nov 03 10:55:53 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MODSERIAL/#b04ce87dc424

diff -r 000000000000 -r 58c11abe4785 TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Sun Nov 03 10:55:53 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/wim/code/TextLCD/#e0da005a777f

diff -r 000000000000 -r 58c11abe4785 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Nov 03 10:55:53 2013 +0000
@@ -0,0 +1,617 @@
+// Hello World! for the TextLCD*//
+
+#include "mbed.h"
+#include "TextLCD.h"
+#include "MODSERIAL.h"
+#include "encoder.h"
+
+// definieren constanten
+#define PI 3.1415926
+//plant
+#define ARM1 0.36
+#define ARM2 0.26
+//PD
+#define CI 0.012
+#define CP 0.001
+#define CD 0.02
+#define CLP1 0.9975
+#define CLP2 0.001
+//Snelheid
+#define SNELHEID 0.05
+//LOOPTIME
+#define LOOPTIME 0.006667
+//Filtering EMG
+#define HP1 0.905
+#define HP2 20.0
+#define HP3 20.0
+#define LP1 0.9753
+#define LP2 0.0247
+//EMG threshold
+#define SET_EMG_MAX1        3.0 //bovenarm rechts > beweging naar rechts
+#define SET_EMG_MIN1        1.5
+#define SET_EMG_MAX2        2.0 //bovenarm links  > beweging naar links
+#define SET_EMG_MIN2        0.75
+#define SET_EMG_MAX3        3.5 //onderarm rechts  > beweging naar boven
+#define SET_EMG_MIN3        1.5
+#define SET_EMG_MAX4        2.0 //onderarm links  > beweging naar onder
+#define SET_EMG_MIN4        1.0
+
+void aansturing(void);
+void uitzetten(void);
+void setlooptimerflag(void);
+void keep_in_range(float * in, float min, float max);
+
+volatile bool looptimerflag;
+
+Serial pc(USBTX, USBRX);
+TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2,NC,NC,TextLCD::HD44780); // rs, e, d4-d7-/*+-9
+AnalogIn EMG1(PTB0);                    //EMG
+AnalogIn EMG2(PTB1);
+AnalogIn EMG3(PTB2);
+AnalogIn EMG4(PTB3);
+AnalogIn potmeter(PTC2);                //potmeter
+DigitalIn ButtonSTOP(PTE21);            //Knopjes voor kalibratie
+DigitalIn ButtonSELECT(PTE20);
+DigitalIn ButtonUP(PTE23);
+DigitalIn ButtonDOWN(PTE22);
+DigitalOut Solenoid(PTD4);              //Solenoid
+Encoder motor1(PTD0,PTC8);              //Encoder
+Encoder motor2(PTD2,PTC9);
+PwmOut pwm_motor1(PTA12);               //motor
+DigitalOut motordir1(PTD3);
+PwmOut pwm_motor2(PTA5);
+DigitalOut motordir2(PTD1);
+
+float numberx = 9;
+int menu=0, t;
+float EMGmax1=SET_EMG_MAX1, EMGmin1=SET_EMG_MIN1, EMGmax2=SET_EMG_MAX2, EMGmin2=SET_EMG_MIN2;
+float EMGmax3=SET_EMG_MAX3, EMGmin3=SET_EMG_MIN3, EMGmax4=SET_EMG_MAX4, EMGmin4=SET_EMG_MIN4;
+float drawspeed=SNELHEID;
+
+
+//Variabelen verwerking EMG
+    float emg_value1, emg_value2, emg_value3, emg_value4;
+    float emg_value1min1=0.5, emg_value2min1=0.5, emg_value3min1=0.5, emg_value4min1=0.5;
+    float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4;
+    float EMGhp1min1=0.5, EMGhp2min1=0.5, EMGhp3min1=0.5, EMGhp4min1=0.5, EMGlp1min1=0.5, EMGlp2min1=0.5, EMGlp3min1=0.5, EMGlp4min1=0.5;
+
+    //Variabelen bepaling input systeem
+    float input;
+    float w1, w2, wM2, phi1, phi2, theta;
+    float a, b, c, d, ai, bi, ci, di;
+    float v1, v2, v3, v4, vx, vy, snelheid;
+    float M1position, M2position, M2phi;
+    float Px, Py;
+    
+    //Variabelen motoraansturing
+    float setpointM1, setpointM2;
+    float setpointmin1M1=800.0, setpointmin1M2=2400.0;
+    float pwm_to_motor1, pwm_to_motor2;
+    float foutM1, foutM2;
+    float foutmin1M1=0.0, foutmin1M2=0.0;
+    float foutverschilM1, foutverschilM2;
+    float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0;
+    float foutImin1=0.0, foutImin2=0.0, foutI1, foutI2;
+    float CDloop=CD/LOOPTIME;
+    float t_sin=0.0;
+    float t_timer=0.0;
+
+int main() {
+    //set buttons PULLDOWN
+    ButtonSTOP.mode(PullNone);
+    ButtonSELECT.mode(PullNone);
+    ButtonUP.mode(PullNone);
+    ButtonDOWN.mode(PullNone);
+    pc.baud(57600);
+    //Aanstuur timing
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,LOOPTIME); 
+    while(1)
+    {
+        switch (menu) 
+        {
+        case 0:
+            lcd.cls();
+            lcd.printf("> DRAW");
+            lcd.locate(0,1);
+            lcd.printf("  SETTINGS");
+            while(menu==0)
+            {
+                if (ButtonDOWN.read()==1)   menu++;
+                if (ButtonSELECT.read()==1) 
+                {
+                    motor1.setPosition(800);
+                    motor2.setPosition(2400);
+                    menu=55;
+                    lcd.cls();
+                    lcd.printf("  Drawing ...");
+                    lcd.locate(0,1);
+                    lcd.printf("UP: Pause");
+                }
+            }
+        break;    
+        case 1:
+            lcd.cls();
+            lcd.printf("> SETTINGS");
+            lcd.locate(0,1);
+            lcd.printf("  RESET ALL");
+            while(menu==1)
+            {
+                if (ButtonUP.read()==1)     menu--;
+                if (ButtonDOWN.read()==1)   menu++;
+                if (ButtonSELECT.read()==1) menu=20;
+            }
+        break;
+        case 2:
+            lcd.cls();
+            lcd.printf("> RESET ALL");
+            lcd.locate(0,1);
+            lcd.printf("           ");
+            while(menu==2)
+                {
+                if (ButtonUP.read()==1)     menu--;
+                if (ButtonSELECT.read()==1)
+                {
+                    EMGmax1=SET_EMG_MAX1;   EMGmin1=SET_EMG_MIN1;
+                    EMGmax2=SET_EMG_MAX2;   EMGmin2=SET_EMG_MIN2;
+                    EMGmax3=SET_EMG_MAX3;   EMGmin3=SET_EMG_MIN3;
+                    EMGmax4=SET_EMG_MAX4;   EMGmin4=SET_EMG_MIN4;
+                    drawspeed=SNELHEID;
+                    lcd.locate(0,1);
+                    lcd.printf(" Reset Completed");
+                    wait(1);
+                    lcd.locate(0,1);
+                    lcd.printf("                ");
+                }
+            }
+        break;
+        case 20:
+            lcd.cls();
+            lcd.printf("> EMG1-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
+            lcd.locate(0,1);
+            lcd.printf("  EMG1-MIN: ");
+            if (ButtonSTOP.read()==1) menu=0;
+            if (ButtonDOWN.read()==1) menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                EMGmax1=(EMGmax1+(((potmeter.read()+0.0005)*2)-1));
+                wait(0.5);
+                }
+        break;
+        case 21:
+            lcd.cls();
+            lcd.printf("> EMG1-MIN: %.2f", (EMGmin1+ (((potmeter.read()+0.0005)*2)-1)));
+            lcd.locate(0,1);
+            lcd.printf("  EMG2-MAX: ");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                EMGmin1=(EMGmin1+ (((potmeter.read()+0.0005)*2)-1));
+                wait(0.5);
+                }
+        break;
+        case 22:
+            lcd.cls();
+            lcd.printf("> EMG2-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
+            lcd.locate(0,1);
+            lcd.printf("  EMG2-MIN: ");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                EMGmax2=(EMGmax1+ (((potmeter.read()+0.0005)*2)-1));
+                wait(0.5);
+                }
+        break;
+        case 23:
+            lcd.cls();
+            lcd.printf("> EMG2-MIN: %.2f", (EMGmin2+ (((potmeter.read()+0.0005)*2)-1)));
+            lcd.locate(0,1);
+            lcd.printf("  EMG3-MAX: ");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                EMGmin2=(EMGmin2+ (((potmeter.read()+0.0005)*2)-1));
+                wait(0.5);
+                }
+        break;
+        case 24:
+            lcd.cls();
+            lcd.printf("> EMG3-MAX: %.2f", (EMGmax3+ (((potmeter.read()+0.0005)*2)-1)));
+            lcd.locate(0,1);
+            lcd.printf("  EMG3-MIN: ");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                EMGmax3=(EMGmax3+ (((potmeter.read()+0.0005)*2)-1));
+                wait(0.5);
+                }
+        break;
+        case 25:
+            lcd.cls();
+            lcd.printf("> EMG3-MIN: %.2f", (EMGmin3+ (((potmeter.read()+0.0005)*2)-1)));
+            lcd.locate(0,1);
+            lcd.printf("  EMG4-MAX: ");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                EMGmin3=(EMGmin3+ (((potmeter.read()+0.0005)*2)-1));
+                wait(0.5);
+                }
+        break;
+        case 26:
+            lcd.cls();
+            lcd.printf("> EMG4-MAX: %.2f", (EMGmax4+ (((potmeter.read()+0.0005)*2)-1)));
+            lcd.locate(0,1);
+            lcd.printf("  EMG4-MIN: ");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                EMGmax4=(EMGmax4+ (((potmeter.read()+0.0005)*2)-1));
+                wait(0.5);
+                }
+        break;
+        case 27:
+            lcd.cls();
+            lcd.printf("> EMG4-MIN: %.2f", (EMGmin4+ (((potmeter.read()+0.0005)*2)-1)));
+            lcd.locate(0,1);
+            lcd.printf("  SPEED   :");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                EMGmin4=(EMGmin4+ (((potmeter.read()+0.0005)*2)-1));
+                wait(0.5);
+                }
+        break;
+        case 28:
+            lcd.cls();
+            lcd.printf("> SPEED  : %.2f", (drawspeed+ (((potmeter.read()+0.0005)/10)-0.05)));
+            lcd.locate(0,1);
+            lcd.printf("  SOLENOID:");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                lcd.locate(0,1);
+                lcd.printf("  SAVED!      ");
+                drawspeed=(drawspeed+ (((potmeter.read()+0.0005)/10)-0.05));
+                wait(0.5);
+                }
+        break;
+        case 29:
+            lcd.cls();
+            lcd.printf("> SOLENOID: OFF");
+            lcd.locate(0,1);
+            lcd.printf("           ");
+            if (ButtonSTOP.read()==1)   menu=0;
+            if (ButtonUP.read()==1)     menu--;
+            //if (ButtonDOWN.read()==1)   menu++;
+            if (ButtonSELECT.read()==1)
+                {
+                    lcd.cls();
+                    lcd.printf("> SOLENOID: ON");
+                    lcd.locate(0,1);
+                    lcd.printf("           ");
+                    Solenoid=1;
+                    wait(1);
+                    Solenoid=0;
+                }
+        break;
+        case 50:            //tekenen afsluiten
+            lcd.cls();
+            lcd.printf(" Shutting Down!");
+            menu=0;
+            
+            uitzetten();
+   
+        break;
+        case 55:         
+            if (ButtonSTOP.read()==1)       menu=50;
+            if (ButtonUP.read()==1)         menu++;
+            
+            
+            aansturing();                                                                                    //aansturing
+            
+            
+        break;
+        case 56:
+            wait(0.2);
+            lcd.cls();
+            lcd.printf("  PAUSE       ");
+            lcd.locate(0,1);
+            lcd.printf("> RESUME");
+                       
+            Solenoid=1;
+            pwm_motor1.write(0);
+            pwm_motor2.write(0);
+            
+            while(ButtonSELECT.read()==1);
+            while(menu==56)
+            {
+            if (ButtonSTOP.read()==1)       menu=50;
+            if (ButtonSELECT.read()==1)
+            {     
+                menu--;
+                 lcd.cls();
+                 lcd.printf("  Drawing ...");
+                 lcd.locate(0,1);
+                 lcd.printf("UP: Pause");
+            }
+            }
+
+                     
+            
+        break;
+    }
+
+            if (menu!=55) wait(0.2);      
+    }
+
+    
+}
+
+void aansturing(void)
+{ 
+        while(looptimerflag != true);
+        looptimerflag = false;
+        
+        //uitlezen
+        emg_value1 = EMG1.read();
+        emg_value2 = EMG2.read();
+        emg_value3 = EMG3.read();
+        emg_value4 = EMG4.read();
+        
+        //filtering
+        EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
+        EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
+        EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
+        EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
+        EMGhp1=abs(EMGhp1);
+        EMGhp2=abs(EMGhp2);
+        EMGhp3=abs(EMGhp3);
+        EMGhp4=abs(EMGhp4);
+        EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
+        EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
+        EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
+        EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
+        //pc.printf("%.2f   ",emg_value1);
+        //pc.printf("%.2f   ",emg_value2);
+        //pc.printf("%.2f   ",emg_value3);
+        //pc.printf("%.2f   ",emg_value4);
+        //pc.printf("%.2f   ",EMGlp1);
+        //pc.printf("%.2f   ",EMGlp2);
+        //pc.printf("%.2f   ",EMGlp3);
+        //pc.printf("%.2f   ",EMGlp4);
+        
+        //berekenen setpoint
+        //hoekinput
+        
+        if((EMGlp1-EMGmin1)<=0.0)    v1=0.0;       
+        else                         v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);       
+        if((EMGlp2-EMGmin2)<=0.0)    v2=0.0;
+        else                         v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);   
+        if((EMGlp3-EMGmin3)<=0.0)    v3=0.0;
+        else                         v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3);    
+        if((EMGlp4-EMGmin4)<=0.0)    v4=0.0;
+        else                         v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4);
+            
+        t_timer=t_timer+LOOPTIME;
+        
+        pc.printf("%.2f   ",v1);
+        pc.printf("%.2f   ",v2);
+        pc.printf("%.2f   ",v3);
+        pc.printf("%.2f   ",v4);
+        if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1) {
+            Solenoid=1;             //Pen van papier
+            input=0.0;
+            snelheid=0.0;
+            }  
+        else {
+            Solenoid=0;             //Pen op papier
+            snelheid=drawspeed;
+            if(v2>v1) {
+            input=(atan((v3-v4)/(v1-v2))+PI);
+            }
+            else {
+            input=(atan((v3-v4)/(v1-v2)));
+            }
+            }
+            pc.printf("%.2f  \n\r",input);
+            //snelheid=drawspeed;
+            //input=t_timer*0.8+PI;
+        
+        //snelheidsvector met beperking positie / encoder uitlezen
+        M1position = motor1.getPosition();
+        M2position = motor2.getPosition();
+        //M2phi=M1position-M2position+1600.0;         //phi2 = phi1 - theta + 1600
+        
+        //Px=cos((M1position/3200.0)*2.0*PI)*ARM1+cos((M2position/3200.0)*2.0*PI)*ARM2;
+        //Py=sin((M1position/3200.0)*2.0*PI)*ARM1+sin((M2position/3200.0)*2.0*PI)*ARM2;
+        
+        //if(Px > 0.29 || Px < 0.125)     vx=0;
+        //else                            
+        vx=cos(input)*snelheid; 
+        //if(Py < -0.425 || Py > -0.125)  vy=0;
+        //else                            
+        vy=sin(input)*snelheid;
+        
+        //input positie
+        phi1=(motor1.getPosition()/3200.0)*2.0*PI;
+        theta=(motor2.getPosition()/3200.0)*2.0*PI;
+        phi2=theta+phi1-PI;
+        
+        //Jacobiaan
+        // [a  b]
+        // [c  d]
+        a=-sin(phi1)*ARM1;
+        b=-sin(phi2)*ARM2;
+        c=cos(phi1)*ARM1;
+        d=cos(phi2)*ARM2;
+        
+        //inverse
+        // [ai  bi]
+        // [ci  di]
+        ai=d/(a*d-b*c);
+        bi=-b/(a*d-b*c);
+        ci=-c/(a*d-b*c);
+        di=a/(a*d-b*c);
+        
+        //vermenigvuldiging
+        // [ai  bi]   [vx]   [w1]
+        // [ci  di] * [vy] = [w2]
+        w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1
+        w2=ci*vx+di*vy;
+        wM2=w2-w1;//hoeksnelheid motor 2
+        
+        //Beveiliging hoeksnelheden
+        keep_in_range(&w1, -1000,1000);
+        keep_in_range(&wM2, -1000,1000);
+    
+        //Motoraansturing
+        //t_sin=t_sin + 0.05; 
+        //if (t_sin>=2*PI) t_sin=0.0;
+        setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1;          //sin(t_sin)*1600;
+        setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
+        
+        //Beperking hoeken
+        keep_in_range(&setpointM1, 0,1600);//Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
+        keep_in_range(&setpointM2, 1600,3050);// Begrensd op 20 graden minimaal, werkelijke minimale waarde is 15 graden
+        
+        foutM1 = setpointM1-M1position;
+        foutM2 = setpointM2-M2position;
+        foutI1 = foutImin1 + foutM1*LOOPTIME;
+        foutI2 = foutImin2 + foutM2*LOOPTIME;
+        foutverschilM1 = foutM1-foutmin1M1;
+        foutverschilM2 = foutM2-foutmin1M2;
+        foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1;
+        foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2;
+        pwm_to_motor1 = foutM1*CP+foutverschilM1*CDloop+foutI1*CI;
+        pwm_to_motor2 = foutM2*CP+foutverschilM2*CDloop+foutI2*CI;//foutM2*CP+foutverschilM2*CDloop;
+        keep_in_range(&pwm_to_motor1, -0.2,0.2);
+        keep_in_range(&pwm_to_motor2, -0.2,0.2);
+        
+        
+        if(pwm_to_motor1 > 0) {
+            motordir1 = 1;
+            pwm_to_motor1=pwm_to_motor1+0.08;
+            }
+        else {
+            motordir1 = 0;
+            pwm_to_motor1=pwm_to_motor1-0.08;
+            }
+        if(pwm_to_motor2 > 0) {
+            motordir2 = 1;
+            pwm_to_motor2=pwm_to_motor2+0.08;
+            }
+        else {
+            motordir2 = 0;
+            pwm_to_motor2=pwm_to_motor2-0.08;
+            }
+        
+        
+        //WRITE VALUE TO MOTOR  
+        pwm_motor1.write(abs(pwm_to_motor1));
+        pwm_motor2.write(abs(pwm_to_motor2));
+        
+        //Definieren waarden in de verleden tijd
+        foutmin1M1=foutM1;
+        foutmin1M2=foutM2;
+        foutverschilmin1M1=foutverschilM1;
+        foutverschilmin1M2=foutverschilM2;
+        foutImin1=foutI1;
+        foutImin2=foutI2;
+        setpointmin1M1=setpointM1;     
+        setpointmin1M2=setpointM2;
+        emg_value1min1=emg_value1;
+        emg_value2min1=emg_value2;
+        emg_value3min1=emg_value3;
+        emg_value4min1=emg_value4;
+        EMGhp1min1=EMGhp1; 
+        EMGhp2min1=EMGhp2;
+        EMGhp3min1=EMGhp3;
+        EMGhp4min1=EMGhp4;
+        EMGlp1min1=EMGlp1;
+        EMGlp2min1=EMGlp2;
+        EMGlp3min1=EMGlp3;
+        EMGlp4min1=EMGlp4;     
+    
+
+}
+void uitzetten(void)
+{
+    float BeginM1 = 800;
+    float BeginM2 = 2400;
+    Solenoid=1;
+    while(BeginM1 - motor1.getPosition() >= 10 || BeginM1 - motor1.getPosition() <= -10 || BeginM2 - motor2.getPosition() >= 10 || BeginM2 - motor2.getPosition() <= -10) 
+    {
+        Ticker looptimer;
+        looptimer.attach(setlooptimerflag,LOOPTIME); 
+        while(looptimerflag != true);
+        looptimerflag = false;
+        M1position=motor1.getPosition();
+        M2position=motor2.getPosition();
+        pc.printf(" %f ",M2position);
+        pwm_to_motor1 = (BeginM1 - M1position)*.002;
+        pwm_to_motor2 = (BeginM2 - M2position)*.002;
+        keep_in_range(&pwm_to_motor1, -0.05,0.05);
+        if(pwm_to_motor1 > 0)
+            motordir1 = 1;
+        else
+            motordir1 = 0;
+        keep_in_range(&pwm_to_motor2, -0.05,0.05);
+        if(pwm_to_motor2 > 0)
+            motordir2 = 1;
+        else
+            motordir2 = 0;
+        //WRITE VALUE TO MOTOR
+        pwm_motor1.write(abs(pwm_to_motor1));
+        pwm_motor2.write(abs(pwm_to_motor2));
+        float sent_pwm = abs(pwm_to_motor2);
+        pc.printf(" %f ",sent_pwm);
+          
+    }
+             
+     pwm_motor1.write(0);
+     pwm_motor2.write(0);  
+     menu=0; 
+}
+
+void keep_in_range(float * in, float min, float max)
+{
+    *in > min ? *in < max? : *in = max: *in = min;
+}
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}

diff -r 000000000000 -r 58c11abe4785 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Nov 03 10:55:53 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file