jacco ton / Mbed 2 deprecated TweeDraaippunten4EMG

script uitleg

Dependencies:   Encoder MODSERIAL mbed

main.cpp@2:4ae32e8863d5, 2013-11-06 (annotated)

Committer:
jaccoton
Date:
Wed Nov 06 16:16:10 2013 +0000
Revision:
2:4ae32e8863d5
Parent:
1:5deb5092d487 
Uitleg van het script

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Chordmaster 0:ba744421930f 1 #include "mbed.h"
Chordmaster 0:ba744421930f 2 #include "encoder.h"
Chordmaster 0:ba744421930f 3 #include "MODSERIAL.h"
Chordmaster 0:ba744421930f 4
Chordmaster 0:ba744421930f 5 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
Chordmaster 0:ba744421930f 6 void keep_in_range(float * in, float min, float max);
Chordmaster 0:ba744421930f 7
Chordmaster 0:ba744421930f 8 /** variable to show when a new loop can be started*/
Chordmaster 0:ba744421930f 9 /** volatile means that it can be changed in an */
Chordmaster 0:ba744421930f 10 /** interrupt routine, and that that change is vis-*/
Chordmaster 0:ba744421930f 11 /** ible in the main loop. */
Chordmaster 0:ba744421930f 12
Chordmaster 0:ba744421930f 13 volatile bool looptimerflag;
Chordmaster 0:ba744421930f 14
Chordmaster 0:ba744421930f 15 /** function called by Ticker "looptimer" */
Chordmaster 0:ba744421930f 16 /** variable 'looptimerflag' is set to 'true' */
Chordmaster 0:ba744421930f 17 /** each time the looptimer expires. */
Chordmaster 0:ba744421930f 18 void setlooptimerflag(void)
Chordmaster 0:ba744421930f 19 {
Chordmaster 0:ba744421930f 20 looptimerflag = true;
Chordmaster 0:ba744421930f 21 }
Chordmaster 0:ba744421930f 22
Chordmaster 0:ba744421930f 23
Chordmaster 0:ba744421930f 24 int main()
Chordmaster 0:ba744421930f 25 {
Chordmaster 0:ba744421930f 26
Chordmaster 0:ba744421930f 27 //LOCAL VARIABLES
Chordmaster 0:ba744421930f 28 /*Potmeter input*/
Chordmaster 0:ba744421930f 29 //AnalogIn potmeter(PTC2);
Chordmaster 0:ba744421930f 30 //AnalogIn potmeter2(PTB0);
Chordmaster 0:ba744421930f 31
Chordmaster 0:ba744421930f 32 //EMG input
Chordmaster 0:ba744421930f 33 AnalogIn emg0(PTB0);
Chordmaster 0:ba744421930f 34 AnalogIn emg1(PTB1);
Chordmaster 0:ba744421930f 35 AnalogIn emg2(PTB2);
Chordmaster 0:ba744421930f 36 AnalogIn emg3(PTB3);
Chordmaster 0:ba744421930f 37 /* Encoder, using my encoder library */
Chordmaster 0:ba744421930f 38 /* First pin should be PTDx or PTAx */
Chordmaster 0:ba744421930f 39 /* because those pins can be used as */
Chordmaster 0:ba744421930f 40 /* InterruptIn */
Chordmaster 0:ba744421930f 41 Encoder motor1(PTD0,PTC9);
Chordmaster 0:ba744421930f 42 Encoder motor2(PTD2,PTC8);
Chordmaster 0:ba744421930f 43 /* MODSERIAL to get non-blocking Serial*/
Chordmaster 0:ba744421930f 44 MODSERIAL pc(USBTX,USBRX);
Chordmaster 0:ba744421930f 45 /* PWM control to motor */
Chordmaster 0:ba744421930f 46 PwmOut pwm_motor(PTA12);
Chordmaster 0:ba744421930f 47 PwmOut pwm_motor2(PTA5);
Chordmaster 0:ba744421930f 48 /* Direction pin */
Chordmaster 0:ba744421930f 49 DigitalOut motordir(PTD3);
Chordmaster 0:ba744421930f 50 DigitalOut motordir2(PTD1);
Chordmaster 0:ba744421930f 51
Chordmaster 0:ba744421930f 52
Chordmaster 0:ba744421930f 53 /*floats van de filters van het emg*/
Chordmaster 0:ba744421930f 54 pc.baud(115200); // Defining the Baud rate for comunication with PC
Chordmaster 0:ba744421930f 55 /*EMG Variables 1*/
Chordmaster 0:ba744421930f 56 float t;
Chordmaster 0:ba744421930f 57 t = 0;
Chordmaster 0:ba744421930f 58
Chordmaster 0:ba744421930f 59 float hoek1, hoek2;
Chordmaster 0:ba744421930f 60 hoek1 = 0;
Chordmaster 0:ba744421930f 61 hoek2 = 0;
Chordmaster 0:ba744421930f 62 /*Floats voor Maximal value*/
Chordmaster 0:ba744421930f 63
Chordmaster 0:ba744421930f 64 float h,h1,hh,hh1,b,bb;
Chordmaster 0:ba744421930f 65 h=0;
Chordmaster 0:ba744421930f 66 h1=0;
Chordmaster 0:ba744421930f 67 hh=0;
Chordmaster 0:ba744421930f 68 hh1=0;
Chordmaster 0:ba744421930f 69 b=0;
Chordmaster 0:ba744421930f 70 bb=0;
Chordmaster 0:ba744421930f 71
Chordmaster 0:ba744421930f 72
Chordmaster 0:ba744421930f 73 float x,y,z,x1,x2,y1,z1,y2,z2,f,f1,f2,s,s1 ; //Defining variables for filter
Chordmaster 0:ba744421930f 74 x1=0; // setting the variables to zero so they don't
Chordmaster 0:ba744421930f 75 x2=0; // have any value jet and are used to store older values later
Chordmaster 0:ba744421930f 76 y1=0;
Chordmaster 0:ba744421930f 77 y2=0;
Chordmaster 0:ba744421930f 78 z1=0;
Chordmaster 0:ba744421930f 79 z2=0;
Chordmaster 0:ba744421930f 80 f1=0;
Chordmaster 0:ba744421930f 81 f2=0;
Chordmaster 0:ba744421930f 82 s=0;
Chordmaster 0:ba744421930f 83 s1=0;
Chordmaster 0:ba744421930f 84 const float Ts = 0.01; //Defining time steps
Chordmaster 0:ba744421930f 85
Chordmaster 0:ba744421930f 86 /*EMG Variables 2*/
Chordmaster 0:ba744421930f 87 float xx,yy,zz,xx1,xx2,yy1,zz1,yy2,zz2,ff,ff1,ff2 ; //Defining variables for filter
Chordmaster 0:ba744421930f 88 xx1=0; // setting the variables to zero so they don't
Chordmaster 0:ba744421930f 89 xx2=0; // have any value jet and are used to store older values later
Chordmaster 0:ba744421930f 90 yy1=0;
Chordmaster 0:ba744421930f 91 yy2=0;
Chordmaster 0:ba744421930f 92 zz1=0;
Chordmaster 0:ba744421930f 93 zz2=0;
Chordmaster 0:ba744421930f 94 ff1=0;
Chordmaster 0:ba744421930f 95 ff2=0;
Chordmaster 0:ba744421930f 96
Chordmaster 0:ba744421930f 97 /*EMG Variables 3*/
Chordmaster 0:ba744421930f 98 float xxx,yyy,zzz,xxx1,xxx2,yyy1,zzz1,yyy2,zzz2,fff,fff1,fff2 ; //Defining variables for filter
Chordmaster 0:ba744421930f 99 xxx1=0; // setting the variables to zero so they don't
Chordmaster 0:ba744421930f 100 xxx2=0; // have any value jet and are used to store older values later
Chordmaster 0:ba744421930f 101 yyy1=0;
Chordmaster 0:ba744421930f 102 yyy2=0;
Chordmaster 0:ba744421930f 103 zzz1=0;
Chordmaster 0:ba744421930f 104 zzz2=0;
Chordmaster 0:ba744421930f 105 fff1=0;
Chordmaster 0:ba744421930f 106 fff2=0;
Chordmaster 0:ba744421930f 107
Chordmaster 0:ba744421930f 108 /*EMG Variables 4*/
Chordmaster 0:ba744421930f 109 float xxxx,yyyy,zzzz,xxxx1,xxxx2,yyyy1,zzzz1,yyyy2,zzzz2,ffff,ffff1,ffff2 ; //Defining variables for filter
Chordmaster 0:ba744421930f 110 xxxx1=0; // setting the variables to zero so they don't
Chordmaster 0:ba744421930f 111 xxxx2=0; // have any value jet and are used to store older values later
Chordmaster 0:ba744421930f 112 yyyy1=0;
Chordmaster 0:ba744421930f 113 yyyy2=0;
Chordmaster 0:ba744421930f 114 zzzz1=0;
Chordmaster 0:ba744421930f 115 zzzz2=0;
Chordmaster 0:ba744421930f 116 ffff1=0;
Chordmaster 0:ba744421930f 117 ffff2=0;
jaccoton 2:4ae32e8863d5 118 float F,FF,FFF,FFFF;
Chordmaster 0:ba744421930f 119 /*Variables to store direction in dependent on the EMG*/
Chordmaster 0:ba744421930f 120 float emg_dir;
Chordmaster 0:ba744421930f 121 float emg_dir2;
Chordmaster 0:ba744421930f 122 float emgPos1, emgPos2, emgPos10, emgPos20;
Chordmaster 0:ba744421930f 123 emgPos10 = 0;
Chordmaster 0:ba744421930f 124 emgPos20 = 0;
Chordmaster 0:ba744421930f 125
Chordmaster 0:ba744421930f 126 /* variable to store setpoint in */
Chordmaster 0:ba744421930f 127 float setpoint;
Chordmaster 0:ba744421930f 128 float setpoint2;
Chordmaster 0:ba744421930f 129
Chordmaster 0:ba744421930f 130 /* variable to store pwm value in*/
Chordmaster 0:ba744421930f 131 float pwm_to_motor;
Chordmaster 0:ba744421930f 132 float pwm_to_motor2;
Chordmaster 0:ba744421930f 133
Chordmaster 0:ba744421930f 134
Chordmaster 0:ba744421930f 135 /* Alle waardes voor de regelaar benoemen. Moet nog afgesteld worden*/
Chordmaster 0:ba744421930f 136 float u, u1, e, e1, ui, ei, up, ei1, ed, ud;
Chordmaster 0:ba744421930f 137 float u2, u12, e2, e12, ui2, ei2, up2, ei12, ed2, ud2;
Chordmaster 0:ba744421930f 138 const float kp = 0.001;//0.02438; //test value 0.001
Chordmaster 0:ba744421930f 139 const float ki = 0.00001;//0.11661; //test value 0.00001
Chordmaster 0:ba744421930f 140 const float kd = 0.00001;//0.00071; //test value 0.00001
Chordmaster 0:ba744421930f 141 //const float Ts = 0.01;
Chordmaster 0:ba744421930f 142 const float kp2 = 0.001;//0.02274; //test value 0.001
Chordmaster 0:ba744421930f 143 const float ki2 = 0.00001;//0.10879; //test value 0.00001
Chordmaster 0:ba744421930f 144 const float kd2 = 0.00001;//0.00065; //test value 0.00001
Chordmaster 0:ba744421930f 145 //const float Ts2 = 0.1;
Chordmaster 0:ba744421930f 146 e1 = 0;
Chordmaster 0:ba744421930f 147 u1 = 0;
Chordmaster 0:ba744421930f 148 ei1 = 0;
Chordmaster 0:ba744421930f 149 e12 = 0;
Chordmaster 0:ba744421930f 150 u12 = 0;
Chordmaster 0:ba744421930f 151 ei12 = 0;
Chordmaster 0:ba744421930f 152 //START OF CODE
Chordmaster 0:ba744421930f 153
Chordmaster 0:ba744421930f 154 /*Set the baudrate (use this number in RealTerm too! */
Chordmaster 0:ba744421930f 155 //pc.baud(115200);
Chordmaster 0:ba744421930f 156
Chordmaster 0:ba744421930f 157 /*Create a ticker, and let it call the */
Chordmaster 0:ba744421930f 158 /*function 'setlooptimerflag' every 0.01s */
Chordmaster 0:ba744421930f 159 Ticker looptimer;
Chordmaster 0:ba744421930f 160 looptimer.attach(setlooptimerflag,Ts);
Chordmaster 0:ba744421930f 161
Chordmaster 0:ba744421930f 162
Chordmaster 0:ba744421930f 163 //INFINITE LOOP
Chordmaster 0:ba744421930f 164 while(1) {
Chordmaster 0:ba744421930f 165 /* Wait until looptimer flag is true. */
Chordmaster 0:ba744421930f 166 /* '!=' means not-is-equal */
Chordmaster 0:ba744421930f 167 while(looptimerflag != true);
Chordmaster 0:ba744421930f 168 /* Clear the looptimerflag, otherwise */
Chordmaster 0:ba744421930f 169 /* the program would simply continue */
Chordmaster 0:ba744421930f 170 /* without waitingin the next iteration*/
Chordmaster 0:ba744421930f 171 looptimerflag = false;
Chordmaster 0:ba744421930f 172
Chordmaster 0:ba744421930f 173 /*while loop voor de emg*/
Chordmaster 0:ba744421930f 174
Chordmaster 0:ba744421930f 175 /* EMG Filter 1*/
Chordmaster 0:ba744421930f 176 x = emg0.read(); //Reading EMG value
Chordmaster 0:ba744421930f 177 y = 0.6389*x+1.2779*x1+0.6389*x2-y1*1.143-y2*0.4128; //Formula for highpass filter at 20Hz as given in slides
Chordmaster 0:ba744421930f 178 z = y*0.3913+y1*-0.7827+y2*0.3913-z1*-0.3695-z2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
Chordmaster 0:ba744421930f 179 z = abs(z); //Rectify the signal
Chordmaster 0:ba744421930f 180 f = z*0.0036+z1*0.0072+z2*0.0036-f1*-1.8227-f2*0.8372; // low pass filter at 5Hz
jaccoton 2:4ae32e8863d5 181 F = f;
Chordmaster 0:ba744421930f 182
Chordmaster 0:ba744421930f 183
Chordmaster 0:ba744421930f 184 z1 = z; // Store older values of variables
Chordmaster 0:ba744421930f 185 z2 = z1;
Chordmaster 0:ba744421930f 186 y2 = y1;
Chordmaster 0:ba744421930f 187 y1 = y;
Chordmaster 0:ba744421930f 188 x1 = x;
Chordmaster 0:ba744421930f 189 x2 = x1;
Chordmaster 0:ba744421930f 190 f1 = f;
Chordmaster 0:ba744421930f 191 f2 = f1;
Chordmaster 0:ba744421930f 192
jaccoton 2:4ae32e8863d5 193 if (f<0.2)
Chordmaster 0:ba744421930f 194 f=0;
Chordmaster 0:ba744421930f 195 else
jaccoton 2:4ae32e8863d5 196 f=f;
Chordmaster 0:ba744421930f 197
Chordmaster 0:ba744421930f 198 //pc.printf("%f \n \r",(s1*f));
Chordmaster 0:ba744421930f 199
Chordmaster 0:ba744421930f 200 /* EMG Filter 2*/
Chordmaster 0:ba744421930f 201 xx = emg1.read(); //Reading EMG value
Chordmaster 0:ba744421930f 202 yy = 0.6389*xx+1.2779*xx1+0.6389*xx2-yy1*1.143-yy2*0.4128; //Formula for highpass filter at 20Hz as given in slides
Chordmaster 0:ba744421930f 203 zz = yy*0.3913+yy1*-0.7827+yy2*0.3913-zz1*-0.3695-zz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
Chordmaster 0:ba744421930f 204 zz = abs(zz); //Rectify the signal
Chordmaster 0:ba744421930f 205 ff = zz*0.0036+zz1*0.0072+zz2*0.0036-ff1*-1.8227-ff2*0.8372; // low pass filter at 5Hz
jaccoton 2:4ae32e8863d5 206 FF = ff;
Chordmaster 0:ba744421930f 207
Chordmaster 0:ba744421930f 208 zz1 = zz; // Store older values of variables
Chordmaster 0:ba744421930f 209 zz2 = zz1;
Chordmaster 0:ba744421930f 210 yy2 = yy1;
Chordmaster 0:ba744421930f 211 yy1 = yy;
Chordmaster 0:ba744421930f 212 xx1 = xx;
Chordmaster 0:ba744421930f 213 xx2 = xx1;
Chordmaster 0:ba744421930f 214 ff1 = ff;
Chordmaster 0:ba744421930f 215 ff2 = ff1;
jaccoton 2:4ae32e8863d5 216 if (ff<0.2)
Chordmaster 0:ba744421930f 217 ff=0;
Chordmaster 0:ba744421930f 218 else
jaccoton 2:4ae32e8863d5 219 ff=ff;
Chordmaster 0:ba744421930f 220 /* EMG Filter 3*/
Chordmaster 0:ba744421930f 221 xxx = emg2.read(); //Reading EMG value
Chordmaster 0:ba744421930f 222 yyy = 0.6389*xxx+1.2779*xxx1+0.6389*xxx2-yyy1*1.143-yyy2*0.4128; //Formula for highpass filter at 20Hz as given in slides
Chordmaster 0:ba744421930f 223 zzz = yyy*0.3913+yyy1*-0.7827+yyy2*0.3913-zzz1*-0.3695-zzz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
Chordmaster 0:ba744421930f 224 zzz = abs(zzz); //Rectify the signal
Chordmaster 0:ba744421930f 225 fff = zzz*0.0036+zzz1*0.0072+zzz2*0.0036-fff1*-1.8227-fff2*0.8372; // low pass filter at 5Hz
jaccoton 2:4ae32e8863d5 226 FFF = fff;
Chordmaster 0:ba744421930f 227
Chordmaster 0:ba744421930f 228 zzz1 = zzz; // Store older values of variables
Chordmaster 0:ba744421930f 229 zzz2 = zzz1;
Chordmaster 0:ba744421930f 230 yyy2 = yyy1;
Chordmaster 0:ba744421930f 231 yyy1 = yyy;
Chordmaster 0:ba744421930f 232 xxx1 = xxx;
Chordmaster 0:ba744421930f 233 xxx2 = xxx1;
Chordmaster 0:ba744421930f 234 fff1 = fff;
Chordmaster 0:ba744421930f 235 fff2 = fff1;
jaccoton 1:5deb5092d487 236 if (fff<0.1)
Chordmaster 0:ba744421930f 237 fff=0;
Chordmaster 0:ba744421930f 238 else
Chordmaster 0:ba744421930f 239 fff=fff;
Chordmaster 0:ba744421930f 240 /* EMG Filter 4*/
Chordmaster 0:ba744421930f 241 xxxx = emg3.read(); //Reading EMG value
Chordmaster 0:ba744421930f 242 yyyy = 0.6389*xxxx+1.2779*xxxx1+0.6389*xxxx2-yyyy1*1.143-yyyy2*0.4128; //Formula for highpass filter at 20Hz as given in slides
Chordmaster 0:ba744421930f 243 zzzz = yyyy*0.3913+yyyy1*-0.7827+yyyy2*0.3913-zzzz1*-0.3695-zzzz2*0.1958; // Formula for low pass filter at 40Hz instead of a Notch filter
Chordmaster 0:ba744421930f 244 zzzz = abs(zzzz); //Rectify the signal
Chordmaster 0:ba744421930f 245 ffff = zzzz*0.0036+zzzz1*0.0072+zzzz2*0.0036-ffff1*-1.8227-ffff2*0.8372; // low pass filter at 5Hz
jaccoton 2:4ae32e8863d5 246 FFFF = ffff;
Chordmaster 0:ba744421930f 247
Chordmaster 0:ba744421930f 248 zzzz1 = zzzz; // Store older values of variables
Chordmaster 0:ba744421930f 249 zzzz2 = zzzz1;
Chordmaster 0:ba744421930f 250 yyyy2 = yyyy1;
Chordmaster 0:ba744421930f 251 yyyy1 = yyyy;
Chordmaster 0:ba744421930f 252 xxxx1 = xxxx;
Chordmaster 0:ba744421930f 253 xxxx2 = xxxx1;
Chordmaster 0:ba744421930f 254 ffff1 = ffff;
Chordmaster 0:ba744421930f 255 ffff2 = ffff1;
jaccoton 1:5deb5092d487 256 if (ffff<0.1)
Chordmaster 0:ba744421930f 257 ffff=0;
Chordmaster 0:ba744421930f 258
Chordmaster 0:ba744421930f 259
Chordmaster 0:ba744421930f 260 else
Chordmaster 0:ba744421930f 261 ffff=ffff;
Chordmaster 0:ba744421930f 262 //Printing the Filtered singnal
Chordmaster 0:ba744421930f 263 //pc.printf("%f \n \r",f);
Chordmaster 0:ba744421930f 264
Chordmaster 0:ba744421930f 265 wait(Ts);
Chordmaster 0:ba744421930f 266 /*Defining the direction of the Motor*/
Chordmaster 0:ba744421930f 267 emg_dir=f;/*-ff; snelheid in de x richting. Of draaiing motor 1*/
Chordmaster 0:ba744421930f 268 emg_dir2=fff;/*-ffff; /*snelheid in de y richting. Of draaiing motor2*/
Chordmaster 0:ba744421930f 269
Chordmaster 0:ba744421930f 270 //omzetten naar hoeksnelheden
Chordmaster 0:ba744421930f 271 //hoekV1 = emg_dir*29.5*cos(hoek2)-emg_dir2*29,5*sin(hoek2); //nu heb ik beide hoeksnelheden
Chordmaster 0:ba744421930f 272 //hoekV2 = -emg_dir*21,5*cos(hoek1)+emg_dir2*21,5*sin(hoek1);
Chordmaster 0:ba744421930f 273
Chordmaster 0:ba744421930f 274
Chordmaster 0:ba744421930f 275 /*emgPosx1 = emg_dir2*Ts + emgPos10;
Chordmaster 0:ba744421930f 276 emgPos2 = emg_dir2*Ts + emgPos20;
Chordmaster 0:ba744421930f 277 emgPos10 = emgPos1;
Chordmaster 0:ba744421930f 278 emgPos20 = emgPos2;*/
Chordmaster 0:ba744421930f 279
Chordmaster 0:ba744421930f 280 /* transformatie van xy naar hoek1 en hoek2*/
Chordmaster 0:ba744421930f 281
Chordmaster 0:ba744421930f 282 //t+=Ts;
Chordmaster 0:ba744421930f 283 /*Defining which way to turn*/
Chordmaster 0:ba744421930f 284
Chordmaster 0:ba744421930f 285
Chordmaster 0:ba744421930f 286 if (f>0.2){ //Defining a threshhold abvoe which theres unlikely to measure any activity of other muscles
Chordmaster 0:ba744421930f 287 if (f>h); //Looking for the biggest value of f
jaccoton 2:4ae32e8863d5 288 h=(f); //Storing it in h
Chordmaster 0:ba744421930f 289 if (h1<h) //if h is a value below the set maximum
Chordmaster 0:ba744421930f 290 h1=h1+0.001; //the variable of the setpoint will try to narrow it in steps of 0.005
Chordmaster 0:ba744421930f 291 }
Chordmaster 0:ba744421930f 292
Chordmaster 0:ba744421930f 293 if (ff>0.2){
Chordmaster 0:ba744421930f 294 if (ff>b)
Chordmaster 0:ba744421930f 295 b=(1-ff);
Chordmaster 0:ba744421930f 296 if (h1>b)
Chordmaster 0:ba744421930f 297 h1=h1-0.001;
Chordmaster 0:ba744421930f 298 }
Chordmaster 0:ba744421930f 299 keep_in_range(&h1,0,1);
Chordmaster 0:ba744421930f 300
Chordmaster 0:ba744421930f 301 if (fff>0.2){
Chordmaster 0:ba744421930f 302 if (fff>hh);
Chordmaster 0:ba744421930f 303 hh=fff;
Chordmaster 0:ba744421930f 304 if (hh1<hh)
Chordmaster 0:ba744421930f 305 hh1=hh1+0.005;
Chordmaster 0:ba744421930f 306 }
Chordmaster 0:ba744421930f 307
Chordmaster 0:ba744421930f 308 if (ffff>0.2){
Chordmaster 0:ba744421930f 309 if (ffff>bb)
Chordmaster 0:ba744421930f 310 bb=(1-ffff);
Chordmaster 0:ba744421930f 311 if (hh1>bb)
Chordmaster 0:ba744421930f 312 hh1=hh1-0.005;
Chordmaster 0:ba744421930f 313 }
Chordmaster 0:ba744421930f 314 keep_in_range(&hh1,0,1);
Chordmaster 0:ba744421930f 315 //emgPosx = 0.5*sin(t);
Chordmaster 0:ba744421930f 316 //emgPosy = 0.5*cos(t);
Chordmaster 0:ba744421930f 317
Chordmaster 0:ba744421930f 318 //emgPos1 =
Chordmaster 0:ba744421930f 319 //emgPos2 =
Chordmaster 0:ba744421930f 320
Chordmaster 0:ba744421930f 321
Chordmaster 0:ba744421930f 322 //pc.printf("%f \n \r",emgPos1);
Chordmaster 0:ba744421930f 323 /* Read EMDG values, apply some math */
Chordmaster 0:ba744421930f 324 /* to get useful setpoint value */
Chordmaster 0:ba744421930f 325 /*setpoint = ((ff)-0.2255)*1226.55;*/ /* SHOUDER kan van -23 tot 79 graden draaien*/
jaccoton 1:5deb5092d487 326 pc.printf("%f,%f \n \r",f,ff);
Chordmaster 0:ba744421930f 327 setpoint = (h1*1226.55); //emgPos moet wel tussen 0 en 1 zitte?
Chordmaster 0:ba744421930f 328 setpoint2 = (hh1*1226.55);
Chordmaster 0:ba744421930f 329 /*setpoint2 = (potmeter2.read())*1226,55; ELLEBOOG kan van 0 tot 102 graden draaien*/
Chordmaster 0:ba744421930f 330
Chordmaster 0:ba744421930f 331 // Print setpoint and current position to serial terminal
Chordmaster 0:ba744421930f 332 //pc.printf("%f,%f,%f,%d \n \r",f,ff,emg_dir,motordir.read());
Chordmaster 0:ba744421930f 333 /*pc.printf("%f, %f \r\n", motor2.getSpeed(),f);*/
Chordmaster 0:ba744421930f 334 /*pc.printf(" %f, %d, %f, %d \n\r", setpoint, motor1.getPosition(), setpoint2, motor2.getPosition());*/
Chordmaster 0:ba744421930f 335 /*pc.printf("s: %f, %d \n\r", setpoint2, motor2.getPosition());*/
Chordmaster 0:ba744421930f 336
Chordmaster 0:ba744421930f 337 /*This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor*/
Chordmaster 0:ba744421930f 338
Chordmaster 0:ba744421930f 339 /* De PID berekeningen*/
Chordmaster 0:ba744421930f 340 e = setpoint - motor1.getPosition();
Chordmaster 0:ba744421930f 341 up = kp*e;
Chordmaster 0:ba744421930f 342 ei = e*Ts + ei1;
Chordmaster 0:ba744421930f 343 ui = ki*ei;
Chordmaster 0:ba744421930f 344 keep_in_range(&ui, -0.5*setpoint,0.5*setpoint);
Chordmaster 0:ba744421930f 345 ed = (e-e1)/Ts;
Chordmaster 0:ba744421930f 346 ud = ed*kd;
Chordmaster 0:ba744421930f 347 u = up + ui + ud;
Chordmaster 0:ba744421930f 348 /*pwm_to_motor = 0;*/
Chordmaster 0:ba744421930f 349 pwm_to_motor = u;
Chordmaster 0:ba744421930f 350
Chordmaster 0:ba744421930f 351 u1= u;
Chordmaster 0:ba744421930f 352 e1= e;
Chordmaster 0:ba744421930f 353 ei1 = ei;
Chordmaster 0:ba744421930f 354
Chordmaster 0:ba744421930f 355 /* Het is nu een PID actie geworden voor motor 2*/
Chordmaster 0:ba744421930f 356 e2 = setpoint2 - motor2.getPosition();
Chordmaster 0:ba744421930f 357 up2 = kp2*e2;
Chordmaster 0:ba744421930f 358 ei2 = e2*Ts + ei12;
Chordmaster 0:ba744421930f 359 ui2 = ki2*ei2;
Chordmaster 0:ba744421930f 360 keep_in_range(&ui, -0.5*setpoint2,0.5*setpoint2);
Chordmaster 0:ba744421930f 361 ed2 = (e2-e12)/Ts;
Chordmaster 0:ba744421930f 362 ud2 = ed2*kd2;
Chordmaster 0:ba744421930f 363 u2 = up2 + ui2 + ud2;
Chordmaster 0:ba744421930f 364 /*u = (kp+ki*Ts)*e-kp*e1+u1;*/
Chordmaster 0:ba744421930f 365 pwm_to_motor2 = u2;
Chordmaster 0:ba744421930f 366 /*pwm_to_motor2 = 0;*/
Chordmaster 0:ba744421930f 367
Chordmaster 0:ba744421930f 368 u12= u2;
Chordmaster 0:ba744421930f 369 e12= e2;
Chordmaster 0:ba744421930f 370 ei12 = ei2;
Chordmaster 0:ba744421930f 371
Chordmaster 0:ba744421930f 372 //Make sure the PWM stays in range
Chordmaster 0:ba744421930f 373 keep_in_range(&pwm_to_motor, -1,1);
Chordmaster 0:ba744421930f 374 keep_in_range(&pwm_to_motor2, -1,1);
Chordmaster 0:ba744421930f 375
Chordmaster 0:ba744421930f 376 /* Control the motor direction pin. based on */
Chordmaster 0:ba744421930f 377 /* the sign of your pwm value. If your */
Chordmaster 0:ba744421930f 378 /* motor keeps spinning when running this code */
Chordmaster 0:ba744421930f 379 /* you probably need to swap the motor wires, */
Chordmaster 0:ba744421930f 380 /* or swap the 'write(1)' and 'write(0)' below */
Chordmaster 0:ba744421930f 381 if(pwm_to_motor > 0){ //if (pwm_to_motor > 0) emg_dir > 0{
Chordmaster 0:ba744421930f 382 motordir.write(1);
Chordmaster 0:ba744421930f 383 }
Chordmaster 0:ba744421930f 384 else{
Chordmaster 0:ba744421930f 385 motordir.write(0);
Chordmaster 0:ba744421930f 386 }
Chordmaster 0:ba744421930f 387
Chordmaster 0:ba744421930f 388 //WRITE VALUE TO MOTOR
Chordmaster 0:ba744421930f 389 /* Take the absolute value of the PWM to send */
Chordmaster 0:ba744421930f 390 /* to the motor. */
Chordmaster 0:ba744421930f 391 pwm_motor.write(abs(pwm_to_motor));//pwm_motor.write(0);
Chordmaster 0:ba744421930f 392
Chordmaster 0:ba744421930f 393 if(pwm_to_motor2 > 0){ //if (pwm_to_motor2 > 0)emg_dir2 > 0{
Chordmaster 0:ba744421930f 394 motordir2.write(1);
Chordmaster 0:ba744421930f 395 }
Chordmaster 0:ba744421930f 396 else{
Chordmaster 0:ba744421930f 397 motordir2.write(0);
Chordmaster 0:ba744421930f 398 }
Chordmaster 0:ba744421930f 399 //pwm_motor2.write(abs(pwm_to_motor2));
Chordmaster 0:ba744421930f 400 pwm_motor2.write(abs(pwm_to_motor2));
Chordmaster 0:ba744421930f 401 //pc.printf("f:%f, emg_dir:%f, setpoint:%f, u:%f, pwm_to_motor:%d, motordir:%d \n \r",f,emg_dir,setpoint,u,pwm_to_motor,motordir.read());
Chordmaster 0:ba744421930f 402 //pc.printf("f:%f, ff:%f, h1:%f, h:%f, b:%f, Pos:%f \n \r",f,ff,h1,h,b,motor2.getPosition());
Chordmaster 0:ba744421930f 403 }
Chordmaster 0:ba744421930f 404 }
Chordmaster 0:ba744421930f 405
Chordmaster 0:ba744421930f 406
Chordmaster 0:ba744421930f 407 //coerces value 'in' to min or max when exceeding those values
Chordmaster 0:ba744421930f 408 //if you'd like to understand the statement below take a google for
Chordmaster 0:ba744421930f 409 //'ternary operators'.
Chordmaster 0:ba744421930f 410 void keep_in_range(float * in, float min, float max)
Chordmaster 0:ba744421930f 411 {
Chordmaster 0:ba744421930f 412 *in > min ? *in < max? : *in = max: *in = min;
Chordmaster 0:ba744421930f 413 }