Boudewijn van den Berg / Mbed 2 deprecated RobotControlVfinal

RobotControlScript Groep 4

Dependencies:   MODSERIAL TextLCD mbed

main.cpp@0:da261c102b95, 2013-11-05 (annotated)

Committer:
bouvdberg
Date:
Tue Nov 05 19:29:42 2013 +0000
Revision:
0:da261c102b95
RobotControlGroep4

Who changed what in which revision?

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