Boudewijn van den Berg / Mbed 2 deprecated HuidigePositieV5

v5

Dependencies:   MODSERIAL TextLCD mbed

main.cpp@1:c18c171bf8b4, 2013-11-05 (annotated)

Committer:
bouvdberg
Date:
Tue Nov 05 10:14:49 2013 +0000
Revision:
1:c18c171bf8b4
Parent:
0:0a171d2a8dc1
Child:
2:b9a7fafd233c
V6

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouvdberg 0:0a171d2a8dc1 1 #include "mbed.h"
bouvdberg 0:0a171d2a8dc1 2 #include "TextLCD.h"
bouvdberg 0:0a171d2a8dc1 3 #include "MODSERIAL.h"
bouvdberg 0:0a171d2a8dc1 4 #include "encoder.h"
bouvdberg 0:0a171d2a8dc1 5
bouvdberg 0:0a171d2a8dc1 6 // definieren constanten
bouvdberg 0:0a171d2a8dc1 7 #define PI 3.141593
bouvdberg 0:0a171d2a8dc1 8 //plant
bouvdberg 0:0a171d2a8dc1 9 #define ARM1 0.36
bouvdberg 0:0a171d2a8dc1 10 #define ARM2 0.26
bouvdberg 0:0a171d2a8dc1 11 //PD
bouvdberg 0:0a171d2a8dc1 12 //#define CI 0.01
bouvdberg 0:0a171d2a8dc1 13 #define CP1 0.01
bouvdberg 0:0a171d2a8dc1 14 #define CP2 0.01
bouvdberg 0:0a171d2a8dc1 15 //#define CD 0.000
bouvdberg 0:0a171d2a8dc1 16 //#define CLP1 0.9975
bouvdberg 0:0a171d2a8dc1 17 ///#define CLP2 0.001
bouvdberg 0:0a171d2a8dc1 18 //Snelheid
bouvdberg 0:0a171d2a8dc1 19 #define SNELHEID 0.01
bouvdberg 0:0a171d2a8dc1 20 //LOOPTIME
bouvdberg 0:0a171d2a8dc1 21 #define LOOPTIME 0.006667
bouvdberg 0:0a171d2a8dc1 22 //Filtering EMG
bouvdberg 0:0a171d2a8dc1 23 #define HP1 0.8752
bouvdberg 0:0a171d2a8dc1 24 #define HP2 20.0
bouvdberg 0:0a171d2a8dc1 25 #define HP3 20.0
bouvdberg 0:0a171d2a8dc1 26 #define LP1 0.9868
bouvdberg 0:0a171d2a8dc1 27 #define LP2 0.01325
bouvdberg 0:0a171d2a8dc1 28 //EMG threshold
bouvdberg 1:c18c171bf8b4 29 #define SET_EMG_MAX1 3.8 //bovenarm rechts > beweging naar rechts
bouvdberg 1:c18c171bf8b4 30 #define SET_EMG_MIN1 2.3
bouvdberg 1:c18c171bf8b4 31 #define SET_EMG_MAX2 3.8 //bovenarm links > beweging naar links
bouvdberg 1:c18c171bf8b4 32 #define SET_EMG_MIN2 0.7
bouvdberg 1:c18c171bf8b4 33 #define SET_EMG_MAX3 7.2 //onderarm rechts > beweging naar boven
bouvdberg 1:c18c171bf8b4 34 #define SET_EMG_MIN3 2.6
bouvdberg 1:c18c171bf8b4 35 #define SET_EMG_MAX4 1.8 //onderarm links > beweging naar onder
bouvdberg 1:c18c171bf8b4 36 #define SET_EMG_MIN4 0.8
bouvdberg 0:0a171d2a8dc1 37
bouvdberg 0:0a171d2a8dc1 38 void aansturing(void);
bouvdberg 0:0a171d2a8dc1 39 void uitzetten(void);
bouvdberg 0:0a171d2a8dc1 40 void setlooptimerflag(void);
bouvdberg 0:0a171d2a8dc1 41 void keep_in_range(float * in, float min, float max);
bouvdberg 0:0a171d2a8dc1 42
bouvdberg 0:0a171d2a8dc1 43 volatile bool looptimerflag;
bouvdberg 0:0a171d2a8dc1 44
bouvdberg 0:0a171d2a8dc1 45 Serial pc(USBTX, USBRX);
bouvdberg 0:0a171d2a8dc1 46 TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2,NC,NC,TextLCD::HD44780); // rs, e, d4-d7-/*+-9
bouvdberg 0:0a171d2a8dc1 47 AnalogIn EMG1(PTB0); //EMG
bouvdberg 0:0a171d2a8dc1 48 AnalogIn EMG2(PTB1);
bouvdberg 0:0a171d2a8dc1 49 AnalogIn EMG3(PTB2);
bouvdberg 0:0a171d2a8dc1 50 AnalogIn EMG4(PTB3);
bouvdberg 0:0a171d2a8dc1 51 AnalogIn potmeter(PTC2); //potmeter
bouvdberg 0:0a171d2a8dc1 52 DigitalIn ButtonSTOP(PTE21); //Knopjes voor kalibratie
bouvdberg 0:0a171d2a8dc1 53 DigitalIn ButtonSELECT(PTE20);
bouvdberg 0:0a171d2a8dc1 54 DigitalIn ButtonUP(PTE23);
bouvdberg 0:0a171d2a8dc1 55 DigitalIn ButtonDOWN(PTE22);
bouvdberg 0:0a171d2a8dc1 56 DigitalOut Solenoid(PTD4); //Solenoid
bouvdberg 0:0a171d2a8dc1 57 Encoder motor1(PTD0,PTC8); //Encoder
bouvdberg 0:0a171d2a8dc1 58 Encoder motor2(PTD2,PTC9);
bouvdberg 0:0a171d2a8dc1 59 PwmOut pwm_motor1(PTA12); //motor
bouvdberg 0:0a171d2a8dc1 60 DigitalOut motordir1(PTD3);
bouvdberg 0:0a171d2a8dc1 61 PwmOut pwm_motor2(PTA5);
bouvdberg 0:0a171d2a8dc1 62 DigitalOut motordir2(PTD1);
bouvdberg 0:0a171d2a8dc1 63 DigitalOut Brake1(PTD5);
bouvdberg 0:0a171d2a8dc1 64 DigitalOut Brake2(PTA13);
bouvdberg 0:0a171d2a8dc1 65
bouvdberg 0:0a171d2a8dc1 66 float numberx = 9;
bouvdberg 0:0a171d2a8dc1 67 int menu=0, t;
bouvdberg 0:0a171d2a8dc1 68 float EMGmax1=SET_EMG_MAX1, EMGmin1=SET_EMG_MIN1, EMGmax2=SET_EMG_MAX2, EMGmin2=SET_EMG_MIN2;
bouvdberg 0:0a171d2a8dc1 69 float EMGmax3=SET_EMG_MAX3, EMGmin3=SET_EMG_MIN3, EMGmax4=SET_EMG_MAX4, EMGmin4=SET_EMG_MIN4;
bouvdberg 0:0a171d2a8dc1 70 float drawspeed=SNELHEID;
bouvdberg 0:0a171d2a8dc1 71
bouvdberg 0:0a171d2a8dc1 72
bouvdberg 0:0a171d2a8dc1 73 //Variabelen verwerking EMG
bouvdberg 0:0a171d2a8dc1 74 float emg_value1, emg_value2, emg_value3, emg_value4;
bouvdberg 0:0a171d2a8dc1 75 float emg_value1min1=0.5, emg_value2min1=0.5, emg_value3min1=0.5, emg_value4min1=0.5;
bouvdberg 0:0a171d2a8dc1 76 float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4;
bouvdberg 0:0a171d2a8dc1 77 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;
bouvdberg 0:0a171d2a8dc1 78
bouvdberg 0:0a171d2a8dc1 79 //Variabelen bepaling input systeem
bouvdberg 0:0a171d2a8dc1 80 float input;
bouvdberg 0:0a171d2a8dc1 81 float w1, w2, wM2, phi1, phi2, theta;
bouvdberg 0:0a171d2a8dc1 82 float a, b, c, d, ai, bi, ci, di;
bouvdberg 0:0a171d2a8dc1 83 float v1, v2, v3, v4, vx, vy, snelheid;
bouvdberg 0:0a171d2a8dc1 84 float M1position, M2position, M2phi;
bouvdberg 0:0a171d2a8dc1 85 float Px, Py;
bouvdberg 0:0a171d2a8dc1 86
bouvdberg 0:0a171d2a8dc1 87 //Variabelen motoraansturing
bouvdberg 0:0a171d2a8dc1 88 float setpointM1=800.0, setpointM2=2400.0;
bouvdberg 0:0a171d2a8dc1 89 float setpointmin1M1=800.0, setpointmin1M2=2400.0;
bouvdberg 0:0a171d2a8dc1 90 float pwm_to_motor1, pwm_to_motor2;
bouvdberg 0:0a171d2a8dc1 91 float foutM1, foutM2;
bouvdberg 0:0a171d2a8dc1 92 //float foutmin1M1=0.0, foutmin1M2=0.0;
bouvdberg 0:0a171d2a8dc1 93 //float foutverschilM1, foutverschilM2;
bouvdberg 0:0a171d2a8dc1 94 //float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0;
bouvdberg 0:0a171d2a8dc1 95 //float foutImin1=0.0, foutImin2=0.0, foutI1, foutI2;
bouvdberg 0:0a171d2a8dc1 96 //float CDloop=CD/LOOPTIME;
bouvdberg 0:0a171d2a8dc1 97 //float t_sin=0.0;
bouvdberg 0:0a171d2a8dc1 98 //float t_timer=0.0;
bouvdberg 0:0a171d2a8dc1 99 int sol_updown=0;
bouvdberg 1:c18c171bf8b4 100 int t_sol=0;
bouvdberg 0:0a171d2a8dc1 101
bouvdberg 0:0a171d2a8dc1 102 int main() {
bouvdberg 0:0a171d2a8dc1 103 //set buttons PULLDOWN
bouvdberg 0:0a171d2a8dc1 104 ButtonSTOP.mode(PullNone);
bouvdberg 0:0a171d2a8dc1 105 ButtonSELECT.mode(PullNone);
bouvdberg 0:0a171d2a8dc1 106 ButtonUP.mode(PullNone);
bouvdberg 0:0a171d2a8dc1 107 ButtonDOWN.mode(PullNone);
bouvdberg 0:0a171d2a8dc1 108 pc.baud(57600);
bouvdberg 0:0a171d2a8dc1 109 //Aanstuur timing
bouvdberg 0:0a171d2a8dc1 110 Ticker looptimer;
bouvdberg 0:0a171d2a8dc1 111 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:0a171d2a8dc1 112 while(1)
bouvdberg 0:0a171d2a8dc1 113 {
bouvdberg 0:0a171d2a8dc1 114 switch (menu)
bouvdberg 0:0a171d2a8dc1 115 {
bouvdberg 0:0a171d2a8dc1 116 case 0:
bouvdberg 0:0a171d2a8dc1 117 lcd.cls();
bouvdberg 0:0a171d2a8dc1 118 lcd.printf("> CALIBRATION");
bouvdberg 0:0a171d2a8dc1 119 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 120 lcd.printf(" DRAW");
bouvdberg 0:0a171d2a8dc1 121 while(menu==0)
bouvdberg 0:0a171d2a8dc1 122 {
bouvdberg 0:0a171d2a8dc1 123 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 124 if (ButtonSELECT.read()==1) menu=70;
bouvdberg 0:0a171d2a8dc1 125 }
bouvdberg 0:0a171d2a8dc1 126 break;
bouvdberg 0:0a171d2a8dc1 127 case 1:
bouvdberg 0:0a171d2a8dc1 128 lcd.cls();
bouvdberg 0:0a171d2a8dc1 129 lcd.printf("> DRAW");
bouvdberg 0:0a171d2a8dc1 130 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 131 lcd.printf(" SETTINGS");
bouvdberg 0:0a171d2a8dc1 132 while(menu==1)
bouvdberg 0:0a171d2a8dc1 133 {
bouvdberg 0:0a171d2a8dc1 134 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 135 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 136 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 137 {
bouvdberg 0:0a171d2a8dc1 138 motor1.setPosition(800);
bouvdberg 0:0a171d2a8dc1 139 motor2.setPosition(2400);
bouvdberg 0:0a171d2a8dc1 140 menu=55;
bouvdberg 0:0a171d2a8dc1 141 lcd.cls();
bouvdberg 1:c18c171bf8b4 142 lcd.printf("UP: Pause");
bouvdberg 1:c18c171bf8b4 143 lcd.locate(0,1);
bouvdberg 1:c18c171bf8b4 144 lcd.printf("DOWN: Up/Down");
bouvdberg 1:c18c171bf8b4 145 Solenoid=1;
bouvdberg 1:c18c171bf8b4 146 sol_updown=0;
bouvdberg 0:0a171d2a8dc1 147 //wait(0.01);
bouvdberg 0:0a171d2a8dc1 148 }
bouvdberg 0:0a171d2a8dc1 149 }
bouvdberg 0:0a171d2a8dc1 150 break;
bouvdberg 0:0a171d2a8dc1 151 case 2:
bouvdberg 0:0a171d2a8dc1 152 lcd.cls();
bouvdberg 0:0a171d2a8dc1 153 lcd.printf("> SETTINGS");
bouvdberg 0:0a171d2a8dc1 154 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 155 lcd.printf(" RESET ALL");
bouvdberg 0:0a171d2a8dc1 156 while(menu==2)
bouvdberg 0:0a171d2a8dc1 157 {
bouvdberg 0:0a171d2a8dc1 158 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 159 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 160 if (ButtonSELECT.read()==1) menu=20;
bouvdberg 0:0a171d2a8dc1 161 }
bouvdberg 0:0a171d2a8dc1 162 break;
bouvdberg 0:0a171d2a8dc1 163 case 3:
bouvdberg 0:0a171d2a8dc1 164 lcd.cls();
bouvdberg 0:0a171d2a8dc1 165 lcd.printf("> RESET ALL");
bouvdberg 0:0a171d2a8dc1 166 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 167 lcd.printf(" ");
bouvdberg 0:0a171d2a8dc1 168 while(menu==3)
bouvdberg 0:0a171d2a8dc1 169 {
bouvdberg 0:0a171d2a8dc1 170 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 171 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 172 {
bouvdberg 0:0a171d2a8dc1 173 EMGmax1=SET_EMG_MAX1; EMGmin1=SET_EMG_MIN1;
bouvdberg 0:0a171d2a8dc1 174 EMGmax2=SET_EMG_MAX2; EMGmin2=SET_EMG_MIN2;
bouvdberg 0:0a171d2a8dc1 175 EMGmax3=SET_EMG_MAX3; EMGmin3=SET_EMG_MIN3;
bouvdberg 0:0a171d2a8dc1 176 EMGmax4=SET_EMG_MAX4; EMGmin4=SET_EMG_MIN4;
bouvdberg 0:0a171d2a8dc1 177 drawspeed=SNELHEID;
bouvdberg 0:0a171d2a8dc1 178 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 179 lcd.printf(" Reset Completed");
bouvdberg 0:0a171d2a8dc1 180 wait(1);
bouvdberg 0:0a171d2a8dc1 181 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 182 lcd.printf(" ");
bouvdberg 0:0a171d2a8dc1 183 }
bouvdberg 0:0a171d2a8dc1 184 }
bouvdberg 0:0a171d2a8dc1 185 break;
bouvdberg 0:0a171d2a8dc1 186 case 20:
bouvdberg 0:0a171d2a8dc1 187 lcd.cls();
bouvdberg 0:0a171d2a8dc1 188 lcd.printf("> EMG1-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:0a171d2a8dc1 189 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 190 lcd.printf(" EMG1-MIN: ");
bouvdberg 0:0a171d2a8dc1 191 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 192 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 193 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 194 {
bouvdberg 0:0a171d2a8dc1 195 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 196 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 197 EMGmax1=(EMGmax1+(((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:0a171d2a8dc1 198 wait(0.5);
bouvdberg 0:0a171d2a8dc1 199 }
bouvdberg 0:0a171d2a8dc1 200 break;
bouvdberg 0:0a171d2a8dc1 201 case 21:
bouvdberg 0:0a171d2a8dc1 202 lcd.cls();
bouvdberg 0:0a171d2a8dc1 203 lcd.printf("> EMG1-MIN: %.2f", (EMGmin1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:0a171d2a8dc1 204 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 205 lcd.printf(" EMG2-MAX: ");
bouvdberg 0:0a171d2a8dc1 206 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 207 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 208 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 209 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 210 {
bouvdberg 0:0a171d2a8dc1 211 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 212 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 213 EMGmin1=(EMGmin1+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:0a171d2a8dc1 214 wait(0.5);
bouvdberg 0:0a171d2a8dc1 215 }
bouvdberg 0:0a171d2a8dc1 216 break;
bouvdberg 0:0a171d2a8dc1 217 case 22:
bouvdberg 0:0a171d2a8dc1 218 lcd.cls();
bouvdberg 0:0a171d2a8dc1 219 lcd.printf("> EMG2-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:0a171d2a8dc1 220 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 221 lcd.printf(" EMG2-MIN: ");
bouvdberg 0:0a171d2a8dc1 222 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 223 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 224 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 225 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 226 {
bouvdberg 0:0a171d2a8dc1 227 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 228 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 229 EMGmax2=(EMGmax1+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:0a171d2a8dc1 230 wait(0.5);
bouvdberg 0:0a171d2a8dc1 231 }
bouvdberg 0:0a171d2a8dc1 232 break;
bouvdberg 0:0a171d2a8dc1 233 case 23:
bouvdberg 0:0a171d2a8dc1 234 lcd.cls();
bouvdberg 0:0a171d2a8dc1 235 lcd.printf("> EMG2-MIN: %.2f", (EMGmin2+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:0a171d2a8dc1 236 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 237 lcd.printf(" EMG3-MAX: ");
bouvdberg 0:0a171d2a8dc1 238 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 239 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 240 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 241 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 242 {
bouvdberg 0:0a171d2a8dc1 243 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 244 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 245 EMGmin2=(EMGmin2+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:0a171d2a8dc1 246 wait(0.5);
bouvdberg 0:0a171d2a8dc1 247 }
bouvdberg 0:0a171d2a8dc1 248 break;
bouvdberg 0:0a171d2a8dc1 249 case 24:
bouvdberg 0:0a171d2a8dc1 250 lcd.cls();
bouvdberg 0:0a171d2a8dc1 251 lcd.printf("> EMG3-MAX: %.2f", (EMGmax3+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:0a171d2a8dc1 252 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 253 lcd.printf(" EMG3-MIN: ");
bouvdberg 0:0a171d2a8dc1 254 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 255 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 256 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 257 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 258 {
bouvdberg 0:0a171d2a8dc1 259 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 260 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 261 EMGmax3=(EMGmax3+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:0a171d2a8dc1 262 wait(0.5);
bouvdberg 0:0a171d2a8dc1 263 }
bouvdberg 0:0a171d2a8dc1 264 break;
bouvdberg 0:0a171d2a8dc1 265 case 25:
bouvdberg 0:0a171d2a8dc1 266 lcd.cls();
bouvdberg 0:0a171d2a8dc1 267 lcd.printf("> EMG3-MIN: %.2f", (EMGmin3+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:0a171d2a8dc1 268 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 269 lcd.printf(" EMG4-MAX: ");
bouvdberg 0:0a171d2a8dc1 270 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 271 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 272 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 273 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 274 {
bouvdberg 0:0a171d2a8dc1 275 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 276 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 277 EMGmin3=(EMGmin3+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:0a171d2a8dc1 278 wait(0.5);
bouvdberg 0:0a171d2a8dc1 279 }
bouvdberg 0:0a171d2a8dc1 280 break;
bouvdberg 0:0a171d2a8dc1 281 case 26:
bouvdberg 0:0a171d2a8dc1 282 lcd.cls();
bouvdberg 0:0a171d2a8dc1 283 lcd.printf("> EMG4-MAX: %.2f", (EMGmax4+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:0a171d2a8dc1 284 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 285 lcd.printf(" EMG4-MIN: ");
bouvdberg 0:0a171d2a8dc1 286 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 287 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 288 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 289 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 290 {
bouvdberg 0:0a171d2a8dc1 291 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 292 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 293 EMGmax4=(EMGmax4+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:0a171d2a8dc1 294 wait(0.5);
bouvdberg 0:0a171d2a8dc1 295 }
bouvdberg 0:0a171d2a8dc1 296 break;
bouvdberg 0:0a171d2a8dc1 297 case 27:
bouvdberg 0:0a171d2a8dc1 298 lcd.cls();
bouvdberg 0:0a171d2a8dc1 299 lcd.printf("> EMG4-MIN: %.2f", (EMGmin4+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:0a171d2a8dc1 300 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 301 lcd.printf(" SPEED :");
bouvdberg 0:0a171d2a8dc1 302 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 303 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 304 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 305 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 306 {
bouvdberg 0:0a171d2a8dc1 307 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 308 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 309 EMGmin4=(EMGmin4+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:0a171d2a8dc1 310 wait(0.5);
bouvdberg 0:0a171d2a8dc1 311 }
bouvdberg 0:0a171d2a8dc1 312 break;
bouvdberg 0:0a171d2a8dc1 313 case 28:
bouvdberg 0:0a171d2a8dc1 314 lcd.cls();
bouvdberg 0:0a171d2a8dc1 315 lcd.printf("> SPEED : %.2f", (drawspeed+ (((potmeter.read()+0.0005)/10)-0.05)));
bouvdberg 0:0a171d2a8dc1 316 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 317 lcd.printf(" SOLENOID:");
bouvdberg 0:0a171d2a8dc1 318 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 319 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 320 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 321 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 322 {
bouvdberg 0:0a171d2a8dc1 323 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 324 lcd.printf(" SAVED! ");
bouvdberg 0:0a171d2a8dc1 325 drawspeed=(drawspeed+ (((potmeter.read()+0.0005)/10)-0.05));
bouvdberg 0:0a171d2a8dc1 326 wait(0.5);
bouvdberg 0:0a171d2a8dc1 327 }
bouvdberg 0:0a171d2a8dc1 328 break;
bouvdberg 0:0a171d2a8dc1 329 case 29:
bouvdberg 0:0a171d2a8dc1 330 lcd.cls();
bouvdberg 0:0a171d2a8dc1 331 lcd.printf("> SOLENOID: OFF");
bouvdberg 0:0a171d2a8dc1 332 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 333 lcd.printf(" ");
bouvdberg 0:0a171d2a8dc1 334 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:0a171d2a8dc1 335 if (ButtonUP.read()==1) menu--;
bouvdberg 0:0a171d2a8dc1 336 //if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:0a171d2a8dc1 337 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 338 {
bouvdberg 0:0a171d2a8dc1 339 lcd.cls();
bouvdberg 0:0a171d2a8dc1 340 lcd.printf("> SOLENOID: ON");
bouvdberg 0:0a171d2a8dc1 341 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 342 lcd.printf(" ");
bouvdberg 0:0a171d2a8dc1 343 Solenoid=1;
bouvdberg 1:c18c171bf8b4 344 sol_updown=1;
bouvdberg 0:0a171d2a8dc1 345 wait(1);
bouvdberg 0:0a171d2a8dc1 346 Solenoid=0;
bouvdberg 1:c18c171bf8b4 347 sol_updown=0;
bouvdberg 0:0a171d2a8dc1 348 }
bouvdberg 0:0a171d2a8dc1 349 break;
bouvdberg 0:0a171d2a8dc1 350 case 50: //tekenen afsluiten
bouvdberg 0:0a171d2a8dc1 351 lcd.cls();
bouvdberg 0:0a171d2a8dc1 352 lcd.printf(" Shutting Down!");
bouvdberg 0:0a171d2a8dc1 353 menu=0;
bouvdberg 0:0a171d2a8dc1 354
bouvdberg 0:0a171d2a8dc1 355 uitzetten();
bouvdberg 0:0a171d2a8dc1 356
bouvdberg 0:0a171d2a8dc1 357 break;
bouvdberg 0:0a171d2a8dc1 358 case 55: //DRAWING
bouvdberg 0:0a171d2a8dc1 359
bouvdberg 0:0a171d2a8dc1 360 if (ButtonSTOP.read()==1) menu=50;
bouvdberg 0:0a171d2a8dc1 361 if (ButtonUP.read()==1) menu++;
bouvdberg 1:c18c171bf8b4 362 if (ButtonDOWN.read()==1) //Misschien in de loop van 'aansturing' zetten????????????????????
bouvdberg 0:0a171d2a8dc1 363 {
bouvdberg 1:c18c171bf8b4 364 t_sol++;
bouvdberg 1:c18c171bf8b4 365 if (t_sol>10)
bouvdberg 0:0a171d2a8dc1 366 {
bouvdberg 1:c18c171bf8b4 367 if (sol_updown==0)
bouvdberg 1:c18c171bf8b4 368 {
bouvdberg 1:c18c171bf8b4 369 sol_updown=1;
bouvdberg 1:c18c171bf8b4 370 Solenoid=1;
bouvdberg 1:c18c171bf8b4 371 }
bouvdberg 1:c18c171bf8b4 372 else
bouvdberg 1:c18c171bf8b4 373 {
bouvdberg 1:c18c171bf8b4 374 sol_updown=0;
bouvdberg 1:c18c171bf8b4 375 Solenoid=0;
bouvdberg 1:c18c171bf8b4 376 }
bouvdberg 1:c18c171bf8b4 377 t_sol=0;
bouvdberg 0:0a171d2a8dc1 378 }
bouvdberg 0:0a171d2a8dc1 379 }
bouvdberg 0:0a171d2a8dc1 380
bouvdberg 0:0a171d2a8dc1 381 aansturing(); //aansturing
bouvdberg 0:0a171d2a8dc1 382
bouvdberg 0:0a171d2a8dc1 383 break;
bouvdberg 0:0a171d2a8dc1 384 case 56: //PAUSE
bouvdberg 0:0a171d2a8dc1 385 wait(0.2);
bouvdberg 0:0a171d2a8dc1 386 lcd.cls();
bouvdberg 0:0a171d2a8dc1 387 lcd.printf(" PAUSE ");
bouvdberg 0:0a171d2a8dc1 388 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 389 lcd.printf("> RESUME");
bouvdberg 0:0a171d2a8dc1 390
bouvdberg 0:0a171d2a8dc1 391 Solenoid=1;
bouvdberg 1:c18c171bf8b4 392 sol_updown=1;
bouvdberg 0:0a171d2a8dc1 393 pwm_motor1.write(0);
bouvdberg 0:0a171d2a8dc1 394 pwm_motor2.write(0);
bouvdberg 0:0a171d2a8dc1 395
bouvdberg 0:0a171d2a8dc1 396 while(ButtonSELECT.read()==1);
bouvdberg 0:0a171d2a8dc1 397 while(menu==56)
bouvdberg 0:0a171d2a8dc1 398 {
bouvdberg 0:0a171d2a8dc1 399 if (ButtonSTOP.read()==1) menu=50;
bouvdberg 0:0a171d2a8dc1 400 if (ButtonSELECT.read()==1)
bouvdberg 0:0a171d2a8dc1 401 {
bouvdberg 0:0a171d2a8dc1 402 menu--;
bouvdberg 0:0a171d2a8dc1 403 lcd.cls();
bouvdberg 0:0a171d2a8dc1 404 lcd.printf("UP: Pause");
bouvdberg 0:0a171d2a8dc1 405 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 406 lcd.printf("DOWN: Up/Down");
bouvdberg 0:0a171d2a8dc1 407 }
bouvdberg 0:0a171d2a8dc1 408 }
bouvdberg 0:0a171d2a8dc1 409
bouvdberg 0:0a171d2a8dc1 410
bouvdberg 0:0a171d2a8dc1 411
bouvdberg 0:0a171d2a8dc1 412 break;
bouvdberg 0:0a171d2a8dc1 413 case 70: //Calibration starting
bouvdberg 0:0a171d2a8dc1 414 wait(0.2);
bouvdberg 0:0a171d2a8dc1 415 lcd.cls();
bouvdberg 0:0a171d2a8dc1 416 lcd.printf("Calibration ...");
bouvdberg 0:0a171d2a8dc1 417 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 418 lcd.printf("start idle in: 3");
bouvdberg 0:0a171d2a8dc1 419 wait(1);
bouvdberg 0:0a171d2a8dc1 420 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 421 lcd.printf("start idle in: 2");
bouvdberg 0:0a171d2a8dc1 422 wait(1);
bouvdberg 0:0a171d2a8dc1 423 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 424 lcd.printf("start idle in: 1");
bouvdberg 0:0a171d2a8dc1 425 wait(1);
bouvdberg 0:0a171d2a8dc1 426 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 427 lcd.printf("start idle NOW");
bouvdberg 0:0a171d2a8dc1 428
bouvdberg 0:0a171d2a8dc1 429 menu++;
bouvdberg 0:0a171d2a8dc1 430
bouvdberg 0:0a171d2a8dc1 431 break;
bouvdberg 0:0a171d2a8dc1 432 case 71: //Calibration rust
bouvdberg 0:0a171d2a8dc1 433 lcd.cls();
bouvdberg 0:0a171d2a8dc1 434 lcd.printf("Calibration idle");
bouvdberg 0:0a171d2a8dc1 435
bouvdberg 0:0a171d2a8dc1 436 float CAL_EMG1_MAX=0;
bouvdberg 0:0a171d2a8dc1 437 float CAL_EMG2_MAX=0;
bouvdberg 0:0a171d2a8dc1 438 float CAL_EMG3_MAX=0;
bouvdberg 0:0a171d2a8dc1 439 float CAL_EMG4_MAX=0;
bouvdberg 0:0a171d2a8dc1 440
bouvdberg 0:0a171d2a8dc1 441 //calc idle
bouvdberg 0:0a171d2a8dc1 442 for (int c=1000; c>=0; c--)
bouvdberg 0:0a171d2a8dc1 443 {
bouvdberg 0:0a171d2a8dc1 444 Ticker looptimer;
bouvdberg 0:0a171d2a8dc1 445 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:0a171d2a8dc1 446 while(looptimerflag != true);
bouvdberg 0:0a171d2a8dc1 447 looptimerflag = false;
bouvdberg 0:0a171d2a8dc1 448 emg_value1=EMG1.read();
bouvdberg 0:0a171d2a8dc1 449 EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
bouvdberg 0:0a171d2a8dc1 450 EMGhp1=abs(EMGhp1);
bouvdberg 0:0a171d2a8dc1 451 EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
bouvdberg 0:0a171d2a8dc1 452 EMGhp1min1=EMGhp1;
bouvdberg 0:0a171d2a8dc1 453 emg_value1min1=emg_value1;
bouvdberg 0:0a171d2a8dc1 454 EMGlp1min1=EMGlp1;
bouvdberg 0:0a171d2a8dc1 455 if (EMGlp1 > CAL_EMG1_MAX) CAL_EMG1_MAX=EMGlp1;
bouvdberg 0:0a171d2a8dc1 456
bouvdberg 0:0a171d2a8dc1 457 emg_value2=EMG2.read();
bouvdberg 0:0a171d2a8dc1 458 EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
bouvdberg 0:0a171d2a8dc1 459 EMGhp2=abs(EMGhp2);
bouvdberg 0:0a171d2a8dc1 460 EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
bouvdberg 0:0a171d2a8dc1 461 EMGhp2min1=EMGhp2;
bouvdberg 0:0a171d2a8dc1 462 emg_value2min1=emg_value2;
bouvdberg 0:0a171d2a8dc1 463 EMGlp2min1=EMGlp2;
bouvdberg 0:0a171d2a8dc1 464 if (EMGlp2 > CAL_EMG2_MAX) CAL_EMG2_MAX=EMGlp2;
bouvdberg 0:0a171d2a8dc1 465
bouvdberg 0:0a171d2a8dc1 466 emg_value3=EMG3.read();
bouvdberg 0:0a171d2a8dc1 467 EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
bouvdberg 0:0a171d2a8dc1 468 EMGhp3=abs(EMGhp3);
bouvdberg 0:0a171d2a8dc1 469 EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
bouvdberg 0:0a171d2a8dc1 470 EMGhp3min1=EMGhp3;
bouvdberg 0:0a171d2a8dc1 471 emg_value3min1=emg_value3;
bouvdberg 0:0a171d2a8dc1 472 EMGlp3min1=EMGlp3;
bouvdberg 0:0a171d2a8dc1 473 if (EMGlp3 > CAL_EMG3_MAX) CAL_EMG3_MAX=EMGlp3;
bouvdberg 0:0a171d2a8dc1 474
bouvdberg 0:0a171d2a8dc1 475 emg_value4=EMG4.read();
bouvdberg 0:0a171d2a8dc1 476 EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
bouvdberg 0:0a171d2a8dc1 477 EMGhp4=abs(EMGhp4);
bouvdberg 0:0a171d2a8dc1 478 EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
bouvdberg 0:0a171d2a8dc1 479 EMGhp4min1=EMGhp4;
bouvdberg 0:0a171d2a8dc1 480 emg_value4min1=emg_value4;
bouvdberg 0:0a171d2a8dc1 481 EMGlp4min1=EMGlp4;
bouvdberg 0:0a171d2a8dc1 482 if (EMGlp4 > CAL_EMG4_MAX) CAL_EMG4_MAX=EMGlp4;
bouvdberg 0:0a171d2a8dc1 483
bouvdberg 0:0a171d2a8dc1 484 }
bouvdberg 0:0a171d2a8dc1 485 EMGmin1= CAL_EMG1_MAX+0.15;
bouvdberg 0:0a171d2a8dc1 486 EMGmin2= CAL_EMG2_MAX+0.15;
bouvdberg 0:0a171d2a8dc1 487 EMGmin3= CAL_EMG3_MAX+0.15;
bouvdberg 0:0a171d2a8dc1 488 EMGmin4= CAL_EMG4_MAX+0.15;
bouvdberg 0:0a171d2a8dc1 489 emg_value1min1=0.5;
bouvdberg 0:0a171d2a8dc1 490 emg_value2min1=0.5;
bouvdberg 0:0a171d2a8dc1 491 emg_value3min1=0.5;
bouvdberg 0:0a171d2a8dc1 492 emg_value4min1=0.5;
bouvdberg 0:0a171d2a8dc1 493 EMGhp1min1=0.5;
bouvdberg 0:0a171d2a8dc1 494 EMGhp2min1=0.5;
bouvdberg 0:0a171d2a8dc1 495 EMGhp3min1=0.5;
bouvdberg 0:0a171d2a8dc1 496 EMGhp4min1=0.5;
bouvdberg 0:0a171d2a8dc1 497 EMGlp1min1=0.5;
bouvdberg 0:0a171d2a8dc1 498 EMGlp2min1=0.5;
bouvdberg 0:0a171d2a8dc1 499 EMGlp3min1=0.5;
bouvdberg 0:0a171d2a8dc1 500 EMGlp4min1=0.5;
bouvdberg 0:0a171d2a8dc1 501
bouvdberg 0:0a171d2a8dc1 502 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 503 lcd.printf("Next EMG 1 in: 3");
bouvdberg 0:0a171d2a8dc1 504 wait(1);
bouvdberg 0:0a171d2a8dc1 505 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 506 lcd.printf("Next EMG 1 in: 2") ;
bouvdberg 0:0a171d2a8dc1 507 wait(1);
bouvdberg 0:0a171d2a8dc1 508 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 509 lcd.printf("Next EMG 1 in: 1");
bouvdberg 0:0a171d2a8dc1 510 wait(1);
bouvdberg 0:0a171d2a8dc1 511 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 512 lcd.printf("Next EMG 1 NOW");
bouvdberg 0:0a171d2a8dc1 513
bouvdberg 0:0a171d2a8dc1 514 menu++;
bouvdberg 0:0a171d2a8dc1 515
bouvdberg 0:0a171d2a8dc1 516 break;
bouvdberg 0:0a171d2a8dc1 517 case 72: //Calibration EMG1
bouvdberg 0:0a171d2a8dc1 518 lcd.cls();
bouvdberg 0:0a171d2a8dc1 519 lcd.printf("Calibration EMG1");
bouvdberg 0:0a171d2a8dc1 520
bouvdberg 0:0a171d2a8dc1 521 //calc EMG1 MAX
bouvdberg 0:0a171d2a8dc1 522 CAL_EMG1_MAX=0;
bouvdberg 0:0a171d2a8dc1 523 for (int c=1000; c>=0; c--)
bouvdberg 0:0a171d2a8dc1 524 {
bouvdberg 0:0a171d2a8dc1 525 Ticker looptimer;
bouvdberg 0:0a171d2a8dc1 526 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:0a171d2a8dc1 527 while(looptimerflag != true);
bouvdberg 0:0a171d2a8dc1 528 looptimerflag = false;
bouvdberg 0:0a171d2a8dc1 529 emg_value1=EMG1.read();
bouvdberg 0:0a171d2a8dc1 530 EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
bouvdberg 0:0a171d2a8dc1 531 EMGhp1=abs(EMGhp1);
bouvdberg 0:0a171d2a8dc1 532 EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
bouvdberg 0:0a171d2a8dc1 533 EMGhp1min1=EMGhp1;
bouvdberg 0:0a171d2a8dc1 534 emg_value1min1=emg_value1;
bouvdberg 0:0a171d2a8dc1 535 EMGlp1min1=EMGlp1;
bouvdberg 0:0a171d2a8dc1 536
bouvdberg 0:0a171d2a8dc1 537 if (EMGlp1 > CAL_EMG1_MAX) CAL_EMG1_MAX=EMGlp1;
bouvdberg 0:0a171d2a8dc1 538 }
bouvdberg 0:0a171d2a8dc1 539 EMGmax1= CAL_EMG1_MAX+1.0;
bouvdberg 0:0a171d2a8dc1 540 emg_value1min1=0.5;
bouvdberg 0:0a171d2a8dc1 541 EMGhp1min1=0.5;
bouvdberg 0:0a171d2a8dc1 542 EMGlp1min1=0.5;
bouvdberg 0:0a171d2a8dc1 543
bouvdberg 0:0a171d2a8dc1 544 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 545 lcd.printf("Next EMG 2 in: 3");
bouvdberg 0:0a171d2a8dc1 546 wait(1);
bouvdberg 0:0a171d2a8dc1 547 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 548 lcd.printf("Next EMG 2 in: 2") ;
bouvdberg 0:0a171d2a8dc1 549 wait(1);
bouvdberg 0:0a171d2a8dc1 550 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 551 lcd.printf("Next EMG 2 in: 1");
bouvdberg 0:0a171d2a8dc1 552 wait(1);
bouvdberg 0:0a171d2a8dc1 553 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 554 lcd.printf("Next EMG 2 NOW");
bouvdberg 0:0a171d2a8dc1 555
bouvdberg 0:0a171d2a8dc1 556 menu++;
bouvdberg 0:0a171d2a8dc1 557
bouvdberg 0:0a171d2a8dc1 558 break;
bouvdberg 0:0a171d2a8dc1 559 case 73: //Calibration EMG2
bouvdberg 0:0a171d2a8dc1 560 lcd.cls();
bouvdberg 0:0a171d2a8dc1 561 lcd.printf("Calibration EMG2");
bouvdberg 0:0a171d2a8dc1 562
bouvdberg 0:0a171d2a8dc1 563 //calc EMG2 MAX
bouvdberg 0:0a171d2a8dc1 564 CAL_EMG2_MAX=0;
bouvdberg 0:0a171d2a8dc1 565 for (int c=1000; c>=0; c--)
bouvdberg 0:0a171d2a8dc1 566 {
bouvdberg 0:0a171d2a8dc1 567 Ticker looptimer;
bouvdberg 0:0a171d2a8dc1 568 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:0a171d2a8dc1 569 while(looptimerflag != true);
bouvdberg 0:0a171d2a8dc1 570 looptimerflag = false;
bouvdberg 0:0a171d2a8dc1 571 emg_value2=EMG2.read();
bouvdberg 0:0a171d2a8dc1 572 EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
bouvdberg 0:0a171d2a8dc1 573 EMGhp2=abs(EMGhp2);
bouvdberg 0:0a171d2a8dc1 574 EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
bouvdberg 0:0a171d2a8dc1 575 EMGhp2min1=EMGhp2;
bouvdberg 0:0a171d2a8dc1 576 emg_value2min1=emg_value2;
bouvdberg 0:0a171d2a8dc1 577 EMGlp2min1=EMGlp2;
bouvdberg 0:0a171d2a8dc1 578
bouvdberg 0:0a171d2a8dc1 579 if (EMGlp2 > CAL_EMG2_MAX) CAL_EMG2_MAX=EMGlp2;
bouvdberg 0:0a171d2a8dc1 580 }
bouvdberg 0:0a171d2a8dc1 581 EMGmax2= CAL_EMG2_MAX+1.0;
bouvdberg 0:0a171d2a8dc1 582 emg_value2min1=0.5;
bouvdberg 0:0a171d2a8dc1 583 EMGhp2min1=0.5;
bouvdberg 0:0a171d2a8dc1 584 EMGlp2min1=0.5;
bouvdberg 0:0a171d2a8dc1 585
bouvdberg 0:0a171d2a8dc1 586 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 587 lcd.printf("Next EMG 3 in: 3");
bouvdberg 0:0a171d2a8dc1 588 wait(1);
bouvdberg 0:0a171d2a8dc1 589 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 590 lcd.printf("Next EMG 3 in: 2") ;
bouvdberg 0:0a171d2a8dc1 591 wait(1);
bouvdberg 0:0a171d2a8dc1 592 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 593 lcd.printf("Next EMG 3 in: 1");
bouvdberg 0:0a171d2a8dc1 594 wait(1);
bouvdberg 0:0a171d2a8dc1 595 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 596 lcd.printf("Next EMG 3 NOW");
bouvdberg 0:0a171d2a8dc1 597
bouvdberg 0:0a171d2a8dc1 598 menu++;
bouvdberg 0:0a171d2a8dc1 599
bouvdberg 0:0a171d2a8dc1 600 break;
bouvdberg 0:0a171d2a8dc1 601 case 74: //Calibration EMG3
bouvdberg 0:0a171d2a8dc1 602 lcd.cls();
bouvdberg 0:0a171d2a8dc1 603 lcd.printf("Calibration EMG3");
bouvdberg 0:0a171d2a8dc1 604
bouvdberg 0:0a171d2a8dc1 605 //calc EMG3 MAX
bouvdberg 0:0a171d2a8dc1 606 CAL_EMG3_MAX=0;
bouvdberg 0:0a171d2a8dc1 607 for (int c=1000; c>=0; c--)
bouvdberg 0:0a171d2a8dc1 608 {
bouvdberg 0:0a171d2a8dc1 609 Ticker looptimer;
bouvdberg 0:0a171d2a8dc1 610 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:0a171d2a8dc1 611 while(looptimerflag != true);
bouvdberg 0:0a171d2a8dc1 612 looptimerflag = false;
bouvdberg 0:0a171d2a8dc1 613 emg_value3=EMG3.read();
bouvdberg 0:0a171d2a8dc1 614 EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
bouvdberg 0:0a171d2a8dc1 615 EMGhp3=abs(EMGhp3);
bouvdberg 0:0a171d2a8dc1 616 EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
bouvdberg 0:0a171d2a8dc1 617 EMGhp3min1=EMGhp3;
bouvdberg 0:0a171d2a8dc1 618 emg_value3min1=emg_value3;
bouvdberg 0:0a171d2a8dc1 619 EMGlp3min1=EMGlp3;
bouvdberg 0:0a171d2a8dc1 620
bouvdberg 0:0a171d2a8dc1 621 if (EMGlp3 > CAL_EMG3_MAX) CAL_EMG3_MAX=EMGlp3;
bouvdberg 0:0a171d2a8dc1 622 }
bouvdberg 0:0a171d2a8dc1 623 EMGmax3= CAL_EMG3_MAX+1.0;
bouvdberg 0:0a171d2a8dc1 624 emg_value3min1=0.5;
bouvdberg 0:0a171d2a8dc1 625 EMGhp3min1=0.5;
bouvdberg 0:0a171d2a8dc1 626 EMGlp3min1=0.5;
bouvdberg 0:0a171d2a8dc1 627
bouvdberg 0:0a171d2a8dc1 628 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 629 lcd.printf("Next EMG 4 in: 3");
bouvdberg 0:0a171d2a8dc1 630 wait(1);
bouvdberg 0:0a171d2a8dc1 631 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 632 lcd.printf("Next EMG 4 in: 2") ;
bouvdberg 0:0a171d2a8dc1 633 wait(1);
bouvdberg 0:0a171d2a8dc1 634 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 635 lcd.printf("Next EMG 4 in: 1");
bouvdberg 0:0a171d2a8dc1 636 wait(1);
bouvdberg 0:0a171d2a8dc1 637 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 638 lcd.printf("Next EMG 4 NOW");
bouvdberg 0:0a171d2a8dc1 639
bouvdberg 0:0a171d2a8dc1 640 menu++;
bouvdberg 0:0a171d2a8dc1 641
bouvdberg 0:0a171d2a8dc1 642 break;
bouvdberg 0:0a171d2a8dc1 643 case 75: //Calibration EMG4
bouvdberg 0:0a171d2a8dc1 644 lcd.cls();
bouvdberg 0:0a171d2a8dc1 645 lcd.printf("Calibration EMG4");
bouvdberg 0:0a171d2a8dc1 646
bouvdberg 0:0a171d2a8dc1 647 //calc EMG4 MAX
bouvdberg 0:0a171d2a8dc1 648 CAL_EMG4_MAX=0;
bouvdberg 0:0a171d2a8dc1 649 for (int c=1000; c>=0; c--)
bouvdberg 0:0a171d2a8dc1 650 {
bouvdberg 0:0a171d2a8dc1 651 Ticker looptimer;
bouvdberg 0:0a171d2a8dc1 652 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:0a171d2a8dc1 653 while(looptimerflag != true);
bouvdberg 0:0a171d2a8dc1 654 looptimerflag = false;
bouvdberg 0:0a171d2a8dc1 655 emg_value4=EMG4.read();
bouvdberg 0:0a171d2a8dc1 656 EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
bouvdberg 0:0a171d2a8dc1 657 EMGhp4=abs(EMGhp4);
bouvdberg 0:0a171d2a8dc1 658 EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
bouvdberg 0:0a171d2a8dc1 659 EMGhp4min1=EMGhp4;
bouvdberg 0:0a171d2a8dc1 660 emg_value4min1=emg_value4;
bouvdberg 0:0a171d2a8dc1 661 EMGlp4min1=EMGlp4;
bouvdberg 0:0a171d2a8dc1 662
bouvdberg 0:0a171d2a8dc1 663 if (EMGlp4 > CAL_EMG4_MAX) CAL_EMG4_MAX=EMGlp4;
bouvdberg 0:0a171d2a8dc1 664 }
bouvdberg 0:0a171d2a8dc1 665 EMGmax4= CAL_EMG4_MAX+1.0;
bouvdberg 0:0a171d2a8dc1 666 emg_value4min1=0.5;
bouvdberg 0:0a171d2a8dc1 667 EMGhp4min1=0.5;
bouvdberg 0:0a171d2a8dc1 668 EMGlp4min1=0.5;
bouvdberg 0:0a171d2a8dc1 669 if ((EMGmax1-EMGmin1)<0.8 || (EMGmax2-EMGmin2)<0.8 || (EMGmax3-EMGmin3)<0.8 || (EMGmax4-EMGmin4)<0.8)
bouvdberg 0:0a171d2a8dc1 670 {
bouvdberg 0:0a171d2a8dc1 671 lcd.cls();
bouvdberg 0:0a171d2a8dc1 672 lcd.printf("Calibration ...");
bouvdberg 0:0a171d2a8dc1 673 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 674 lcd.printf("failed! retry!");
bouvdberg 0:0a171d2a8dc1 675 wait(1);
bouvdberg 0:0a171d2a8dc1 676 menu=70;
bouvdberg 0:0a171d2a8dc1 677 }
bouvdberg 0:0a171d2a8dc1 678 else
bouvdberg 0:0a171d2a8dc1 679 {
bouvdberg 0:0a171d2a8dc1 680 lcd.cls();
bouvdberg 0:0a171d2a8dc1 681 lcd.printf("Calibration ...");
bouvdberg 0:0a171d2a8dc1 682 lcd.locate(0,1);
bouvdberg 0:0a171d2a8dc1 683 lcd.printf("Done! Data Saved!");
bouvdberg 0:0a171d2a8dc1 684 wait(1);
bouvdberg 0:0a171d2a8dc1 685
bouvdberg 0:0a171d2a8dc1 686 menu=0;
bouvdberg 0:0a171d2a8dc1 687 }
bouvdberg 0:0a171d2a8dc1 688
bouvdberg 0:0a171d2a8dc1 689 break;
bouvdberg 0:0a171d2a8dc1 690 }
bouvdberg 0:0a171d2a8dc1 691
bouvdberg 0:0a171d2a8dc1 692
bouvdberg 0:0a171d2a8dc1 693 if (menu!=55) wait(0.2);
bouvdberg 0:0a171d2a8dc1 694 }
bouvdberg 0:0a171d2a8dc1 695
bouvdberg 0:0a171d2a8dc1 696
bouvdberg 0:0a171d2a8dc1 697 }
bouvdberg 0:0a171d2a8dc1 698
bouvdberg 0:0a171d2a8dc1 699 void aansturing(void)
bouvdberg 0:0a171d2a8dc1 700 {
bouvdberg 0:0a171d2a8dc1 701 while(looptimerflag != true);
bouvdberg 0:0a171d2a8dc1 702 looptimerflag = false;
bouvdberg 0:0a171d2a8dc1 703
bouvdberg 0:0a171d2a8dc1 704 //uitlezen
bouvdberg 0:0a171d2a8dc1 705 emg_value1 = EMG1.read();
bouvdberg 0:0a171d2a8dc1 706 emg_value2 = EMG2.read();
bouvdberg 0:0a171d2a8dc1 707 emg_value3 = EMG3.read();
bouvdberg 0:0a171d2a8dc1 708 emg_value4 = EMG4.read();
bouvdberg 0:0a171d2a8dc1 709
bouvdberg 0:0a171d2a8dc1 710 //filtering
bouvdberg 0:0a171d2a8dc1 711 EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
bouvdberg 0:0a171d2a8dc1 712 EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
bouvdberg 0:0a171d2a8dc1 713 EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
bouvdberg 0:0a171d2a8dc1 714 EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
bouvdberg 0:0a171d2a8dc1 715 EMGhp1=abs(EMGhp1);
bouvdberg 0:0a171d2a8dc1 716 EMGhp2=abs(EMGhp2);
bouvdberg 0:0a171d2a8dc1 717 EMGhp3=abs(EMGhp3);
bouvdberg 0:0a171d2a8dc1 718 EMGhp4=abs(EMGhp4);
bouvdberg 0:0a171d2a8dc1 719 EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
bouvdberg 0:0a171d2a8dc1 720 EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
bouvdberg 0:0a171d2a8dc1 721 EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
bouvdberg 0:0a171d2a8dc1 722 EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
bouvdberg 0:0a171d2a8dc1 723 //pc.printf("%.2f ",emg_value1);
bouvdberg 0:0a171d2a8dc1 724 //pc.printf("%.2f ",emg_value2);
bouvdberg 0:0a171d2a8dc1 725 //pc.printf("%.2f ",emg_value3);
bouvdberg 0:0a171d2a8dc1 726 //pc.printf("%.2f ",emg_value4);
bouvdberg 0:0a171d2a8dc1 727 //pc.printf("%.2f ",EMGlp1);
bouvdberg 0:0a171d2a8dc1 728 //pc.printf("%.2f ",EMGlp2);
bouvdberg 0:0a171d2a8dc1 729 //pc.printf("%.2f ",EMGlp3);
bouvdberg 0:0a171d2a8dc1 730 //pc.printf("%.2f ",EMGlp4);
bouvdberg 0:0a171d2a8dc1 731
bouvdberg 0:0a171d2a8dc1 732 //berekenen setpoint
bouvdberg 0:0a171d2a8dc1 733 //hoekinput
bouvdberg 0:0a171d2a8dc1 734
bouvdberg 0:0a171d2a8dc1 735 if((EMGlp1-EMGmin1)<=0.0) v1=0.0;
bouvdberg 0:0a171d2a8dc1 736 else v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);
bouvdberg 0:0a171d2a8dc1 737 if((EMGlp2-EMGmin2)<=0.0) v2=0.0;
bouvdberg 0:0a171d2a8dc1 738 else v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);
bouvdberg 0:0a171d2a8dc1 739 if((EMGlp3-EMGmin3)<=0.0) v3=0.0;
bouvdberg 0:0a171d2a8dc1 740 else v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3);
bouvdberg 0:0a171d2a8dc1 741 if((EMGlp4-EMGmin4)<=0.0) v4=0.0;
bouvdberg 0:0a171d2a8dc1 742 else v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4);
bouvdberg 0:0a171d2a8dc1 743
bouvdberg 0:0a171d2a8dc1 744 //t_timer=t_timer+LOOPTIME;
bouvdberg 0:0a171d2a8dc1 745
bouvdberg 0:0a171d2a8dc1 746 //pc.printf("%.2f ",v1);
bouvdberg 0:0a171d2a8dc1 747 //pc.printf("%.2f ",v2);
bouvdberg 0:0a171d2a8dc1 748 //pc.printf("%.2f ",v3);
bouvdberg 0:0a171d2a8dc1 749 //pc.printf("%.2f ",v4);
bouvdberg 1:c18c171bf8b4 750 if(v1<=0.0 && v2<=0.0 && v3<=0.0 && v4<=0.0) {
bouvdberg 0:0a171d2a8dc1 751 Solenoid=1; //Pen van papier
bouvdberg 0:0a171d2a8dc1 752 input=0.0;
bouvdberg 0:0a171d2a8dc1 753 snelheid=0.0;
bouvdberg 0:0a171d2a8dc1 754 }
bouvdberg 0:0a171d2a8dc1 755 else {
bouvdberg 0:0a171d2a8dc1 756 if (sol_updown==1) Solenoid=1; //Pen op papier
bouvdberg 1:c18c171bf8b4 757 else
bouvdberg 1:c18c171bf8b4 758 {
bouvdberg 1:c18c171bf8b4 759 Solenoid=0;
bouvdberg 1:c18c171bf8b4 760 sol_updown=0;
bouvdberg 1:c18c171bf8b4 761 }
bouvdberg 0:0a171d2a8dc1 762 snelheid=drawspeed; //Is dit goed zo????????????????????????????????????????????????????????? Haakjes nodig????
bouvdberg 0:0a171d2a8dc1 763 if(v2>v1) {
bouvdberg 0:0a171d2a8dc1 764 input=(atan((v3-v4)/(v1-v2))+PI);
bouvdberg 0:0a171d2a8dc1 765 }
bouvdberg 0:0a171d2a8dc1 766 else {
bouvdberg 0:0a171d2a8dc1 767 input=(atan((v3-v4)/(v1-v2)));
bouvdberg 0:0a171d2a8dc1 768 }
bouvdberg 0:0a171d2a8dc1 769 }
bouvdberg 0:0a171d2a8dc1 770 //pc.printf("%.2f \n\r",input);
bouvdberg 0:0a171d2a8dc1 771 //snelheid=drawspeed;
bouvdberg 0:0a171d2a8dc1 772 //input=t_timer*0.8+PI;
bouvdberg 0:0a171d2a8dc1 773
bouvdberg 0:0a171d2a8dc1 774 //snelheidsvector met beperking positie / encoder uitlezen
bouvdberg 0:0a171d2a8dc1 775 M1position = motor1.getPosition();
bouvdberg 0:0a171d2a8dc1 776 M2position = motor2.getPosition();
bouvdberg 0:0a171d2a8dc1 777 M2phi=M1position+M2position-1600.0; //phi2 = phi1 + theta - 1600
bouvdberg 0:0a171d2a8dc1 778
bouvdberg 0:0a171d2a8dc1 779 Px=cos((M1position/3200.0)*2.0*PI)*ARM1+cos((M2phi/3200.0)*2.0*PI)*ARM2;
bouvdberg 0:0a171d2a8dc1 780 Py=sin((M1position/3200.0)*2.0*PI)*ARM1+sin((M2phi/3200.0)*2.0*PI)*ARM2;
bouvdberg 0:0a171d2a8dc1 781
bouvdberg 0:0a171d2a8dc1 782 vx=cos(input)*snelheid;
bouvdberg 0:0a171d2a8dc1 783 vy=sin(input)*snelheid;
bouvdberg 0:0a171d2a8dc1 784
bouvdberg 0:0a171d2a8dc1 785 if(Py >= ARM1 && vy>=0 || Py <= 0.080 && vy<=0) {
bouvdberg 0:0a171d2a8dc1 786 vy=0.0;
bouvdberg 0:0a171d2a8dc1 787 }
bouvdberg 0:0a171d2a8dc1 788 if(Px <= -0.425 && vx<=0 || Px >= -0.080 && vx>=0) {
bouvdberg 0:0a171d2a8dc1 789 vx=0.0;
bouvdberg 0:0a171d2a8dc1 790 }
bouvdberg 0:0a171d2a8dc1 791 pc.printf("%.2f ",vx);
bouvdberg 0:0a171d2a8dc1 792 pc.printf("%.2f \n\r",vy);
bouvdberg 0:0a171d2a8dc1 793
bouvdberg 0:0a171d2a8dc1 794 //input positie
bouvdberg 0:0a171d2a8dc1 795 phi1=(motor1.getPosition()/3200.0)*2.0*PI;
bouvdberg 0:0a171d2a8dc1 796 theta=(motor2.getPosition()/3200.0)*2.0*PI;
bouvdberg 0:0a171d2a8dc1 797 phi2=theta+phi1-PI;
bouvdberg 0:0a171d2a8dc1 798
bouvdberg 0:0a171d2a8dc1 799 //Jacobiaan
bouvdberg 0:0a171d2a8dc1 800 // [a b]
bouvdberg 0:0a171d2a8dc1 801 // [c d]
bouvdberg 0:0a171d2a8dc1 802 a=-sin(phi1)*ARM1;
bouvdberg 0:0a171d2a8dc1 803 b=-sin(phi2)*ARM2;
bouvdberg 0:0a171d2a8dc1 804 c=cos(phi1)*ARM1;
bouvdberg 0:0a171d2a8dc1 805 d=cos(phi2)*ARM2;
bouvdberg 0:0a171d2a8dc1 806
bouvdberg 0:0a171d2a8dc1 807 //inverse
bouvdberg 0:0a171d2a8dc1 808 // [ai bi]
bouvdberg 0:0a171d2a8dc1 809 // [ci di]
bouvdberg 0:0a171d2a8dc1 810 ai=d/(a*d-b*c);
bouvdberg 0:0a171d2a8dc1 811 bi=-b/(a*d-b*c);
bouvdberg 0:0a171d2a8dc1 812 ci=-c/(a*d-b*c);
bouvdberg 0:0a171d2a8dc1 813 di=a/(a*d-b*c);
bouvdberg 0:0a171d2a8dc1 814
bouvdberg 0:0a171d2a8dc1 815 //vermenigvuldiging
bouvdberg 0:0a171d2a8dc1 816 // [ai bi] [vx] [w1]
bouvdberg 0:0a171d2a8dc1 817 // [ci di] * [vy] = [w2]
bouvdberg 0:0a171d2a8dc1 818 w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1
bouvdberg 0:0a171d2a8dc1 819 w2=ci*vx+di*vy;
bouvdberg 0:0a171d2a8dc1 820 wM2=w2-w1;//hoeksnelheid motor 2
bouvdberg 0:0a171d2a8dc1 821
bouvdberg 0:0a171d2a8dc1 822 //Beveiliging hoeksnelheden
bouvdberg 0:0a171d2a8dc1 823 keep_in_range(&w1, -600,600);
bouvdberg 0:0a171d2a8dc1 824 keep_in_range(&wM2, -600,600);
bouvdberg 0:0a171d2a8dc1 825
bouvdberg 0:0a171d2a8dc1 826 //Motoraansturing
bouvdberg 0:0a171d2a8dc1 827 //t_sin=t_sin + 0.05;
bouvdberg 0:0a171d2a8dc1 828 //if (t_sin>=2*PI) t_sin=0.0;
bouvdberg 1:c18c171bf8b4 829 setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1;
bouvdberg 0:0a171d2a8dc1 830 setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
bouvdberg 0:0a171d2a8dc1 831
bouvdberg 0:0a171d2a8dc1 832 //Beperking hoeken
bouvdberg 1:c18c171bf8b4 833 keep_in_range(&setpointM1, 500,1400); //Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
bouvdberg 1:c18c171bf8b4 834 keep_in_range(&setpointM2, 1600,2950); // Begrenzing hoeken
bouvdberg 0:0a171d2a8dc1 835
bouvdberg 0:0a171d2a8dc1 836 foutM1 = setpointM1-M1position;
bouvdberg 0:0a171d2a8dc1 837 foutM2 = setpointM2-M2position;
bouvdberg 0:0a171d2a8dc1 838 //foutI1 = foutImin1 + foutM1*LOOPTIME;
bouvdberg 0:0a171d2a8dc1 839 //foutI2 = foutImin2 + foutM2*LOOPTIME;
bouvdberg 0:0a171d2a8dc1 840 //foutverschilM1 = foutM1-foutmin1M1;
bouvdberg 0:0a171d2a8dc1 841 //foutverschilM2 = foutM2-foutmin1M2;
bouvdberg 0:0a171d2a8dc1 842 //foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1;
bouvdberg 0:0a171d2a8dc1 843 //foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2;
bouvdberg 0:0a171d2a8dc1 844 pwm_to_motor1 = foutM1*CP1; //+foutverschilM1*CDloop+foutI1*CI;
bouvdberg 0:0a171d2a8dc1 845 pwm_to_motor2 = foutM2*CP2; //+foutverschilM2*CDloop+foutI2*CI;//foutM2*CP+foutverschilM2*CDloop;
bouvdberg 0:0a171d2a8dc1 846 keep_in_range(&pwm_to_motor1, -0.2,0.2);
bouvdberg 0:0a171d2a8dc1 847 keep_in_range(&pwm_to_motor2, -0.2,0.2);
bouvdberg 0:0a171d2a8dc1 848
bouvdberg 0:0a171d2a8dc1 849
bouvdberg 0:0a171d2a8dc1 850 if(pwm_to_motor1 > 0) {
bouvdberg 0:0a171d2a8dc1 851 motordir1 = 1;
bouvdberg 1:c18c171bf8b4 852 pwm_to_motor1=pwm_to_motor1+0.03;
bouvdberg 0:0a171d2a8dc1 853 }
bouvdberg 0:0a171d2a8dc1 854 else {
bouvdberg 0:0a171d2a8dc1 855 motordir1 = 0;
bouvdberg 1:c18c171bf8b4 856 pwm_to_motor1=pwm_to_motor1-0.03;
bouvdberg 0:0a171d2a8dc1 857 }
bouvdberg 0:0a171d2a8dc1 858 if(pwm_to_motor2 > 0) {
bouvdberg 0:0a171d2a8dc1 859 motordir2 = 1;
bouvdberg 0:0a171d2a8dc1 860 pwm_to_motor2=pwm_to_motor2+0.02;
bouvdberg 0:0a171d2a8dc1 861 }
bouvdberg 0:0a171d2a8dc1 862 else {
bouvdberg 0:0a171d2a8dc1 863 motordir2 = 0;
bouvdberg 0:0a171d2a8dc1 864 pwm_to_motor2=pwm_to_motor2-0.02;
bouvdberg 0:0a171d2a8dc1 865 }
bouvdberg 0:0a171d2a8dc1 866
bouvdberg 0:0a171d2a8dc1 867
bouvdberg 0:0a171d2a8dc1 868 //WRITE VALUE TO MOTOR
bouvdberg 1:c18c171bf8b4 869 //if(v1<=0.01 && v2<=0.01 && v3<=0.01 && v4<=0.01)
bouvdberg 0:0a171d2a8dc1 870 //{
bouvdberg 1:c18c171bf8b4 871 // motordir1 = 1;
bouvdberg 1:c18c171bf8b4 872 // motordir2 = 1;
bouvdberg 1:c18c171bf8b4 873 // Brake1=1;
bouvdberg 1:c18c171bf8b4 874 // Brake2=1;
bouvdberg 0:0a171d2a8dc1 875 // pwm_motor1.write(0);
bouvdberg 0:0a171d2a8dc1 876 // pwm_motor2.write(0); //motor stil
bouvdberg 0:0a171d2a8dc1 877 //}
bouvdberg 0:0a171d2a8dc1 878 //else
bouvdberg 0:0a171d2a8dc1 879 //{
bouvdberg 0:0a171d2a8dc1 880 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:0a171d2a8dc1 881 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:0a171d2a8dc1 882 //}
bouvdberg 0:0a171d2a8dc1 883
bouvdberg 0:0a171d2a8dc1 884 //Definieren waarden in de verleden tijd
bouvdberg 0:0a171d2a8dc1 885 //foutmin1M1=foutM1;
bouvdberg 0:0a171d2a8dc1 886 //foutmin1M2=foutM2;
bouvdberg 0:0a171d2a8dc1 887 //foutverschilmin1M1=foutverschilM1;
bouvdberg 0:0a171d2a8dc1 888 //foutverschilmin1M2=foutverschilM2;
bouvdberg 0:0a171d2a8dc1 889 //foutImin1=foutI1;
bouvdberg 0:0a171d2a8dc1 890 //foutImin2=foutI2;
bouvdberg 0:0a171d2a8dc1 891 setpointmin1M1=setpointM1;
bouvdberg 0:0a171d2a8dc1 892 setpointmin1M2=setpointM2;
bouvdberg 0:0a171d2a8dc1 893 emg_value1min1=emg_value1;
bouvdberg 0:0a171d2a8dc1 894 emg_value2min1=emg_value2;
bouvdberg 0:0a171d2a8dc1 895 emg_value3min1=emg_value3;
bouvdberg 0:0a171d2a8dc1 896 emg_value4min1=emg_value4;
bouvdberg 0:0a171d2a8dc1 897 EMGhp1min1=EMGhp1;
bouvdberg 0:0a171d2a8dc1 898 EMGhp2min1=EMGhp2;
bouvdberg 0:0a171d2a8dc1 899 EMGhp3min1=EMGhp3;
bouvdberg 0:0a171d2a8dc1 900 EMGhp4min1=EMGhp4;
bouvdberg 0:0a171d2a8dc1 901 EMGlp1min1=EMGlp1;
bouvdberg 0:0a171d2a8dc1 902 EMGlp2min1=EMGlp2;
bouvdberg 0:0a171d2a8dc1 903 EMGlp3min1=EMGlp3;
bouvdberg 0:0a171d2a8dc1 904 EMGlp4min1=EMGlp4;
bouvdberg 0:0a171d2a8dc1 905
bouvdberg 0:0a171d2a8dc1 906
bouvdberg 0:0a171d2a8dc1 907 }
bouvdberg 0:0a171d2a8dc1 908 void uitzetten(void)
bouvdberg 0:0a171d2a8dc1 909 {
bouvdberg 0:0a171d2a8dc1 910 float BeginM1 = 800;
bouvdberg 0:0a171d2a8dc1 911 float BeginM2 = 2400;
bouvdberg 1:c18c171bf8b4 912 int i_timer=500;
bouvdberg 0:0a171d2a8dc1 913 Solenoid=1;
bouvdberg 1:c18c171bf8b4 914 sol_updown=1;
bouvdberg 0:0a171d2a8dc1 915 while(BeginM1 - motor1.getPosition() >= 10 || BeginM1 - motor1.getPosition() <= -10 || BeginM2 - motor2.getPosition() >= 10 || BeginM2 - motor2.getPosition() <= -10 || motor1.getSpeed()>=20 || motor2.getSpeed()>=20)
bouvdberg 0:0a171d2a8dc1 916 {
bouvdberg 0:0a171d2a8dc1 917 Ticker looptimer;
bouvdberg 0:0a171d2a8dc1 918 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:0a171d2a8dc1 919 while(looptimerflag != true);
bouvdberg 0:0a171d2a8dc1 920 looptimerflag = false;
bouvdberg 0:0a171d2a8dc1 921 M1position=motor1.getPosition();
bouvdberg 0:0a171d2a8dc1 922 M2position=motor2.getPosition();
bouvdberg 0:0a171d2a8dc1 923 pwm_to_motor1 = (BeginM1 - M1position)*.008;
bouvdberg 0:0a171d2a8dc1 924 pwm_to_motor2 = (BeginM2 - M2position)*.008;
bouvdberg 0:0a171d2a8dc1 925 keep_in_range(&pwm_to_motor1, -0.03,0.03);
bouvdberg 0:0a171d2a8dc1 926 if(pwm_to_motor1 > 0)
bouvdberg 0:0a171d2a8dc1 927 motordir1 = 1;
bouvdberg 0:0a171d2a8dc1 928 else
bouvdberg 0:0a171d2a8dc1 929 motordir1 = 0;
bouvdberg 0:0a171d2a8dc1 930 keep_in_range(&pwm_to_motor2, -0.05,0.05);
bouvdberg 0:0a171d2a8dc1 931 if(pwm_to_motor2 > 0)
bouvdberg 0:0a171d2a8dc1 932 motordir2 = 1;
bouvdberg 0:0a171d2a8dc1 933 else
bouvdberg 0:0a171d2a8dc1 934 motordir2 = 0;
bouvdberg 0:0a171d2a8dc1 935 //WRITE VALUE TO MOTOR
bouvdberg 0:0a171d2a8dc1 936 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:0a171d2a8dc1 937 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:0a171d2a8dc1 938 float sent_pwm = abs(pwm_to_motor2);
bouvdberg 0:0a171d2a8dc1 939
bouvdberg 0:0a171d2a8dc1 940 if(i_timer<=0) break;
bouvdberg 0:0a171d2a8dc1 941 i_timer--;
bouvdberg 0:0a171d2a8dc1 942
bouvdberg 0:0a171d2a8dc1 943 }
bouvdberg 1:c18c171bf8b4 944 motordir1 = 1;
bouvdberg 1:c18c171bf8b4 945 motordir2 = 1;
bouvdberg 1:c18c171bf8b4 946 Brake1=1;
bouvdberg 1:c18c171bf8b4 947 Brake2=1;
bouvdberg 0:0a171d2a8dc1 948 pwm_motor1.write(0);
bouvdberg 0:0a171d2a8dc1 949 pwm_motor2.write(0);
bouvdberg 0:0a171d2a8dc1 950 wait(1.0);
bouvdberg 0:0a171d2a8dc1 951 Brake1=0;
bouvdberg 0:0a171d2a8dc1 952 Brake2=0;
bouvdberg 1:c18c171bf8b4 953 i_timer=300;
bouvdberg 0:0a171d2a8dc1 954 while(BeginM1 - motor1.getPosition() >= 10 || BeginM1 - motor1.getPosition() <= -10 || BeginM2 - motor2.getPosition() >= 10 || BeginM2 - motor2.getPosition() <= -10 || motor1.getSpeed()>=20 || motor2.getSpeed()>=20)
bouvdberg 0:0a171d2a8dc1 955 {
bouvdberg 0:0a171d2a8dc1 956 Ticker looptimer;
bouvdberg 0:0a171d2a8dc1 957 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:0a171d2a8dc1 958 while(looptimerflag != true);
bouvdberg 0:0a171d2a8dc1 959 looptimerflag = false;
bouvdberg 0:0a171d2a8dc1 960 M1position=motor1.getPosition();
bouvdberg 0:0a171d2a8dc1 961 M2position=motor2.getPosition();
bouvdberg 0:0a171d2a8dc1 962 pwm_to_motor1 = (BeginM1 - M1position)*.008;
bouvdberg 0:0a171d2a8dc1 963 pwm_to_motor2 = (BeginM2 - M2position)*.008;
bouvdberg 0:0a171d2a8dc1 964 keep_in_range(&pwm_to_motor1, -0.05,0.05);
bouvdberg 0:0a171d2a8dc1 965 if(pwm_to_motor1 > 0)
bouvdberg 0:0a171d2a8dc1 966 motordir1 = 1;
bouvdberg 0:0a171d2a8dc1 967 else
bouvdberg 0:0a171d2a8dc1 968 motordir1 = 0;
bouvdberg 0:0a171d2a8dc1 969 keep_in_range(&pwm_to_motor2, -0.09,0.09);
bouvdberg 0:0a171d2a8dc1 970 if(pwm_to_motor2 > 0)
bouvdberg 0:0a171d2a8dc1 971 motordir2 = 1;
bouvdberg 0:0a171d2a8dc1 972 else
bouvdberg 0:0a171d2a8dc1 973 motordir2 = 0;
bouvdberg 0:0a171d2a8dc1 974 //WRITE VALUE TO MOTOR
bouvdberg 0:0a171d2a8dc1 975 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:0a171d2a8dc1 976 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:0a171d2a8dc1 977 float sent_pwm = abs(pwm_to_motor2);
bouvdberg 0:0a171d2a8dc1 978
bouvdberg 0:0a171d2a8dc1 979 if(i_timer<=0) break;
bouvdberg 0:0a171d2a8dc1 980 i_timer--;
bouvdberg 0:0a171d2a8dc1 981
bouvdberg 0:0a171d2a8dc1 982 }
bouvdberg 1:c18c171bf8b4 983 motordir1 = 1;
bouvdberg 1:c18c171bf8b4 984 motordir2 = 1;
bouvdberg 1:c18c171bf8b4 985 Brake1=1;
bouvdberg 1:c18c171bf8b4 986 Brake2=1;
bouvdberg 0:0a171d2a8dc1 987 pwm_motor1.write(0);
bouvdberg 0:0a171d2a8dc1 988 pwm_motor2.write(0);
bouvdberg 0:0a171d2a8dc1 989 wait(1.0);
bouvdberg 0:0a171d2a8dc1 990 Brake1=0;
bouvdberg 0:0a171d2a8dc1 991 Brake2=0;
bouvdberg 0:0a171d2a8dc1 992 setpointM1=800.0;
bouvdberg 0:0a171d2a8dc1 993 setpointM2=2400.0;
bouvdberg 1:c18c171bf8b4 994 setpointmin1M1=800.0;
bouvdberg 1:c18c171bf8b4 995 setpointmin1M2=2400.0;
bouvdberg 0:0a171d2a8dc1 996 pwm_to_motor1=0.0;
bouvdberg 0:0a171d2a8dc1 997 pwm_to_motor2=0.0;
bouvdberg 0:0a171d2a8dc1 998 foutM1=0.0;
bouvdberg 0:0a171d2a8dc1 999 foutM2=0.0;
bouvdberg 0:0a171d2a8dc1 1000 //foutmin1M1=0.0;
bouvdberg 0:0a171d2a8dc1 1001 //foutmin1M2=0.0;
bouvdberg 0:0a171d2a8dc1 1002 //foutverschilM1=0.0;
bouvdberg 0:0a171d2a8dc1 1003 //foutverschilM2=0.0;
bouvdberg 0:0a171d2a8dc1 1004 //foutverschilmin1M1=0.0;
bouvdberg 0:0a171d2a8dc1 1005 //foutverschilmin1M2=0.0;
bouvdberg 0:0a171d2a8dc1 1006 //foutImin1=0.0;
bouvdberg 0:0a171d2a8dc1 1007 //foutImin2=0.0;
bouvdberg 0:0a171d2a8dc1 1008 //foutI1=0.0;
bouvdberg 0:0a171d2a8dc1 1009 //foutI2=0.0;
bouvdberg 0:0a171d2a8dc1 1010 //t_sin=0.0;
bouvdberg 0:0a171d2a8dc1 1011 //t_timer=0.0;
bouvdberg 0:0a171d2a8dc1 1012 emg_value1min1=0.5;
bouvdberg 0:0a171d2a8dc1 1013 emg_value2min1=0.5;
bouvdberg 0:0a171d2a8dc1 1014 emg_value3min1=0.5;
bouvdberg 0:0a171d2a8dc1 1015 emg_value4min1=0.5;
bouvdberg 0:0a171d2a8dc1 1016 EMGhp1min1=0.5;
bouvdberg 0:0a171d2a8dc1 1017 EMGhp2min1=0.5;
bouvdberg 0:0a171d2a8dc1 1018 EMGhp3min1=0.5;
bouvdberg 0:0a171d2a8dc1 1019 EMGhp4min1=0.5;
bouvdberg 0:0a171d2a8dc1 1020 EMGlp1min1=0.5;
bouvdberg 0:0a171d2a8dc1 1021 EMGlp2min1=0.5;
bouvdberg 0:0a171d2a8dc1 1022 EMGlp3min1=0.5;
bouvdberg 0:0a171d2a8dc1 1023 EMGlp4min1=0.5;
bouvdberg 0:0a171d2a8dc1 1024 menu=0;
bouvdberg 0:0a171d2a8dc1 1025 }
bouvdberg 0:0a171d2a8dc1 1026
bouvdberg 0:0a171d2a8dc1 1027 void keep_in_range(float * in, float min, float max)
bouvdberg 0:0a171d2a8dc1 1028 {
bouvdberg 0:0a171d2a8dc1 1029 *in > min ? *in < max? : *in = max: *in = min;
bouvdberg 0:0a171d2a8dc1 1030 }
bouvdberg 0:0a171d2a8dc1 1031
bouvdberg 0:0a171d2a8dc1 1032 void setlooptimerflag(void)
bouvdberg 0:0a171d2a8dc1 1033 {
bouvdberg 0:0a171d2a8dc1 1034 looptimerflag = true;
bouvdberg 0:0a171d2a8dc1 1035 }