Boudewijn van den Berg
..
Dependencies: MODSERIAL TextLCD mbed
Diff: main.cpp
- Revision:
- 0:4b5b608f1f71
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Nov 02 14:44:53 2013 +0000
@@ -0,0 +1,686 @@
+// 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.000
+#define CP1 0.0000000005
+#define CP2 0.0000000005
+#define CD 0.00
+#define CLP1 0.9975
+#define CLP2 0.001
+//Snelheid
+#define SNELHEID 0.02
+//LOOPTIME
+#define LOOPTIME 0.006667
+//Filtering EMG
+#define HP1 0.8752
+#define HP2 20.0
+#define HP3 20.0
+#define LP1 0.9868
+#define LP2 0.01325
+//EMG threshold
+#define SET_EMG_MAX1 10.0 //bovenarm rechts > beweging naar rechts
+#define SET_EMG_MIN1 8.0
+#define SET_EMG_MAX2 10.0 //bovenarm links > beweging naar links
+#define SET_EMG_MIN2 5.0
+#define SET_EMG_MAX3 10.0 //onderarm rechts > beweging naar boven
+#define SET_EMG_MIN3 7.0
+#define SET_EMG_MAX4 10.0 //onderarm links > beweging naar onder
+#define SET_EMG_MIN4 5.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);
+DigitalOut Brake1(PTD5);
+DigitalOut Brake2(PTA13);
+
+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;
+ int sol_updown=0;
+
+ //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("UP:Pause Drawing");
+ lcd.locate(0,1);
+ lcd.printf("DOWN:Pen Up/Down");
+ Solenoid=0;
+ sol_updown=0;
+ wait(0.01);
+ }
+ }
+ 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;
+ sol_updown=1;
+ wait(1);
+ Solenoid=0;
+ sol_updown=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++;
+ if (ButtonDOWN.read()==1)
+ {
+ if (sol_updown==1)
+ {
+ Solenoid=0;
+ sol_updown=0;
+ }
+ if (sol_updown==0)
+ {
+ Solenoid=1;
+ sol_updown=1;
+ }
+ while (ButtonDOWN.read()==1);
+ }
+
+ 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("UP:Pause Drawing");
+ lcd.locate(0,1);
+ lcd.printf("DOWN:Pen Up/Down");
+ }
+ }
+
+
+
+ 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;
+
+ if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1 && sol_updown==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 ",input);
+ }
+ pc.printf("%.2f ",v1);
+ pc.printf("%.2f ",v2);
+ pc.printf("%.2f ",v3);
+ pc.printf("%.2f \n\r",v4);
+ //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*CP1; //+foutverschilM1*CDloop+foutI1*CI;
+ pwm_to_motor2 = foutM2*CP2; //+foutverschilM2*CDloop+foutI2*CI;
+ 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.06;
+ }
+ else {
+ motordir1 = 0;
+ pwm_to_motor1=pwm_to_motor1-0.06;
+ }
+ if(pwm_to_motor2 > 0) {
+ motordir2 = 1;
+ pwm_to_motor2=pwm_to_motor2+0.06;
+ }
+ else {
+ motordir2 = 0;
+ pwm_to_motor2=pwm_to_motor2-0.06;
+ }
+
+ //WRITE VALUE TO MOTOR
+ if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1)
+ {
+ pwm_motor1.write(0);
+ pwm_motor2.write(0);
+ }
+ else
+ {
+ 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;
+ int i_timer=800;
+ Solenoid=1;
+ while(BeginM1 - motor1.getPosition() >= 10 || BeginM1 - motor1.getPosition() <= -10 || BeginM2 - motor2.getPosition() >= 10 || BeginM2 - motor2.getPosition() <= -10 || motor1.getSpeed()>=20 || motor2.getSpeed()>=20)
+ {
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,LOOPTIME);
+ while(looptimerflag != true);
+ looptimerflag = false;
+ M1position=motor1.getPosition();
+ M2position=motor2.getPosition();
+ pwm_to_motor1 = (BeginM1 - M1position)*.008;
+ pwm_to_motor2 = (BeginM2 - M2position)*.008;
+ keep_in_range(&pwm_to_motor1, -0.02,0.02);
+ if(pwm_to_motor1 > 0)
+ motordir1 = 1;
+ else
+ motordir1 = 0;
+ keep_in_range(&pwm_to_motor2, -0.04,0.04);
+ 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);
+
+ if(i_timer<=0) break;
+ i_timer--;
+
+ }
+ pwm_motor1.write(0);
+ pwm_motor2.write(0);
+ motordir1 = 0;
+ motordir2 = 0;
+ Brake1=1;
+ Brake2=1;
+ wait(0.4);
+ Brake1=0;
+ Brake2=0;
+ setpointM1=800.0;
+ setpointM2=2400.0;
+ setpointmin1M1=800.0;
+ setpointmin1M2=2400.0;
+ pwm_to_motor1=0.0;
+ pwm_to_motor2=0.0;
+ foutM1=0.0;
+ foutM2=0.0;
+ foutmin1M1=0.0;
+ foutmin1M2=0.0;
+ foutverschilM1=0.0;
+ foutverschilM2=0.0;
+ foutverschilmin1M1=0.0;
+ foutverschilmin1M2=0.0;
+ foutImin1=0.0;
+ foutImin2=0.0;
+ foutI1=0.0;
+ foutI2=0.0;
+ t_sin=0.0;
+ t_timer=0.0;
+ emg_value1min1=0.5;
+ emg_value2min1=0.5;
+ emg_value3min1=0.5;
+ emg_value4min1=0.5;
+ EMGhp1min1=0.5;
+ EMGhp2min1=0.5;
+ EMGhp3min1=0.5;
+ EMGhp4min1=0.5;
+ EMGlp1min1=0.5;
+ EMGlp2min1=0.5;
+ EMGlp3min1=0.5;
+ EMGlp4min1=0.5;
+ 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;
+}