jacco ton / Mbed 2 deprecated TweeDraaipunten

script dat de twee motoren los van elkaar laat draaien

Dependencies:   mbed Encoder MODSERIAL

main.cpp@4:3f2f48dad1ff, 2013-11-06 (annotated)

Committer:
jaccoton
Date:
Wed Nov 06 16:33:03 2013 +0000
Revision:
4:3f2f48dad1ff
Parent:
3:4db5e8ed9657 
script potmeter uitleg opnieuw

Who changed what in which revision?

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