jacco ton / Mbed 2 deprecated BackupDing

jo

Dependencies:   mbed

main.cpp@1:a79ae3f2da21, 2013-11-06 (annotated)

Committer:
jaccoton
Date:
Wed Nov 06 16:20:43 2013 +0000
Revision:
1:a79ae3f2da21
Parent:
0:ef25f3cccede 
uitleg potmeter script

Who changed what in which revision?

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