Boudewijn van den Berg / Mbed 2 deprecated EMGverwerking

Aansturing

Dependencies:   Encoder MODSERIAL mbed

main.cpp@1:adc1d0589d54, 2013-10-29 (annotated)

Committer:
bouvdberg
Date:
Tue Oct 29 16:09:09 2013 +0000
Revision:
1:adc1d0589d54
Parent:
0:ba5ff341b020 
Nieuwe versie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouvdberg 0:ba5ff341b020 1 #include "mbed.h"
bouvdberg 0:ba5ff341b020 2 #include "MODSERIAL.h"
bouvdberg 0:ba5ff341b020 3 #include "encoder.h"
bouvdberg 0:ba5ff341b020 4
bouvdberg 0:ba5ff341b020 5 // definieren constanten
bouvdberg 0:ba5ff341b020 6 #define PI 3.1415926
bouvdberg 0:ba5ff341b020 7 //plant
bouvdberg 0:ba5ff341b020 8 #define ARM1 0.36
bouvdberg 0:ba5ff341b020 9 #define ARM2 0.18
bouvdberg 0:ba5ff341b020 10 //PD
bouvdberg 0:ba5ff341b020 11 #define CP 0.0002
bouvdberg 0:ba5ff341b020 12 #define CD 0.002
bouvdberg 0:ba5ff341b020 13 #define CLP1 0.9975
bouvdberg 0:ba5ff341b020 14 #define CLP2 0.001
bouvdberg 0:ba5ff341b020 15 //Snelheid
bouvdberg 0:ba5ff341b020 16 #define SNELHEID 0.05
bouvdberg 0:ba5ff341b020 17 //LOOPTIME
bouvdberg 0:ba5ff341b020 18 #define LOOPTIME 0.01
bouvdberg 0:ba5ff341b020 19 //Filtering EMG
bouvdberg 0:ba5ff341b020 20 #define HP1 0.8187
bouvdberg 0:ba5ff341b020 21 #define HP2 20.0
bouvdberg 0:ba5ff341b020 22 #define HP3 20.0
bouvdberg 0:ba5ff341b020 23 #define LP1 0.9512
bouvdberg 0:ba5ff341b020 24 #define LP2 0.04877
bouvdberg 0:ba5ff341b020 25
bouvdberg 0:ba5ff341b020 26 void keep_in_range(float * in, float min, float max);
bouvdberg 0:ba5ff341b020 27
bouvdberg 0:ba5ff341b020 28 volatile bool looptimerflag;
bouvdberg 0:ba5ff341b020 29
bouvdberg 0:ba5ff341b020 30 void setlooptimerflag(void)
bouvdberg 0:ba5ff341b020 31 {
bouvdberg 0:ba5ff341b020 32 looptimerflag = true;
bouvdberg 0:ba5ff341b020 33 }
bouvdberg 0:ba5ff341b020 34
bouvdberg 0:ba5ff341b020 35 int main() {
bouvdberg 0:ba5ff341b020 36 AnalogIn EMG0(PTB0);
bouvdberg 0:ba5ff341b020 37 AnalogIn EMG1(PTB1);
bouvdberg 0:ba5ff341b020 38 AnalogIn EMG2(PTB2);
bouvdberg 0:ba5ff341b020 39 AnalogIn EMG3(PTB3);
bouvdberg 0:ba5ff341b020 40 AnalogIn potmeter(PTC2);
bouvdberg 0:ba5ff341b020 41 DigitalIn ButtonUP(PTE20); //Knopjes voor kalibratie
bouvdberg 0:ba5ff341b020 42 DigitalIn ButtonDOWN(PTE21);
bouvdberg 0:ba5ff341b020 43 DigitalIn ButtonSELECT(PTE22);
bouvdberg 0:ba5ff341b020 44 DigitalIn ButtonSTOP(PTE23);
bouvdberg 0:ba5ff341b020 45 DigitalOut Solenoid(PTD4); //Solenoid
bouvdberg 1:adc1d0589d54 46 Encoder motor1(PTD0,PTD2);//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
bouvdberg 0:ba5ff341b020 47 Encoder motor2(;
bouvdberg 0:ba5ff341b020 48 PwmOut pwm_motor1(PTA12);
bouvdberg 0:ba5ff341b020 49 DigitalOut motordir1(PTD3);
bouvdberg 0:ba5ff341b020 50 PwmOut pwm_motor2(PTA5);
bouvdberg 0:ba5ff341b020 51 DigitalOut motordir2(PTD1);
bouvdberg 0:ba5ff341b020 52
bouvdberg 1:adc1d0589d54 53 void motor1.setPosition(1050) //UITZOEKEN HOE DIT WEL MOET: NU FOUTMELDING!!!!!!!!!!!!!!!!!!!!!!!!!!
bouvdberg 1:adc1d0589d54 54 void motor2.setPosition(190)
bouvdberg 1:adc1d0589d54 55
bouvdberg 0:ba5ff341b020 56 //Variabelen verwerking EMG
bouvdberg 0:ba5ff341b020 57 float emg_value1, emg_value2, emg_value3, emg_value4;
bouvdberg 0:ba5ff341b020 58 float emg_value1min1=0.0, emg_value2min1=0.0, emg_value3min1=0.0, emg_value4min1=0.0;
bouvdberg 0:ba5ff341b020 59 float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4;
bouvdberg 0:ba5ff341b020 60 float EMGhp1min1=0.0, EMGhp2min1=0.0, EMGhp3min1=0.0, EMGhp4min1=0.0, EMGlp1min1=0.0, EMGlp2min1=0.0, EMGlp3min1=0.0, EMGlp4min1=0.0;
bouvdberg 0:ba5ff341b020 61 float EMGmax1, EMGmin1, EMGmax2, EMGmin2, EMGmax3, EMGmin3, EMGmax4, EMGmin4;
bouvdberg 0:ba5ff341b020 62
bouvdberg 0:ba5ff341b020 63 //Variabelen bepaling input systeem
bouvdberg 0:ba5ff341b020 64 float input;
bouvdberg 0:ba5ff341b020 65 float w1, w2, wM2, phi1, phi2, theta;
bouvdberg 0:ba5ff341b020 66 float a, b, c, d, ai, bi, ci, di;
bouvdberg 1:adc1d0589d54 67 float v1, v2, v3, v4, vx, vy, snelheid;
bouvdberg 1:adc1d0589d54 68 float M1position, M2position, M2phi;
bouvdberg 1:adc1d0589d54 69 float Px, Py;
bouvdberg 0:ba5ff341b020 70
bouvdberg 0:ba5ff341b020 71 //Variabelen motoraansturing
bouvdberg 0:ba5ff341b020 72 float setpointM1, setpointM2;
bouvdberg 0:ba5ff341b020 73 float setpointmin1M1=0.0, setpointmin1M2=0.0;
bouvdberg 0:ba5ff341b020 74 float pwm_to_motor1, pwm_to_motor2;
bouvdberg 0:ba5ff341b020 75 float foutM1, foutM2;
bouvdberg 0:ba5ff341b020 76 float foutmin1M1=0.0, foutmin1M2=0.0;
bouvdberg 0:ba5ff341b020 77 float foutverschilM1, foutverschilM2;
bouvdberg 0:ba5ff341b020 78 float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0;
bouvdberg 0:ba5ff341b020 79 float CDloop=CD/LOOPTIME;
bouvdberg 0:ba5ff341b020 80
bouvdberg 0:ba5ff341b020 81 //Kalibratie
bouvdberg 0:ba5ff341b020 82 //1. Bepalen EMGmax1 EMGmin1
bouvdberg 0:ba5ff341b020 83 //2. Bepalen EMGmax2 EMGmin2
bouvdberg 0:ba5ff341b020 84 //3. Bepalen EMGmax3 EMGmin3
bouvdberg 0:ba5ff341b020 85 //4. Bepalen EMGmax4 EMGmin4
bouvdberg 1:adc1d0589d54 86
bouvdberg 1:adc1d0589d54 87
bouvdberg 1:adc1d0589d54 88
bouvdberg 0:ba5ff341b020 89
bouvdberg 0:ba5ff341b020 90 //Aansturing
bouvdberg 0:ba5ff341b020 91 Ticker looptimer;
bouvdberg 0:ba5ff341b020 92 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:ba5ff341b020 93 while(1) {
bouvdberg 0:ba5ff341b020 94 while(looptimerflag != true);
bouvdberg 0:ba5ff341b020 95 looptimerflag = false;
bouvdberg 0:ba5ff341b020 96
bouvdberg 0:ba5ff341b020 97 //uitlezen
bouvdberg 0:ba5ff341b020 98 emg_value1 = EMG0.read();
bouvdberg 0:ba5ff341b020 99 emg_value2 = EMG1.read();
bouvdberg 0:ba5ff341b020 100 emg_value3 = EMG2.read();
bouvdberg 0:ba5ff341b020 101 emg_value4 = EMG3.read();
bouvdberg 0:ba5ff341b020 102
bouvdberg 0:ba5ff341b020 103 //filtering
bouvdberg 0:ba5ff341b020 104 EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
bouvdberg 0:ba5ff341b020 105 EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
bouvdberg 0:ba5ff341b020 106 EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
bouvdberg 0:ba5ff341b020 107 EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
bouvdberg 0:ba5ff341b020 108 EMGhp1=abs(EMGhp1);
bouvdberg 0:ba5ff341b020 109 EMGhp2=abs(EMGhp2);
bouvdberg 0:ba5ff341b020 110 EMGhp3=abs(EMGhp3);
bouvdberg 0:ba5ff341b020 111 EMGhp4=abs(EMGhp4);
bouvdberg 0:ba5ff341b020 112 EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
bouvdberg 0:ba5ff341b020 113 EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
bouvdberg 0:ba5ff341b020 114 EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
bouvdberg 0:ba5ff341b020 115 EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
bouvdberg 0:ba5ff341b020 116
bouvdberg 0:ba5ff341b020 117 //berekenen setpoint
bouvdberg 0:ba5ff341b020 118 //hoekinput
bouvdberg 0:ba5ff341b020 119 v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);
bouvdberg 1:adc1d0589d54 120 if((EMGlp1-EMGmin1)<0.0)
bouvdberg 1:adc1d0589d54 121 v1=0.0;
bouvdberg 1:adc1d0589d54 122 else
bouvdberg 1:adc1d0589d54 123 v1=v1;
bouvdberg 0:ba5ff341b020 124 v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);
bouvdberg 1:adc1d0589d54 125 if((EMGlp2-EMGmin2)<0.0)
bouvdberg 1:adc1d0589d54 126 v2=0.0;
bouvdberg 1:adc1d0589d54 127 else
bouvdberg 1:adc1d0589d54 128 v2=v2;
bouvdberg 1:adc1d0589d54 129 v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3)
bouvdberg 1:adc1d0589d54 130 if((EMGlp3-EMGmin3)<0.0)
bouvdberg 1:adc1d0589d54 131 v3=0.0;
bouvdberg 1:adc1d0589d54 132 else
bouvdberg 1:adc1d0589d54 133 v3=v3;
bouvdberg 0:ba5ff341b020 134 v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4);
bouvdberg 1:adc1d0589d54 135 if((EMGlp4-EMGmin4)<0.0)
bouvdberg 1:adc1d0589d54 136 v4=0.0;
bouvdberg 1:adc1d0589d54 137 else
bouvdberg 1:adc1d0589d54 138 v4=v4;
bouvdberg 1:adc1d0589d54 139
bouvdberg 1:adc1d0589d54 140 if(v1=0.0 && v2=0.0 && v3=0.0 && v4=0.0) {
bouvdberg 1:adc1d0589d54 141 P_solenoid=0; //Uitzoeken welke klasse dit moet zijn!!!!!!!!!!!!!!!!
bouvdberg 1:adc1d0589d54 142 input=0.0;
bouvdberg 1:adc1d0589d54 143 snelheid=0.0;
bouvdberg 1:adc1d0589d54 144 }
bouvdberg 1:adc1d0589d54 145 else {
bouvdberg 1:adc1d0589d54 146 P_solenoid=1;
bouvdberg 1:adc1d0589d54 147 snelheid=SNELHEID;
bouvdberg 1:adc1d0589d54 148 input=tan((v1-v2)/(v3-v4));
bouvdberg 1:adc1d0589d54 149 }
bouvdberg 0:ba5ff341b020 150
bouvdberg 1:adc1d0589d54 151 //snelheidsvector met beperking positie
bouvdberg 1:adc1d0589d54 152 M1position = motor1.getPosition();
bouvdberg 1:adc1d0589d54 153 M2position = motor2.getPosition();
bouvdberg 1:adc1d0589d54 154 M2phi=M1position-M2position+1600.0;
bouvdberg 1:adc1d0589d54 155
bouvdberg 1:adc1d0589d54 156 Px=cos((M1position/3200.0)*2.0*PI)*ARM1+cos((M2position/3200.0)*2.0*PI)*ARM2;
bouvdberg 1:adc1d0589d54 157 Py=sin((M1position/3200.0)*2.0*PI)*ARM1+sin((M2position/3200.0)*2.0*PI)*ARM2;
bouvdberg 1:adc1d0589d54 158
bouvdberg 1:adc1d0589d54 159 if(Px > 0.29 || Px < 0.125)
bouvdberg 1:adc1d0589d54 160 vx=0;
bouvdberg 1:adc1d0589d54 161 else
bouvdberg 1:adc1d0589d54 162 vx=cos(input)*snelheid;
bouvdberg 1:adc1d0589d54 163
bouvdberg 1:adc1d0589d54 164 if(Py < -0.425 || Py > -0.125)
bouvdberg 1:adc1d0589d54 165 vy=0;
bouvdberg 1:adc1d0589d54 166 else
bouvdberg 1:adc1d0589d54 167 vy=sin(input)*snelheid;
bouvdberg 0:ba5ff341b020 168
bouvdberg 0:ba5ff341b020 169 //input positie
bouvdberg 1:adc1d0589d54 170 phi1=motor1.getPosition();
bouvdberg 0:ba5ff341b020 171 theta=motor2.getPosition();
bouvdberg 0:ba5ff341b020 172 phi2=theta+phi1-PI;
bouvdberg 0:ba5ff341b020 173
bouvdberg 0:ba5ff341b020 174 //Jacobiaan
bouvdberg 0:ba5ff341b020 175 // [a b]
bouvdberg 0:ba5ff341b020 176 // [c d]
bouvdberg 1:adc1d0589d54 177 a=-sin(phi1)*ARM1;
bouvdberg 1:adc1d0589d54 178 b=cos(phi1)*ARM1;
bouvdberg 1:adc1d0589d54 179 c=-sin(phi2)*ARM2;
bouvdberg 1:adc1d0589d54 180 d=cos(phi2)*ARM2;
bouvdberg 0:ba5ff341b020 181
bouvdberg 0:ba5ff341b020 182 //inverse
bouvdberg 0:ba5ff341b020 183 // [ai bi]
bouvdberg 0:ba5ff341b020 184 // [ci di]
bouvdberg 0:ba5ff341b020 185 ai=d/(a*d-b*c);
bouvdberg 0:ba5ff341b020 186 bi=-b/(a*d-b*c);
bouvdberg 0:ba5ff341b020 187 ci=-c/(a*d-b*c);
bouvdberg 0:ba5ff341b020 188 di=a/(a*d-b*c);
bouvdberg 0:ba5ff341b020 189
bouvdberg 0:ba5ff341b020 190 //vermenigvuldiging
bouvdberg 0:ba5ff341b020 191 // [ai bi] [vx] [w1]
bouvdberg 0:ba5ff341b020 192 // [ci di] * [vy] = [w2]
bouvdberg 0:ba5ff341b020 193 w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1
bouvdberg 0:ba5ff341b020 194 w2=ci*vx+di*vy;
bouvdberg 0:ba5ff341b020 195 wM2=w2-w1;//hoeksnelheid motor 2
bouvdberg 1:adc1d0589d54 196
bouvdberg 1:adc1d0589d54 197 //Beveiliging hoeksnelheden
bouvdberg 1:adc1d0589d54 198 keep_in_range(&w1, 200,1400);
bouvdberg 0:ba5ff341b020 199
bouvdberg 0:ba5ff341b020 200 //Motoraansturing
bouvdberg 0:ba5ff341b020 201 setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1;
bouvdberg 0:ba5ff341b020 202 setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
bouvdberg 0:ba5ff341b020 203 foutM1 = setpointM1-M1position;
bouvdberg 0:ba5ff341b020 204 foutM2 = setpointM2-M2position;
bouvdberg 0:ba5ff341b020 205 foutverschilM1 = foutM1-foutmin1M1;
bouvdberg 0:ba5ff341b020 206 foutverschilM2 = foutM2-foutmin1M2;
bouvdberg 0:ba5ff341b020 207 foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1;
bouvdberg 0:ba5ff341b020 208 foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2;
bouvdberg 0:ba5ff341b020 209 pwm_to_motor1 = foutM1*CP+foutverschilM1*CDloop;
bouvdberg 0:ba5ff341b020 210 pwm_to_motor2 = foutM2*CP+foutverschilM2*CDloop;
bouvdberg 0:ba5ff341b020 211 keep_in_range(&pwm_to_motor1, -1,1);
bouvdberg 0:ba5ff341b020 212 keep_in_range(&pwm_to_motor2, -1,1);
bouvdberg 1:adc1d0589d54 213
bouvdberg 1:adc1d0589d54 214 //Beperking hoeken
bouvdberg 1:adc1d0589d54 215 keep_in_range(&setpointM1, 200,1400);//Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
bouvdberg 1:adc1d0589d54 216 keep_in_range(&setpointM2, 178,1550);// Begrensd op 20 graden minimaal, werkelijke minimale waarde is 15 graden
bouvdberg 1:adc1d0589d54 217
bouvdberg 0:ba5ff341b020 218 if(pwm_to_motor1 > 0)
bouvdberg 0:ba5ff341b020 219 motordir1 = 0;
bouvdberg 0:ba5ff341b020 220 else
bouvdberg 0:ba5ff341b020 221 motordir1 = 1;
bouvdberg 0:ba5ff341b020 222 if(pwm_to_motor2 > 0)
bouvdberg 0:ba5ff341b020 223 motordir2 = 0;
bouvdberg 0:ba5ff341b020 224 else
bouvdberg 0:ba5ff341b020 225 motordir2 = 1;
bouvdberg 0:ba5ff341b020 226
bouvdberg 1:adc1d0589d54 227
bouvdberg 0:ba5ff341b020 228 //WRITE VALUE TO MOTOR
bouvdberg 0:ba5ff341b020 229 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:ba5ff341b020 230 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:ba5ff341b020 231
bouvdberg 0:ba5ff341b020 232 //Definieren waarden in de verleden tijd
bouvdberg 0:ba5ff341b020 233 foutmin1M1=foutM1;
bouvdberg 0:ba5ff341b020 234 foutmin1M2=foutM2;
bouvdberg 0:ba5ff341b020 235 foutverschilmin1M1=foutverschilM1;
bouvdberg 0:ba5ff341b020 236 foutverschilmin1M2=foutverschilM2;
bouvdberg 0:ba5ff341b020 237 setpointmin1M1=setpointM1;
bouvdberg 0:ba5ff341b020 238 setpointmin1M2=setpointM2;
bouvdberg 0:ba5ff341b020 239 }
bouvdberg 0:ba5ff341b020 240 }
bouvdberg 0:ba5ff341b020 241
bouvdberg 0:ba5ff341b020 242 void keep_in_range(float * in, float min, float max)
bouvdberg 0:ba5ff341b020 243 {
bouvdberg 0:ba5ff341b020 244 *in > min ? *in < max? : *in = max: *in = min;
bouvdberg 0:ba5ff341b020 245 }