jacco ton / Mbed 2 deprecated Marian

groep6 K9

Dependencies:   Encoder MODSERIAL mbed

main.cpp@0:605ed03bf57b, 2013-11-04 (annotated)

Committer:
jaccoton
Date:
Mon Nov 04 13:51:50 2013 +0000
Revision:
0:605ed03bf57b
Groep6. we willen de output van de emg integreren om er een veranderende setpoint van te maken. Dit doen we tussen regel 234 en 250 in het script. De uitkomst is echter dat de setpoint alleen maar groter wordt. Misschien gaat het ergens anders mis?

Who changed what in which revision?

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