Boudewijn van den Berg
RobotControlScript Groep 4
Dependencies: MODSERIAL TextLCD mbed
Revision 0:da261c102b95, committed 2013-11-05
- Comitter:
- bouvdberg
- Date:
- Tue Nov 05 19:29:42 2013 +0000
- Commit message:
- RobotControlGroep4
Changed in this revision
diff -r 000000000000 -r da261c102b95 MODSERIAL.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Tue Nov 05 19:29:42 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#b04ce87dc424
diff -r 000000000000 -r da261c102b95 TextLCD.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Tue Nov 05 19:29:42 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/wim/code/TextLCD/#e0da005a777f
diff -r 000000000000 -r da261c102b95 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Nov 05 19:29:42 2013 +0000
@@ -0,0 +1,1012 @@
+#include "mbed.h"
+#include "TextLCD.h"
+#include "MODSERIAL.h"
+#include "encoder.h"
+
+// definieren constanten
+#define PI 3.141593
+//plant
+#define ARM1 0.36
+#define ARM2 0.26
+//PD
+//#define CI 0.01 /mogelijke constante Integral Control
+#define CP1 0.01
+#define CP2 0.01
+//#define CD 0.000 //mogelijke Differential Control
+//#define CLP1 0.9975 //lowpass filter voor differential control
+///#define CLP2 0.001
+//Snelheid
+#define SNELHEID 0.02 // instelling snelheid
+//LOOPTIME
+#define LOOPTIME 0.006667 // sample frequentie: 150hz
+//Filtering EMG
+#define HP1 0.8752 // High-pass filter EMG
+#define HP2 20.0
+#define HP3 20.0
+#define LP1 0.9868 // Low-pass filter EMG
+#define LP2 0.01325
+//EMG threshold
+#define SET_EMG_MAX1 2.9 //bovenarm rechts > beweging naar rechts
+#define SET_EMG_MIN1 0.9
+#define SET_EMG_MAX2 7.4 //bovenarm links > beweging naar links
+#define SET_EMG_MIN2 5.3
+#define SET_EMG_MAX3 2.0 //onderarm rechts > beweging naar boven
+#define SET_EMG_MIN3 0.9
+#define SET_EMG_MAX4 6.6 //onderarm links > beweging naar onder
+#define SET_EMG_MIN4 3.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); //Remmen motoren
+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;
+
+ //Variabelen motoraansturing
+ float setpointM1=800.0, setpointM2=2400.0;
+ 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 sol_updown=0;
+ int t_sol=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) // Aanvang Menu
+ {
+ switch (menu)
+ {
+ case 0:
+ lcd.cls();
+ lcd.printf("> CALIBRATION");
+ lcd.locate(0,1);
+ lcd.printf(" DRAW");
+ while(menu==0)
+ {
+ if (ButtonDOWN.read()==1) menu++;
+ if (ButtonSELECT.read()==1) menu=70;
+ }
+ break;
+ case 1:
+ lcd.cls();
+ lcd.printf("> DRAW");
+ lcd.locate(0,1);
+ lcd.printf(" SETTINGS");
+ while(menu==1)
+ {
+ if (ButtonDOWN.read()==1) menu++;
+ if (ButtonUP.read()==1) menu--;
+ if (ButtonSELECT.read()==1)
+ {
+ motor1.setPosition(800);
+ motor2.setPosition(2400);
+ menu=55;
+ lcd.cls();
+ lcd.printf("UP: Pause");
+ lcd.locate(0,1);
+ lcd.printf("DOWN: Up/Down");
+ Solenoid=1;
+ sol_updown=1;
+ //wait(0.01);
+ }
+ }
+ break;
+ case 2:
+ lcd.cls();
+ lcd.printf("> SETTINGS");
+ lcd.locate(0,1);
+ lcd.printf(" RESET ALL");
+ while(menu==2)
+ {
+ if (ButtonUP.read()==1) menu--;
+ if (ButtonDOWN.read()==1) menu++;
+ if (ButtonSELECT.read()==1) menu=20;
+ }
+ break;
+ case 3:
+ lcd.cls();
+ lcd.printf("> RESET ALL");
+ lcd.locate(0,1);
+ lcd.printf(" ");
+ while(menu==3)
+ {
+ 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: //Settings voor handmatig instellen EMG thresholds
+ 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", (EMGmax2+ (((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=(EMGmax2+ (((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(); //Functie om de solenoid te testen via de settings
+ 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, voor functie, zie einde script, regel 909
+ lcd.cls();
+ lcd.printf(" Shutting Down!");
+ menu=0;
+
+ uitzetten();
+
+ break;
+ case 55: //tekenen, voor functie, regel 707
+
+ if (ButtonSTOP.read()==1) menu=50;
+ if (ButtonUP.read()==1) menu++;
+ if (ButtonDOWN.read()==1)
+ {
+ t_sol++;
+ if (t_sol>25)
+ {
+ if (sol_updown==0)
+ {
+ sol_updown=1;
+ Solenoid=1;
+ }
+ else
+ {
+ sol_updown=0;
+ Solenoid=0;
+ }
+ t_sol=0;
+ }
+ }
+
+ aansturing();
+
+ break;
+ case 56: //PAUSE, arm blijft op dezelfde plaats staan, Solenoid gaat omhoog
+ wait(0.2);
+ lcd.cls();
+ lcd.printf(" PAUSE ");
+ lcd.locate(0,1);
+ lcd.printf("> RESUME");
+
+ Solenoid=1;
+ sol_updown=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");
+ lcd.locate(0,1);
+ lcd.printf("DOWN: Up/Down");
+ Solenoid=0;
+ sol_updown=0;
+ }
+ }
+
+
+
+ break;
+ case 70: //Automatische kallibratie
+ wait(0.2);
+ lcd.cls();
+ lcd.printf("Calibration ...");
+ lcd.locate(0,1);
+ lcd.printf("start idle in: 3");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("start idle in: 2");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("start idle in: 1");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("start idle NOW");
+
+ menu++;
+
+ break;
+ case 71: //Kallibratie in rust
+ lcd.cls();
+ lcd.printf("Calibration idle");
+
+ float CAL_EMG1_MAX=0;
+ float CAL_EMG2_MAX=0;
+ float CAL_EMG3_MAX=0;
+ float CAL_EMG4_MAX=0;
+
+ //calc idle
+ for (int c=1000; c>=0; c--)
+ {
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,LOOPTIME);
+ while(looptimerflag != true);
+ looptimerflag = false;
+ emg_value1=EMG1.read();
+ EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
+ EMGhp1=abs(EMGhp1);
+ EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
+ EMGhp1min1=EMGhp1;
+ emg_value1min1=emg_value1;
+ EMGlp1min1=EMGlp1;
+ if (EMGlp1 > CAL_EMG1_MAX) CAL_EMG1_MAX=EMGlp1;
+
+ emg_value2=EMG2.read();
+ EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
+ EMGhp2=abs(EMGhp2);
+ EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
+ EMGhp2min1=EMGhp2;
+ emg_value2min1=emg_value2;
+ EMGlp2min1=EMGlp2;
+ if (EMGlp2 > CAL_EMG2_MAX) CAL_EMG2_MAX=EMGlp2;
+
+ emg_value3=EMG3.read();
+ EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
+ EMGhp3=abs(EMGhp3);
+ EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
+ EMGhp3min1=EMGhp3;
+ emg_value3min1=emg_value3;
+ EMGlp3min1=EMGlp3;
+ if (EMGlp3 > CAL_EMG3_MAX) CAL_EMG3_MAX=EMGlp3;
+
+ emg_value4=EMG4.read();
+ EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
+ EMGhp4=abs(EMGhp4);
+ EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
+ EMGhp4min1=EMGhp4;
+ emg_value4min1=emg_value4;
+ EMGlp4min1=EMGlp4;
+ if (EMGlp4 > CAL_EMG4_MAX) CAL_EMG4_MAX=EMGlp4;
+
+ }
+ EMGmin1= CAL_EMG1_MAX+0.35;
+ EMGmin2= CAL_EMG2_MAX+0.35;
+ EMGmin3= CAL_EMG3_MAX+0.35;
+ EMGmin4= CAL_EMG4_MAX+0.35;
+ 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;
+
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 1 in: 3");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 1 in: 2") ;
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 1 in: 1");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 1 NOW");
+
+ menu++;
+
+ break;
+ case 72: //Calibration EMG1
+ lcd.cls();
+ lcd.printf("Calibration EMG1");
+
+ //calc EMG1 MAX
+ CAL_EMG1_MAX=0;
+ for (int c=1000; c>=0; c--)
+ {
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,LOOPTIME);
+ while(looptimerflag != true);
+ looptimerflag = false;
+ emg_value1=EMG1.read();
+ EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
+ EMGhp1=abs(EMGhp1);
+ EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
+ EMGhp1min1=EMGhp1;
+ emg_value1min1=emg_value1;
+ EMGlp1min1=EMGlp1;
+
+ if (EMGlp1 > CAL_EMG1_MAX) CAL_EMG1_MAX=EMGlp1;
+ }
+ EMGmax1= CAL_EMG1_MAX+1.0;
+ emg_value1min1=0.5;
+ EMGhp1min1=0.5;
+ EMGlp1min1=0.5;
+
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 2 in: 3");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 2 in: 2") ;
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 2 in: 1");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 2 NOW");
+
+ menu++;
+
+ break;
+ case 73: //Calibration EMG2
+ lcd.cls();
+ lcd.printf("Calibration EMG2");
+
+ //calc EMG2 MAX
+ CAL_EMG2_MAX=0;
+ for (int c=1000; c>=0; c--)
+ {
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,LOOPTIME);
+ while(looptimerflag != true);
+ looptimerflag = false;
+ emg_value2=EMG2.read();
+ EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
+ EMGhp2=abs(EMGhp2);
+ EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
+ EMGhp2min1=EMGhp2;
+ emg_value2min1=emg_value2;
+ EMGlp2min1=EMGlp2;
+
+ if (EMGlp2 > CAL_EMG2_MAX) CAL_EMG2_MAX=EMGlp2;
+ }
+ EMGmax2= CAL_EMG2_MAX+1.0;
+ emg_value2min1=0.5;
+ EMGhp2min1=0.5;
+ EMGlp2min1=0.5;
+
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 3 in: 3");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 3 in: 2") ;
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 3 in: 1");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 3 NOW");
+
+ menu++;
+
+ break;
+ case 74: //Calibration EMG3
+ lcd.cls();
+ lcd.printf("Calibration EMG3");
+
+ //calc EMG3 MAX
+ CAL_EMG3_MAX=0;
+ for (int c=1000; c>=0; c--)
+ {
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,LOOPTIME);
+ while(looptimerflag != true);
+ looptimerflag = false;
+ emg_value3=EMG3.read();
+ EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
+ EMGhp3=abs(EMGhp3);
+ EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
+ EMGhp3min1=EMGhp3;
+ emg_value3min1=emg_value3;
+ EMGlp3min1=EMGlp3;
+
+ if (EMGlp3 > CAL_EMG3_MAX) CAL_EMG3_MAX=EMGlp3;
+ }
+ EMGmax3= CAL_EMG3_MAX+1.0;
+ emg_value3min1=0.5;
+ EMGhp3min1=0.5;
+ EMGlp3min1=0.5;
+
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 4 in: 3");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 4 in: 2") ;
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 4 in: 1");
+ wait(1);
+ lcd.locate(0,1);
+ lcd.printf("Next EMG 4 NOW");
+
+ menu++;
+
+ break;
+ case 75: //Calibration EMG4
+ lcd.cls();
+ lcd.printf("Calibration EMG4");
+
+ //calc EMG4 MAX
+ CAL_EMG4_MAX=0;
+ for (int c=1000; c>=0; c--)
+ {
+ Ticker looptimer;
+ looptimer.attach(setlooptimerflag,LOOPTIME);
+ while(looptimerflag != true);
+ looptimerflag = false;
+ emg_value4=EMG4.read();
+ EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
+ EMGhp4=abs(EMGhp4);
+ EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
+ EMGhp4min1=EMGhp4;
+ emg_value4min1=emg_value4;
+ EMGlp4min1=EMGlp4;
+
+ if (EMGlp4 > CAL_EMG4_MAX) CAL_EMG4_MAX=EMGlp4;
+ }
+ EMGmax4= CAL_EMG4_MAX+1.0;
+ emg_value4min1=0.5;
+ EMGhp4min1=0.5;
+ EMGlp4min1=0.5;
+ if ((EMGmax1-EMGmin1)<0.6 || (EMGmax2-EMGmin2)<0.6 || (EMGmax3-EMGmin3)<0.6 || (EMGmax4-EMGmin4)<0.6)
+ {
+ lcd.cls();
+ lcd.printf("Calibration ...");
+ lcd.locate(0,1);
+ lcd.printf("failed! retry!");
+ wait(1);
+ menu=70;
+ }
+ else
+ {
+ lcd.cls();
+ lcd.printf("Calibration ...");
+ lcd.locate(0,1);
+ lcd.printf("Done! Data Saved");
+ wait(1);
+
+ menu=0;
+ }
+
+ break;
+ }
+
+
+ if (menu!=55) wait(0.2);
+ }
+
+
+}
+
+void aansturing(void) //Functie voor tekenen
+{
+ while(looptimerflag != true);
+ looptimerflag = false;
+
+ //EMG uitlezen
+ emg_value1 = EMG1.read();
+ emg_value2 = EMG2.read();
+ emg_value3 = EMG3.read();
+ emg_value4 = EMG4.read();
+
+ //filtering en bepaling envelope
+ EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1; //20hz high-pass
+ 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); //rectify
+ EMGhp2=abs(EMGhp2);
+ EMGhp3=abs(EMGhp3);
+ EMGhp4=abs(EMGhp4);
+ EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1; //2hz lowpass
+ EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
+ EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
+ EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
+
+ //berekenen setpoint
+ //hoekinput, bepalen hoogte tussen minimale en maximale thershold
+
+ 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);
+
+
+ pc.printf("%.2f ",v1); //Eventueel voor monitoring input vanuit EMG op computer
+ pc.printf("%.2f ",v2);
+ pc.printf("%.2f ",v3);
+ pc.printf("%.2f ",v4);
+ if(v1<=0.0 && v2<=0.0 && v3<=0.0 && v4<=0.0) {
+ Solenoid=1; //Pen van papier
+ input=0.0;
+ snelheid=0.0;
+ }
+ else {
+ if (sol_updown==1) Solenoid=1; //Pen op papier
+ else
+ {
+ Solenoid=0;
+ }
+ snelheid=drawspeed;
+ if(v2>v1) {
+ input=(atan((v3-v4)/(v1-v2))+PI);
+ }
+ else {
+ input=(atan((v3-v4)/(v1-v2)));
+ }
+ }
+
+ //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((M2phi/3200.0)*2.0*PI)*ARM2; //Beperking x/y positie
+ Py=sin((M1position/3200.0)*2.0*PI)*ARM1+sin((M2phi/3200.0)*2.0*PI)*ARM2;
+
+ vx=cos(input)*snelheid;
+ vy=sin(input)*snelheid;
+
+ if(Py >= ARM1 && vy>=0 || Py <= 0.080 && vy<=0) {
+ vy=0.0;
+ }
+ if(Px <= -0.425 && vx<=0 || Px >= -0.080 && vx>=0) {
+ vx=0.0;
+ }
+
+ //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, -600,600);
+ keep_in_range(&wM2, -600,600);
+
+ //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;
+ setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
+
+ //Beperking hoeken
+ keep_in_range(&setpointM1, 500,1400); //Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
+ keep_in_range(&setpointM2, 1600,2950); // Begrenzing hoeken
+
+ foutM1 = setpointM1-M1position;
+ foutM2 = setpointM2-M2position;
+ //foutI1 = foutImin1 + foutM1*LOOPTIME; //Groene regel: Eventuele regelaars
+ //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;//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.03; //Optelling PWM om statische wrijving beter te overwinnen
+ }
+ else {
+ motordir1 = 0;
+ pwm_to_motor1=pwm_to_motor1-0.03;
+ }
+ if(pwm_to_motor2 > 0) {
+ motordir2 = 1;
+ pwm_to_motor2=pwm_to_motor2+0.02;
+ }
+ else {
+ motordir2 = 0;
+ pwm_to_motor2=pwm_to_motor2-0.02;
+ }
+
+
+ //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) //Functie voor uitzetten
+{
+ float BeginM1 = 800;
+ float BeginM2 = 2400;
+ int i_timer=500;
+ Solenoid=1;
+ sol_updown=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; //Eerste keer regelen naar beginpositie
+ 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.03,0.03);
+ 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);
+
+ if(i_timer<=0) break;
+ i_timer--;
+
+ }
+ motordir1 = 1;
+ motordir2 = 1;
+ Brake1=1;
+ Brake2=1;
+ pwm_motor1.write(0);
+ pwm_motor2.write(0);
+ wait(1.0);
+ Brake1=0;
+ Brake2=0;
+ i_timer=300;
+ while(BeginM1 - motor1.getPosition() >= 5 || BeginM1 - motor1.getPosition() <= -5 || BeginM2 - motor2.getPosition() >= 5 || BeginM2 - motor2.getPosition() <= -5) //|| motor1.getSpeed()>=20 || motor2.getSpeed()>=20
+ {
+ Ticker looptimer; //Tweede keer regelen naar beginpositie
+ looptimer.attach(setlooptimerflag,LOOPTIME);
+ while(looptimerflag != true);
+ looptimerflag = false;
+ M1position=motor1.getPosition();
+ M2position=motor2.getPosition();
+ pwm_to_motor1 = (BeginM1 - M1position)*.012;
+ pwm_to_motor2 = (BeginM2 - M2position)*.008;
+ 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.06,0.06);
+ 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--;
+
+ }
+ motordir1 = 1;
+ motordir2 = 1; //Definieren beginwaarden
+ Brake1=1;
+ Brake2=1;
+ pwm_motor1.write(0);
+ pwm_motor2.write(0);
+ wait(1.0);
+ 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;
+}
\ No newline at end of file
diff -r 000000000000 -r da261c102b95 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Nov 05 19:29:42 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file