Boudewijn van den Berg / Mbed 2 deprecated HuidigePositieV2

..

Dependencies:   MODSERIAL TextLCD mbed

main.cpp@0:4b5b608f1f71, 2013-11-02 (annotated)

Committer:
bouvdberg
Date:
Sat Nov 02 14:44:53 2013 +0000
Revision:
0:4b5b608f1f71
Nieuwste Versie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouvdberg 0:4b5b608f1f71 1 // Hello World! for the TextLCD*//
bouvdberg 0:4b5b608f1f71 2
bouvdberg 0:4b5b608f1f71 3 #include "mbed.h"
bouvdberg 0:4b5b608f1f71 4 #include "TextLCD.h"
bouvdberg 0:4b5b608f1f71 5 #include "MODSERIAL.h"
bouvdberg 0:4b5b608f1f71 6 #include "encoder.h"
bouvdberg 0:4b5b608f1f71 7
bouvdberg 0:4b5b608f1f71 8 // definieren constanten
bouvdberg 0:4b5b608f1f71 9 #define PI 3.1415926
bouvdberg 0:4b5b608f1f71 10 //plant
bouvdberg 0:4b5b608f1f71 11 #define ARM1 0.36
bouvdberg 0:4b5b608f1f71 12 #define ARM2 0.26
bouvdberg 0:4b5b608f1f71 13 //PD
bouvdberg 0:4b5b608f1f71 14 #define CI 0.000
bouvdberg 0:4b5b608f1f71 15 #define CP1 0.0000000005
bouvdberg 0:4b5b608f1f71 16 #define CP2 0.0000000005
bouvdberg 0:4b5b608f1f71 17 #define CD 0.00
bouvdberg 0:4b5b608f1f71 18 #define CLP1 0.9975
bouvdberg 0:4b5b608f1f71 19 #define CLP2 0.001
bouvdberg 0:4b5b608f1f71 20 //Snelheid
bouvdberg 0:4b5b608f1f71 21 #define SNELHEID 0.02
bouvdberg 0:4b5b608f1f71 22 //LOOPTIME
bouvdberg 0:4b5b608f1f71 23 #define LOOPTIME 0.006667
bouvdberg 0:4b5b608f1f71 24 //Filtering EMG
bouvdberg 0:4b5b608f1f71 25 #define HP1 0.8752
bouvdberg 0:4b5b608f1f71 26 #define HP2 20.0
bouvdberg 0:4b5b608f1f71 27 #define HP3 20.0
bouvdberg 0:4b5b608f1f71 28 #define LP1 0.9868
bouvdberg 0:4b5b608f1f71 29 #define LP2 0.01325
bouvdberg 0:4b5b608f1f71 30 //EMG threshold
bouvdberg 0:4b5b608f1f71 31 #define SET_EMG_MAX1 10.0 //bovenarm rechts > beweging naar rechts
bouvdberg 0:4b5b608f1f71 32 #define SET_EMG_MIN1 8.0
bouvdberg 0:4b5b608f1f71 33 #define SET_EMG_MAX2 10.0 //bovenarm links > beweging naar links
bouvdberg 0:4b5b608f1f71 34 #define SET_EMG_MIN2 5.0
bouvdberg 0:4b5b608f1f71 35 #define SET_EMG_MAX3 10.0 //onderarm rechts > beweging naar boven
bouvdberg 0:4b5b608f1f71 36 #define SET_EMG_MIN3 7.0
bouvdberg 0:4b5b608f1f71 37 #define SET_EMG_MAX4 10.0 //onderarm links > beweging naar onder
bouvdberg 0:4b5b608f1f71 38 #define SET_EMG_MIN4 5.0
bouvdberg 0:4b5b608f1f71 39
bouvdberg 0:4b5b608f1f71 40 void aansturing(void);
bouvdberg 0:4b5b608f1f71 41 void uitzetten(void);
bouvdberg 0:4b5b608f1f71 42 void setlooptimerflag(void);
bouvdberg 0:4b5b608f1f71 43 void keep_in_range(float * in, float min, float max);
bouvdberg 0:4b5b608f1f71 44
bouvdberg 0:4b5b608f1f71 45 volatile bool looptimerflag;
bouvdberg 0:4b5b608f1f71 46
bouvdberg 0:4b5b608f1f71 47 Serial pc(USBTX, USBRX);
bouvdberg 0:4b5b608f1f71 48 TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2,NC,NC,TextLCD::HD44780); // rs, e, d4-d7-/*+-9
bouvdberg 0:4b5b608f1f71 49 AnalogIn EMG1(PTB0); //EMG
bouvdberg 0:4b5b608f1f71 50 AnalogIn EMG2(PTB1);
bouvdberg 0:4b5b608f1f71 51 AnalogIn EMG3(PTB2);
bouvdberg 0:4b5b608f1f71 52 AnalogIn EMG4(PTB3);
bouvdberg 0:4b5b608f1f71 53 AnalogIn potmeter(PTC2); //potmeter
bouvdberg 0:4b5b608f1f71 54 DigitalIn ButtonSTOP(PTE21); //Knopjes voor kalibratie
bouvdberg 0:4b5b608f1f71 55 DigitalIn ButtonSELECT(PTE20);
bouvdberg 0:4b5b608f1f71 56 DigitalIn ButtonUP(PTE23);
bouvdberg 0:4b5b608f1f71 57 DigitalIn ButtonDOWN(PTE22);
bouvdberg 0:4b5b608f1f71 58 DigitalOut Solenoid(PTD4); //Solenoid
bouvdberg 0:4b5b608f1f71 59 Encoder motor1(PTD0,PTC8); //Encoder
bouvdberg 0:4b5b608f1f71 60 Encoder motor2(PTD2,PTC9);
bouvdberg 0:4b5b608f1f71 61 PwmOut pwm_motor1(PTA12); //motor
bouvdberg 0:4b5b608f1f71 62 DigitalOut motordir1(PTD3);
bouvdberg 0:4b5b608f1f71 63 PwmOut pwm_motor2(PTA5);
bouvdberg 0:4b5b608f1f71 64 DigitalOut motordir2(PTD1);
bouvdberg 0:4b5b608f1f71 65 DigitalOut Brake1(PTD5);
bouvdberg 0:4b5b608f1f71 66 DigitalOut Brake2(PTA13);
bouvdberg 0:4b5b608f1f71 67
bouvdberg 0:4b5b608f1f71 68 float numberx = 9;
bouvdberg 0:4b5b608f1f71 69 int menu=0, t;
bouvdberg 0:4b5b608f1f71 70 float EMGmax1=SET_EMG_MAX1, EMGmin1=SET_EMG_MIN1, EMGmax2=SET_EMG_MAX2, EMGmin2=SET_EMG_MIN2;
bouvdberg 0:4b5b608f1f71 71 float EMGmax3=SET_EMG_MAX3, EMGmin3=SET_EMG_MIN3, EMGmax4=SET_EMG_MAX4, EMGmin4=SET_EMG_MIN4;
bouvdberg 0:4b5b608f1f71 72 float drawspeed=SNELHEID;
bouvdberg 0:4b5b608f1f71 73
bouvdberg 0:4b5b608f1f71 74
bouvdberg 0:4b5b608f1f71 75 //Variabelen verwerking EMG
bouvdberg 0:4b5b608f1f71 76 float emg_value1, emg_value2, emg_value3, emg_value4;
bouvdberg 0:4b5b608f1f71 77 float emg_value1min1=0.5, emg_value2min1=0.5, emg_value3min1=0.5, emg_value4min1=0.5;
bouvdberg 0:4b5b608f1f71 78 float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4;
bouvdberg 0:4b5b608f1f71 79 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:4b5b608f1f71 80
bouvdberg 0:4b5b608f1f71 81 //Variabelen bepaling input systeem
bouvdberg 0:4b5b608f1f71 82 float input;
bouvdberg 0:4b5b608f1f71 83 float w1, w2, wM2, phi1, phi2, theta;
bouvdberg 0:4b5b608f1f71 84 float a, b, c, d, ai, bi, ci, di;
bouvdberg 0:4b5b608f1f71 85 float v1, v2, v3, v4, vx, vy, snelheid;
bouvdberg 0:4b5b608f1f71 86 float M1position, M2position, M2phi;
bouvdberg 0:4b5b608f1f71 87 float Px, Py;
bouvdberg 0:4b5b608f1f71 88 int sol_updown=0;
bouvdberg 0:4b5b608f1f71 89
bouvdberg 0:4b5b608f1f71 90 //Variabelen motoraansturing
bouvdberg 0:4b5b608f1f71 91 float setpointM1, setpointM2;
bouvdberg 0:4b5b608f1f71 92 float setpointmin1M1=800.0, setpointmin1M2=2400.0;
bouvdberg 0:4b5b608f1f71 93 float pwm_to_motor1, pwm_to_motor2;
bouvdberg 0:4b5b608f1f71 94 float foutM1, foutM2;
bouvdberg 0:4b5b608f1f71 95 float foutmin1M1=0.0, foutmin1M2=0.0;
bouvdberg 0:4b5b608f1f71 96 float foutverschilM1, foutverschilM2;
bouvdberg 0:4b5b608f1f71 97 float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0;
bouvdberg 0:4b5b608f1f71 98 float foutImin1=0.0, foutImin2=0.0, foutI1, foutI2;
bouvdberg 0:4b5b608f1f71 99 float CDloop=CD/LOOPTIME;
bouvdberg 0:4b5b608f1f71 100 float t_sin=0.0;
bouvdberg 0:4b5b608f1f71 101 float t_timer=0.0;
bouvdberg 0:4b5b608f1f71 102
bouvdberg 0:4b5b608f1f71 103 int main() {
bouvdberg 0:4b5b608f1f71 104 //set buttons PULLDOWN
bouvdberg 0:4b5b608f1f71 105 ButtonSTOP.mode(PullNone);
bouvdberg 0:4b5b608f1f71 106 ButtonSELECT.mode(PullNone);
bouvdberg 0:4b5b608f1f71 107 ButtonUP.mode(PullNone);
bouvdberg 0:4b5b608f1f71 108 ButtonDOWN.mode(PullNone);
bouvdberg 0:4b5b608f1f71 109 pc.baud(57600);
bouvdberg 0:4b5b608f1f71 110 //Aanstuur timing
bouvdberg 0:4b5b608f1f71 111 Ticker looptimer;
bouvdberg 0:4b5b608f1f71 112 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:4b5b608f1f71 113 while(1)
bouvdberg 0:4b5b608f1f71 114 {
bouvdberg 0:4b5b608f1f71 115 switch (menu)
bouvdberg 0:4b5b608f1f71 116 {
bouvdberg 0:4b5b608f1f71 117 case 0:
bouvdberg 0:4b5b608f1f71 118 lcd.cls();
bouvdberg 0:4b5b608f1f71 119 lcd.printf("> DRAW");
bouvdberg 0:4b5b608f1f71 120 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 121 lcd.printf(" SETTINGS");
bouvdberg 0:4b5b608f1f71 122 while(menu==0)
bouvdberg 0:4b5b608f1f71 123 {
bouvdberg 0:4b5b608f1f71 124 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 125 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 126 {
bouvdberg 0:4b5b608f1f71 127 motor1.setPosition(800);
bouvdberg 0:4b5b608f1f71 128 motor2.setPosition(2400);
bouvdberg 0:4b5b608f1f71 129 menu=55;
bouvdberg 0:4b5b608f1f71 130 lcd.cls();
bouvdberg 0:4b5b608f1f71 131 lcd.printf("UP:Pause Drawing");
bouvdberg 0:4b5b608f1f71 132 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 133 lcd.printf("DOWN:Pen Up/Down");
bouvdberg 0:4b5b608f1f71 134 Solenoid=0;
bouvdberg 0:4b5b608f1f71 135 sol_updown=0;
bouvdberg 0:4b5b608f1f71 136 wait(0.01);
bouvdberg 0:4b5b608f1f71 137 }
bouvdberg 0:4b5b608f1f71 138 }
bouvdberg 0:4b5b608f1f71 139 break;
bouvdberg 0:4b5b608f1f71 140 case 1:
bouvdberg 0:4b5b608f1f71 141 lcd.cls();
bouvdberg 0:4b5b608f1f71 142 lcd.printf("> SETTINGS");
bouvdberg 0:4b5b608f1f71 143 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 144 lcd.printf(" RESET ALL");
bouvdberg 0:4b5b608f1f71 145 while(menu==1)
bouvdberg 0:4b5b608f1f71 146 {
bouvdberg 0:4b5b608f1f71 147 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 148 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 149 if (ButtonSELECT.read()==1) menu=20;
bouvdberg 0:4b5b608f1f71 150 }
bouvdberg 0:4b5b608f1f71 151 break;
bouvdberg 0:4b5b608f1f71 152 case 2:
bouvdberg 0:4b5b608f1f71 153 lcd.cls();
bouvdberg 0:4b5b608f1f71 154 lcd.printf("> RESET ALL");
bouvdberg 0:4b5b608f1f71 155 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 156 lcd.printf(" ");
bouvdberg 0:4b5b608f1f71 157 while(menu==2)
bouvdberg 0:4b5b608f1f71 158 {
bouvdberg 0:4b5b608f1f71 159 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 160 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 161 {
bouvdberg 0:4b5b608f1f71 162 EMGmax1=SET_EMG_MAX1; EMGmin1=SET_EMG_MIN1;
bouvdberg 0:4b5b608f1f71 163 EMGmax2=SET_EMG_MAX2; EMGmin2=SET_EMG_MIN2;
bouvdberg 0:4b5b608f1f71 164 EMGmax3=SET_EMG_MAX3; EMGmin3=SET_EMG_MIN3;
bouvdberg 0:4b5b608f1f71 165 EMGmax4=SET_EMG_MAX4; EMGmin4=SET_EMG_MIN4;
bouvdberg 0:4b5b608f1f71 166 drawspeed=SNELHEID;
bouvdberg 0:4b5b608f1f71 167 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 168 lcd.printf(" Reset Completed");
bouvdberg 0:4b5b608f1f71 169 wait(1);
bouvdberg 0:4b5b608f1f71 170 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 171 lcd.printf(" ");
bouvdberg 0:4b5b608f1f71 172 }
bouvdberg 0:4b5b608f1f71 173 }
bouvdberg 0:4b5b608f1f71 174 break;
bouvdberg 0:4b5b608f1f71 175 case 20:
bouvdberg 0:4b5b608f1f71 176 lcd.cls();
bouvdberg 0:4b5b608f1f71 177 lcd.printf("> EMG1-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:4b5b608f1f71 178 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 179 lcd.printf(" EMG1-MIN: ");
bouvdberg 0:4b5b608f1f71 180 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 181 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 182 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 183 {
bouvdberg 0:4b5b608f1f71 184 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 185 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 186 EMGmax1=(EMGmax1+(((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:4b5b608f1f71 187 wait(0.5);
bouvdberg 0:4b5b608f1f71 188 }
bouvdberg 0:4b5b608f1f71 189 break;
bouvdberg 0:4b5b608f1f71 190 case 21:
bouvdberg 0:4b5b608f1f71 191 lcd.cls();
bouvdberg 0:4b5b608f1f71 192 lcd.printf("> EMG1-MIN: %.2f", (EMGmin1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:4b5b608f1f71 193 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 194 lcd.printf(" EMG2-MAX: ");
bouvdberg 0:4b5b608f1f71 195 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 196 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 197 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 198 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 199 {
bouvdberg 0:4b5b608f1f71 200 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 201 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 202 EMGmin1=(EMGmin1+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:4b5b608f1f71 203 wait(0.5);
bouvdberg 0:4b5b608f1f71 204 }
bouvdberg 0:4b5b608f1f71 205 break;
bouvdberg 0:4b5b608f1f71 206 case 22:
bouvdberg 0:4b5b608f1f71 207 lcd.cls();
bouvdberg 0:4b5b608f1f71 208 lcd.printf("> EMG2-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:4b5b608f1f71 209 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 210 lcd.printf(" EMG2-MIN: ");
bouvdberg 0:4b5b608f1f71 211 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 212 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 213 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 214 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 215 {
bouvdberg 0:4b5b608f1f71 216 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 217 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 218 EMGmax2=(EMGmax1+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:4b5b608f1f71 219 wait(0.5);
bouvdberg 0:4b5b608f1f71 220 }
bouvdberg 0:4b5b608f1f71 221 break;
bouvdberg 0:4b5b608f1f71 222 case 23:
bouvdberg 0:4b5b608f1f71 223 lcd.cls();
bouvdberg 0:4b5b608f1f71 224 lcd.printf("> EMG2-MIN: %.2f", (EMGmin2+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:4b5b608f1f71 225 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 226 lcd.printf(" EMG3-MAX: ");
bouvdberg 0:4b5b608f1f71 227 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 228 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 229 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 230 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 231 {
bouvdberg 0:4b5b608f1f71 232 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 233 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 234 EMGmin2=(EMGmin2+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:4b5b608f1f71 235 wait(0.5);
bouvdberg 0:4b5b608f1f71 236 }
bouvdberg 0:4b5b608f1f71 237 break;
bouvdberg 0:4b5b608f1f71 238 case 24:
bouvdberg 0:4b5b608f1f71 239 lcd.cls();
bouvdberg 0:4b5b608f1f71 240 lcd.printf("> EMG3-MAX: %.2f", (EMGmax3+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:4b5b608f1f71 241 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 242 lcd.printf(" EMG3-MIN: ");
bouvdberg 0:4b5b608f1f71 243 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 244 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 245 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 246 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 247 {
bouvdberg 0:4b5b608f1f71 248 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 249 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 250 EMGmax3=(EMGmax3+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:4b5b608f1f71 251 wait(0.5);
bouvdberg 0:4b5b608f1f71 252 }
bouvdberg 0:4b5b608f1f71 253 break;
bouvdberg 0:4b5b608f1f71 254 case 25:
bouvdberg 0:4b5b608f1f71 255 lcd.cls();
bouvdberg 0:4b5b608f1f71 256 lcd.printf("> EMG3-MIN: %.2f", (EMGmin3+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:4b5b608f1f71 257 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 258 lcd.printf(" EMG4-MAX: ");
bouvdberg 0:4b5b608f1f71 259 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 260 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 261 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 262 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 263 {
bouvdberg 0:4b5b608f1f71 264 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 265 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 266 EMGmin3=(EMGmin3+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:4b5b608f1f71 267 wait(0.5);
bouvdberg 0:4b5b608f1f71 268 }
bouvdberg 0:4b5b608f1f71 269 break;
bouvdberg 0:4b5b608f1f71 270 case 26:
bouvdberg 0:4b5b608f1f71 271 lcd.cls();
bouvdberg 0:4b5b608f1f71 272 lcd.printf("> EMG4-MAX: %.2f", (EMGmax4+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:4b5b608f1f71 273 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 274 lcd.printf(" EMG4-MIN: ");
bouvdberg 0:4b5b608f1f71 275 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 276 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 277 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 278 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 279 {
bouvdberg 0:4b5b608f1f71 280 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 281 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 282 EMGmax4=(EMGmax4+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:4b5b608f1f71 283 wait(0.5);
bouvdberg 0:4b5b608f1f71 284 }
bouvdberg 0:4b5b608f1f71 285 break;
bouvdberg 0:4b5b608f1f71 286 case 27:
bouvdberg 0:4b5b608f1f71 287 lcd.cls();
bouvdberg 0:4b5b608f1f71 288 lcd.printf("> EMG4-MIN: %.2f", (EMGmin4+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:4b5b608f1f71 289 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 290 lcd.printf(" SPEED :");
bouvdberg 0:4b5b608f1f71 291 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 292 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 293 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 294 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 295 {
bouvdberg 0:4b5b608f1f71 296 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 297 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 298 EMGmin4=(EMGmin4+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:4b5b608f1f71 299 wait(0.5);
bouvdberg 0:4b5b608f1f71 300 }
bouvdberg 0:4b5b608f1f71 301 break;
bouvdberg 0:4b5b608f1f71 302 case 28:
bouvdberg 0:4b5b608f1f71 303 lcd.cls();
bouvdberg 0:4b5b608f1f71 304 lcd.printf("> SPEED : %.2f", (drawspeed+ (((potmeter.read()+0.0005)/10)-0.05)));
bouvdberg 0:4b5b608f1f71 305 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 306 lcd.printf(" SOLENOID:");
bouvdberg 0:4b5b608f1f71 307 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 308 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 309 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 310 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 311 {
bouvdberg 0:4b5b608f1f71 312 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 313 lcd.printf(" SAVED! ");
bouvdberg 0:4b5b608f1f71 314 drawspeed=(drawspeed+ (((potmeter.read()+0.0005)/10)-0.05));
bouvdberg 0:4b5b608f1f71 315 wait(0.5);
bouvdberg 0:4b5b608f1f71 316 }
bouvdberg 0:4b5b608f1f71 317 break;
bouvdberg 0:4b5b608f1f71 318 case 29:
bouvdberg 0:4b5b608f1f71 319 lcd.cls();
bouvdberg 0:4b5b608f1f71 320 lcd.printf("> SOLENOID: OFF");
bouvdberg 0:4b5b608f1f71 321 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 322 lcd.printf(" ");
bouvdberg 0:4b5b608f1f71 323 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:4b5b608f1f71 324 if (ButtonUP.read()==1) menu--;
bouvdberg 0:4b5b608f1f71 325 //if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 326 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 327 {
bouvdberg 0:4b5b608f1f71 328 lcd.cls();
bouvdberg 0:4b5b608f1f71 329 lcd.printf("> SOLENOID: ON");
bouvdberg 0:4b5b608f1f71 330 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 331 lcd.printf(" ");
bouvdberg 0:4b5b608f1f71 332 Solenoid=1;
bouvdberg 0:4b5b608f1f71 333 sol_updown=1;
bouvdberg 0:4b5b608f1f71 334 wait(1);
bouvdberg 0:4b5b608f1f71 335 Solenoid=0;
bouvdberg 0:4b5b608f1f71 336 sol_updown=0;
bouvdberg 0:4b5b608f1f71 337 }
bouvdberg 0:4b5b608f1f71 338 break;
bouvdberg 0:4b5b608f1f71 339 case 50: //tekenen afsluiten
bouvdberg 0:4b5b608f1f71 340 lcd.cls();
bouvdberg 0:4b5b608f1f71 341 lcd.printf(" Shutting Down!");
bouvdberg 0:4b5b608f1f71 342 menu=0;
bouvdberg 0:4b5b608f1f71 343
bouvdberg 0:4b5b608f1f71 344 uitzetten();
bouvdberg 0:4b5b608f1f71 345
bouvdberg 0:4b5b608f1f71 346 break;
bouvdberg 0:4b5b608f1f71 347 case 55:
bouvdberg 0:4b5b608f1f71 348
bouvdberg 0:4b5b608f1f71 349 if (ButtonSTOP.read()==1) menu=50;
bouvdberg 0:4b5b608f1f71 350 if (ButtonUP.read()==1) menu++;
bouvdberg 0:4b5b608f1f71 351 if (ButtonDOWN.read()==1)
bouvdberg 0:4b5b608f1f71 352 {
bouvdberg 0:4b5b608f1f71 353 if (sol_updown==1)
bouvdberg 0:4b5b608f1f71 354 {
bouvdberg 0:4b5b608f1f71 355 Solenoid=0;
bouvdberg 0:4b5b608f1f71 356 sol_updown=0;
bouvdberg 0:4b5b608f1f71 357 }
bouvdberg 0:4b5b608f1f71 358 if (sol_updown==0)
bouvdberg 0:4b5b608f1f71 359 {
bouvdberg 0:4b5b608f1f71 360 Solenoid=1;
bouvdberg 0:4b5b608f1f71 361 sol_updown=1;
bouvdberg 0:4b5b608f1f71 362 }
bouvdberg 0:4b5b608f1f71 363 while (ButtonDOWN.read()==1);
bouvdberg 0:4b5b608f1f71 364 }
bouvdberg 0:4b5b608f1f71 365
bouvdberg 0:4b5b608f1f71 366 aansturing(); //aansturing
bouvdberg 0:4b5b608f1f71 367
bouvdberg 0:4b5b608f1f71 368 break;
bouvdberg 0:4b5b608f1f71 369 case 56:
bouvdberg 0:4b5b608f1f71 370 wait(0.2);
bouvdberg 0:4b5b608f1f71 371 lcd.cls();
bouvdberg 0:4b5b608f1f71 372 lcd.printf(" PAUSE ");
bouvdberg 0:4b5b608f1f71 373 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 374 lcd.printf("> RESUME");
bouvdberg 0:4b5b608f1f71 375
bouvdberg 0:4b5b608f1f71 376 Solenoid=1;
bouvdberg 0:4b5b608f1f71 377 pwm_motor1.write(0);
bouvdberg 0:4b5b608f1f71 378 pwm_motor2.write(0);
bouvdberg 0:4b5b608f1f71 379
bouvdberg 0:4b5b608f1f71 380 while(ButtonSELECT.read()==1);
bouvdberg 0:4b5b608f1f71 381 while(menu==56)
bouvdberg 0:4b5b608f1f71 382 {
bouvdberg 0:4b5b608f1f71 383 if (ButtonSTOP.read()==1) menu=50;
bouvdberg 0:4b5b608f1f71 384 if (ButtonSELECT.read()==1)
bouvdberg 0:4b5b608f1f71 385 {
bouvdberg 0:4b5b608f1f71 386 menu--;
bouvdberg 0:4b5b608f1f71 387 lcd.cls();
bouvdberg 0:4b5b608f1f71 388 lcd.printf("UP:Pause Drawing");
bouvdberg 0:4b5b608f1f71 389 lcd.locate(0,1);
bouvdberg 0:4b5b608f1f71 390 lcd.printf("DOWN:Pen Up/Down");
bouvdberg 0:4b5b608f1f71 391 }
bouvdberg 0:4b5b608f1f71 392 }
bouvdberg 0:4b5b608f1f71 393
bouvdberg 0:4b5b608f1f71 394
bouvdberg 0:4b5b608f1f71 395
bouvdberg 0:4b5b608f1f71 396 break;
bouvdberg 0:4b5b608f1f71 397 }
bouvdberg 0:4b5b608f1f71 398
bouvdberg 0:4b5b608f1f71 399 if (menu!=55) wait(0.2);
bouvdberg 0:4b5b608f1f71 400 }
bouvdberg 0:4b5b608f1f71 401
bouvdberg 0:4b5b608f1f71 402
bouvdberg 0:4b5b608f1f71 403 }
bouvdberg 0:4b5b608f1f71 404
bouvdberg 0:4b5b608f1f71 405 void aansturing(void)
bouvdberg 0:4b5b608f1f71 406 {
bouvdberg 0:4b5b608f1f71 407 while(looptimerflag != true);
bouvdberg 0:4b5b608f1f71 408 looptimerflag = false;
bouvdberg 0:4b5b608f1f71 409
bouvdberg 0:4b5b608f1f71 410 //uitlezen
bouvdberg 0:4b5b608f1f71 411 emg_value1 = EMG1.read();
bouvdberg 0:4b5b608f1f71 412 emg_value2 = EMG2.read();
bouvdberg 0:4b5b608f1f71 413 emg_value3 = EMG3.read();
bouvdberg 0:4b5b608f1f71 414 emg_value4 = EMG4.read();
bouvdberg 0:4b5b608f1f71 415
bouvdberg 0:4b5b608f1f71 416 //filtering
bouvdberg 0:4b5b608f1f71 417 EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
bouvdberg 0:4b5b608f1f71 418 EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
bouvdberg 0:4b5b608f1f71 419 EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
bouvdberg 0:4b5b608f1f71 420 EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
bouvdberg 0:4b5b608f1f71 421 EMGhp1=abs(EMGhp1);
bouvdberg 0:4b5b608f1f71 422 EMGhp2=abs(EMGhp2);
bouvdberg 0:4b5b608f1f71 423 EMGhp3=abs(EMGhp3);
bouvdberg 0:4b5b608f1f71 424 EMGhp4=abs(EMGhp4);
bouvdberg 0:4b5b608f1f71 425 EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
bouvdberg 0:4b5b608f1f71 426 EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
bouvdberg 0:4b5b608f1f71 427 EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
bouvdberg 0:4b5b608f1f71 428 EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
bouvdberg 0:4b5b608f1f71 429 //pc.printf("%.2f ",emg_value1);
bouvdberg 0:4b5b608f1f71 430 //pc.printf("%.2f ",emg_value2);
bouvdberg 0:4b5b608f1f71 431 //pc.printf("%.2f ",emg_value3);
bouvdberg 0:4b5b608f1f71 432 //pc.printf("%.2f ",emg_value4);
bouvdberg 0:4b5b608f1f71 433 //pc.printf("%.2f ",EMGlp1);
bouvdberg 0:4b5b608f1f71 434 //pc.printf("%.2f ",EMGlp2);
bouvdberg 0:4b5b608f1f71 435 //pc.printf("%.2f ",EMGlp3);
bouvdberg 0:4b5b608f1f71 436 //pc.printf("%.2f ",EMGlp4);
bouvdberg 0:4b5b608f1f71 437
bouvdberg 0:4b5b608f1f71 438 //berekenen setpoint
bouvdberg 0:4b5b608f1f71 439 //hoekinput
bouvdberg 0:4b5b608f1f71 440
bouvdberg 0:4b5b608f1f71 441 if((EMGlp1-EMGmin1)<=0.0) v1=0.0;
bouvdberg 0:4b5b608f1f71 442 else v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);
bouvdberg 0:4b5b608f1f71 443 if((EMGlp2-EMGmin2)<=0.0) v2=0.0;
bouvdberg 0:4b5b608f1f71 444 else v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);
bouvdberg 0:4b5b608f1f71 445 if((EMGlp3-EMGmin3)<=0.0) v3=0.0;
bouvdberg 0:4b5b608f1f71 446 else v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3);
bouvdberg 0:4b5b608f1f71 447 if((EMGlp4-EMGmin4)<=0.0) v4=0.0;
bouvdberg 0:4b5b608f1f71 448 else v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4);
bouvdberg 0:4b5b608f1f71 449
bouvdberg 0:4b5b608f1f71 450 t_timer=t_timer+LOOPTIME;
bouvdberg 0:4b5b608f1f71 451
bouvdberg 0:4b5b608f1f71 452 if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1 && sol_updown==1) {
bouvdberg 0:4b5b608f1f71 453 Solenoid=1; //Pen van papier
bouvdberg 0:4b5b608f1f71 454 input=0.0;
bouvdberg 0:4b5b608f1f71 455 snelheid=0.0;
bouvdberg 0:4b5b608f1f71 456 }
bouvdberg 0:4b5b608f1f71 457 else {
bouvdberg 0:4b5b608f1f71 458 Solenoid=0; //Pen op papier
bouvdberg 0:4b5b608f1f71 459 snelheid=drawspeed;
bouvdberg 0:4b5b608f1f71 460 if(v2>v1) {
bouvdberg 0:4b5b608f1f71 461 input=(atan((v3-v4)/(v1-v2))+PI);
bouvdberg 0:4b5b608f1f71 462 }
bouvdberg 0:4b5b608f1f71 463 else {
bouvdberg 0:4b5b608f1f71 464 input=(atan((v3-v4)/(v1-v2)));
bouvdberg 0:4b5b608f1f71 465 }
bouvdberg 0:4b5b608f1f71 466 pc.printf("%.2f ",input);
bouvdberg 0:4b5b608f1f71 467 }
bouvdberg 0:4b5b608f1f71 468 pc.printf("%.2f ",v1);
bouvdberg 0:4b5b608f1f71 469 pc.printf("%.2f ",v2);
bouvdberg 0:4b5b608f1f71 470 pc.printf("%.2f ",v3);
bouvdberg 0:4b5b608f1f71 471 pc.printf("%.2f \n\r",v4);
bouvdberg 0:4b5b608f1f71 472 //input=t_timer*0.8+PI;
bouvdberg 0:4b5b608f1f71 473
bouvdberg 0:4b5b608f1f71 474 //snelheidsvector met beperking positie / encoder uitlezen
bouvdberg 0:4b5b608f1f71 475 M1position = motor1.getPosition();
bouvdberg 0:4b5b608f1f71 476 M2position = motor2.getPosition();
bouvdberg 0:4b5b608f1f71 477 //M2phi=M1position-M2position+1600.0; //phi2 = phi1 - theta + 1600
bouvdberg 0:4b5b608f1f71 478
bouvdberg 0:4b5b608f1f71 479 //Px=cos((M1position/3200.0)*2.0*PI)*ARM1+cos((M2position/3200.0)*2.0*PI)*ARM2;
bouvdberg 0:4b5b608f1f71 480 //Py=sin((M1position/3200.0)*2.0*PI)*ARM1+sin((M2position/3200.0)*2.0*PI)*ARM2;
bouvdberg 0:4b5b608f1f71 481
bouvdberg 0:4b5b608f1f71 482 //if(Px > 0.29 || Px < 0.125) vx=0;
bouvdberg 0:4b5b608f1f71 483 //else
bouvdberg 0:4b5b608f1f71 484 vx=cos(input)*snelheid;
bouvdberg 0:4b5b608f1f71 485 //if(Py < -0.425 || Py > -0.125) vy=0;
bouvdberg 0:4b5b608f1f71 486 //else
bouvdberg 0:4b5b608f1f71 487 vy=sin(input)*snelheid;
bouvdberg 0:4b5b608f1f71 488
bouvdberg 0:4b5b608f1f71 489 //input positie
bouvdberg 0:4b5b608f1f71 490 phi1=(motor1.getPosition()/3200.0)*2.0*PI;
bouvdberg 0:4b5b608f1f71 491 theta=(motor2.getPosition()/3200.0)*2.0*PI;
bouvdberg 0:4b5b608f1f71 492 phi2=theta+phi1-PI;
bouvdberg 0:4b5b608f1f71 493
bouvdberg 0:4b5b608f1f71 494 //Jacobiaan
bouvdberg 0:4b5b608f1f71 495 // [a b]
bouvdberg 0:4b5b608f1f71 496 // [c d]
bouvdberg 0:4b5b608f1f71 497 a=-sin(phi1)*ARM1;
bouvdberg 0:4b5b608f1f71 498 b=-sin(phi2)*ARM2;
bouvdberg 0:4b5b608f1f71 499 c=cos(phi1)*ARM1;
bouvdberg 0:4b5b608f1f71 500 d=cos(phi2)*ARM2;
bouvdberg 0:4b5b608f1f71 501
bouvdberg 0:4b5b608f1f71 502 //inverse
bouvdberg 0:4b5b608f1f71 503 // [ai bi]
bouvdberg 0:4b5b608f1f71 504 // [ci di]
bouvdberg 0:4b5b608f1f71 505 ai=d/(a*d-b*c);
bouvdberg 0:4b5b608f1f71 506 bi=-b/(a*d-b*c);
bouvdberg 0:4b5b608f1f71 507 ci=-c/(a*d-b*c);
bouvdberg 0:4b5b608f1f71 508 di=a/(a*d-b*c);
bouvdberg 0:4b5b608f1f71 509
bouvdberg 0:4b5b608f1f71 510 //vermenigvuldiging
bouvdberg 0:4b5b608f1f71 511 // [ai bi] [vx] [w1]
bouvdberg 0:4b5b608f1f71 512 // [ci di] * [vy] = [w2]
bouvdberg 0:4b5b608f1f71 513 w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1
bouvdberg 0:4b5b608f1f71 514 w2=ci*vx+di*vy;
bouvdberg 0:4b5b608f1f71 515 wM2=w2-w1;//hoeksnelheid motor 2
bouvdberg 0:4b5b608f1f71 516
bouvdberg 0:4b5b608f1f71 517 //Beveiliging hoeksnelheden
bouvdberg 0:4b5b608f1f71 518 keep_in_range(&w1, -1000,1000);
bouvdberg 0:4b5b608f1f71 519 keep_in_range(&wM2, -1000,1000);
bouvdberg 0:4b5b608f1f71 520
bouvdberg 0:4b5b608f1f71 521 //Motoraansturing
bouvdberg 0:4b5b608f1f71 522 //t_sin=t_sin + 0.05;
bouvdberg 0:4b5b608f1f71 523 //if (t_sin>=2*PI) t_sin=0.0;
bouvdberg 0:4b5b608f1f71 524 setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1; //sin(t_sin)*1600;
bouvdberg 0:4b5b608f1f71 525 setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
bouvdberg 0:4b5b608f1f71 526
bouvdberg 0:4b5b608f1f71 527 //Beperking hoeken
bouvdberg 0:4b5b608f1f71 528 keep_in_range(&setpointM1, 0,1600);//Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
bouvdberg 0:4b5b608f1f71 529 keep_in_range(&setpointM2, 1600,3050);// Begrensd op 20 graden minimaal, werkelijke minimale waarde is 15 graden
bouvdberg 0:4b5b608f1f71 530
bouvdberg 0:4b5b608f1f71 531 foutM1 = setpointM1-M1position;
bouvdberg 0:4b5b608f1f71 532 foutM2 = setpointM2-M2position;
bouvdberg 0:4b5b608f1f71 533 //foutI1 = foutImin1 + foutM1*LOOPTIME;
bouvdberg 0:4b5b608f1f71 534 //foutI2 = foutImin2 + foutM2*LOOPTIME;
bouvdberg 0:4b5b608f1f71 535 //foutverschilM1 = foutM1-foutmin1M1;
bouvdberg 0:4b5b608f1f71 536 //foutverschilM2 = foutM2-foutmin1M2;
bouvdberg 0:4b5b608f1f71 537 //foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1;
bouvdberg 0:4b5b608f1f71 538 //foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2;
bouvdberg 0:4b5b608f1f71 539 pwm_to_motor1 = foutM1*CP1; //+foutverschilM1*CDloop+foutI1*CI;
bouvdberg 0:4b5b608f1f71 540 pwm_to_motor2 = foutM2*CP2; //+foutverschilM2*CDloop+foutI2*CI;
bouvdberg 0:4b5b608f1f71 541 keep_in_range(&pwm_to_motor1, -0.2,0.2);
bouvdberg 0:4b5b608f1f71 542 keep_in_range(&pwm_to_motor2, -0.2,0.2);
bouvdberg 0:4b5b608f1f71 543
bouvdberg 0:4b5b608f1f71 544
bouvdberg 0:4b5b608f1f71 545 if(pwm_to_motor1 > 0) {
bouvdberg 0:4b5b608f1f71 546 motordir1 = 1;
bouvdberg 0:4b5b608f1f71 547 pwm_to_motor1=pwm_to_motor1+0.06;
bouvdberg 0:4b5b608f1f71 548 }
bouvdberg 0:4b5b608f1f71 549 else {
bouvdberg 0:4b5b608f1f71 550 motordir1 = 0;
bouvdberg 0:4b5b608f1f71 551 pwm_to_motor1=pwm_to_motor1-0.06;
bouvdberg 0:4b5b608f1f71 552 }
bouvdberg 0:4b5b608f1f71 553 if(pwm_to_motor2 > 0) {
bouvdberg 0:4b5b608f1f71 554 motordir2 = 1;
bouvdberg 0:4b5b608f1f71 555 pwm_to_motor2=pwm_to_motor2+0.06;
bouvdberg 0:4b5b608f1f71 556 }
bouvdberg 0:4b5b608f1f71 557 else {
bouvdberg 0:4b5b608f1f71 558 motordir2 = 0;
bouvdberg 0:4b5b608f1f71 559 pwm_to_motor2=pwm_to_motor2-0.06;
bouvdberg 0:4b5b608f1f71 560 }
bouvdberg 0:4b5b608f1f71 561
bouvdberg 0:4b5b608f1f71 562 //WRITE VALUE TO MOTOR
bouvdberg 0:4b5b608f1f71 563 if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1)
bouvdberg 0:4b5b608f1f71 564 {
bouvdberg 0:4b5b608f1f71 565 pwm_motor1.write(0);
bouvdberg 0:4b5b608f1f71 566 pwm_motor2.write(0);
bouvdberg 0:4b5b608f1f71 567 }
bouvdberg 0:4b5b608f1f71 568 else
bouvdberg 0:4b5b608f1f71 569 {
bouvdberg 0:4b5b608f1f71 570 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:4b5b608f1f71 571 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:4b5b608f1f71 572 }
bouvdberg 0:4b5b608f1f71 573
bouvdberg 0:4b5b608f1f71 574
bouvdberg 0:4b5b608f1f71 575 //Definieren waarden in de verleden tijd
bouvdberg 0:4b5b608f1f71 576 //foutmin1M1=foutM1;
bouvdberg 0:4b5b608f1f71 577 //foutmin1M2=foutM2;
bouvdberg 0:4b5b608f1f71 578 //foutverschilmin1M1=foutverschilM1;
bouvdberg 0:4b5b608f1f71 579 //foutverschilmin1M2=foutverschilM2;
bouvdberg 0:4b5b608f1f71 580 //foutImin1=foutI1;
bouvdberg 0:4b5b608f1f71 581 //foutImin2=foutI2;
bouvdberg 0:4b5b608f1f71 582 setpointmin1M1=setpointM1;
bouvdberg 0:4b5b608f1f71 583 setpointmin1M2=setpointM2;
bouvdberg 0:4b5b608f1f71 584 emg_value1min1=emg_value1;
bouvdberg 0:4b5b608f1f71 585 emg_value2min1=emg_value2;
bouvdberg 0:4b5b608f1f71 586 emg_value3min1=emg_value3;
bouvdberg 0:4b5b608f1f71 587 emg_value4min1=emg_value4;
bouvdberg 0:4b5b608f1f71 588 EMGhp1min1=EMGhp1;
bouvdberg 0:4b5b608f1f71 589 EMGhp2min1=EMGhp2;
bouvdberg 0:4b5b608f1f71 590 EMGhp3min1=EMGhp3;
bouvdberg 0:4b5b608f1f71 591 EMGhp4min1=EMGhp4;
bouvdberg 0:4b5b608f1f71 592 EMGlp1min1=EMGlp1;
bouvdberg 0:4b5b608f1f71 593 EMGlp2min1=EMGlp2;
bouvdberg 0:4b5b608f1f71 594 EMGlp3min1=EMGlp3;
bouvdberg 0:4b5b608f1f71 595 EMGlp4min1=EMGlp4;
bouvdberg 0:4b5b608f1f71 596
bouvdberg 0:4b5b608f1f71 597
bouvdberg 0:4b5b608f1f71 598 }
bouvdberg 0:4b5b608f1f71 599 void uitzetten(void)
bouvdberg 0:4b5b608f1f71 600 {
bouvdberg 0:4b5b608f1f71 601 float BeginM1 = 800;
bouvdberg 0:4b5b608f1f71 602 float BeginM2 = 2400;
bouvdberg 0:4b5b608f1f71 603 int i_timer=800;
bouvdberg 0:4b5b608f1f71 604 Solenoid=1;
bouvdberg 0:4b5b608f1f71 605 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:4b5b608f1f71 606 {
bouvdberg 0:4b5b608f1f71 607 Ticker looptimer;
bouvdberg 0:4b5b608f1f71 608 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:4b5b608f1f71 609 while(looptimerflag != true);
bouvdberg 0:4b5b608f1f71 610 looptimerflag = false;
bouvdberg 0:4b5b608f1f71 611 M1position=motor1.getPosition();
bouvdberg 0:4b5b608f1f71 612 M2position=motor2.getPosition();
bouvdberg 0:4b5b608f1f71 613 pwm_to_motor1 = (BeginM1 - M1position)*.008;
bouvdberg 0:4b5b608f1f71 614 pwm_to_motor2 = (BeginM2 - M2position)*.008;
bouvdberg 0:4b5b608f1f71 615 keep_in_range(&pwm_to_motor1, -0.02,0.02);
bouvdberg 0:4b5b608f1f71 616 if(pwm_to_motor1 > 0)
bouvdberg 0:4b5b608f1f71 617 motordir1 = 1;
bouvdberg 0:4b5b608f1f71 618 else
bouvdberg 0:4b5b608f1f71 619 motordir1 = 0;
bouvdberg 0:4b5b608f1f71 620 keep_in_range(&pwm_to_motor2, -0.04,0.04);
bouvdberg 0:4b5b608f1f71 621 if(pwm_to_motor2 > 0)
bouvdberg 0:4b5b608f1f71 622 motordir2 = 1;
bouvdberg 0:4b5b608f1f71 623 else
bouvdberg 0:4b5b608f1f71 624 motordir2 = 0;
bouvdberg 0:4b5b608f1f71 625 //WRITE VALUE TO MOTOR
bouvdberg 0:4b5b608f1f71 626 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:4b5b608f1f71 627 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:4b5b608f1f71 628 float sent_pwm = abs(pwm_to_motor2);
bouvdberg 0:4b5b608f1f71 629
bouvdberg 0:4b5b608f1f71 630 if(i_timer<=0) break;
bouvdberg 0:4b5b608f1f71 631 i_timer--;
bouvdberg 0:4b5b608f1f71 632
bouvdberg 0:4b5b608f1f71 633 }
bouvdberg 0:4b5b608f1f71 634 pwm_motor1.write(0);
bouvdberg 0:4b5b608f1f71 635 pwm_motor2.write(0);
bouvdberg 0:4b5b608f1f71 636 motordir1 = 0;
bouvdberg 0:4b5b608f1f71 637 motordir2 = 0;
bouvdberg 0:4b5b608f1f71 638 Brake1=1;
bouvdberg 0:4b5b608f1f71 639 Brake2=1;
bouvdberg 0:4b5b608f1f71 640 wait(0.4);
bouvdberg 0:4b5b608f1f71 641 Brake1=0;
bouvdberg 0:4b5b608f1f71 642 Brake2=0;
bouvdberg 0:4b5b608f1f71 643 setpointM1=800.0;
bouvdberg 0:4b5b608f1f71 644 setpointM2=2400.0;
bouvdberg 0:4b5b608f1f71 645 setpointmin1M1=800.0;
bouvdberg 0:4b5b608f1f71 646 setpointmin1M2=2400.0;
bouvdberg 0:4b5b608f1f71 647 pwm_to_motor1=0.0;
bouvdberg 0:4b5b608f1f71 648 pwm_to_motor2=0.0;
bouvdberg 0:4b5b608f1f71 649 foutM1=0.0;
bouvdberg 0:4b5b608f1f71 650 foutM2=0.0;
bouvdberg 0:4b5b608f1f71 651 foutmin1M1=0.0;
bouvdberg 0:4b5b608f1f71 652 foutmin1M2=0.0;
bouvdberg 0:4b5b608f1f71 653 foutverschilM1=0.0;
bouvdberg 0:4b5b608f1f71 654 foutverschilM2=0.0;
bouvdberg 0:4b5b608f1f71 655 foutverschilmin1M1=0.0;
bouvdberg 0:4b5b608f1f71 656 foutverschilmin1M2=0.0;
bouvdberg 0:4b5b608f1f71 657 foutImin1=0.0;
bouvdberg 0:4b5b608f1f71 658 foutImin2=0.0;
bouvdberg 0:4b5b608f1f71 659 foutI1=0.0;
bouvdberg 0:4b5b608f1f71 660 foutI2=0.0;
bouvdberg 0:4b5b608f1f71 661 t_sin=0.0;
bouvdberg 0:4b5b608f1f71 662 t_timer=0.0;
bouvdberg 0:4b5b608f1f71 663 emg_value1min1=0.5;
bouvdberg 0:4b5b608f1f71 664 emg_value2min1=0.5;
bouvdberg 0:4b5b608f1f71 665 emg_value3min1=0.5;
bouvdberg 0:4b5b608f1f71 666 emg_value4min1=0.5;
bouvdberg 0:4b5b608f1f71 667 EMGhp1min1=0.5;
bouvdberg 0:4b5b608f1f71 668 EMGhp2min1=0.5;
bouvdberg 0:4b5b608f1f71 669 EMGhp3min1=0.5;
bouvdberg 0:4b5b608f1f71 670 EMGhp4min1=0.5;
bouvdberg 0:4b5b608f1f71 671 EMGlp1min1=0.5;
bouvdberg 0:4b5b608f1f71 672 EMGlp2min1=0.5;
bouvdberg 0:4b5b608f1f71 673 EMGlp3min1=0.5;
bouvdberg 0:4b5b608f1f71 674 EMGlp4min1=0.5;
bouvdberg 0:4b5b608f1f71 675 menu=0;
bouvdberg 0:4b5b608f1f71 676 }
bouvdberg 0:4b5b608f1f71 677
bouvdberg 0:4b5b608f1f71 678 void keep_in_range(float * in, float min, float max)
bouvdberg 0:4b5b608f1f71 679 {
bouvdberg 0:4b5b608f1f71 680 *in > min ? *in < max? : *in = max: *in = min;
bouvdberg 0:4b5b608f1f71 681 }
bouvdberg 0:4b5b608f1f71 682
bouvdberg 0:4b5b608f1f71 683 void setlooptimerflag(void)
bouvdberg 0:4b5b608f1f71 684 {
bouvdberg 0:4b5b608f1f71 685 looptimerflag = true;
bouvdberg 0:4b5b608f1f71 686 }