groep6 K9
Dependencies: Encoder MODSERIAL mbed
main.cpp
- Committer:
- jaccoton
- Date:
- 2013-11-04
- Revision:
- 0:605ed03bf57b
File content as of revision 0:605ed03bf57b:
#include "mbed.h" #include "encoder.h" #include "MODSERIAL.h" /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ void keep_in_range(float * in, float min, float max); /** variable to show when a new loop can be started*/ /** volatile means that it can be changed in an */ /** interrupt routine, and that that change is vis-*/ /** ible in the main loop. */ volatile bool looptimerflag; /** function called by Ticker "looptimer" */ /** variable 'looptimerflag' is set to 'true' */ /** each time the looptimer expires. */ void setlooptimerflag(void) { looptimerflag = true; } int main() { //LOCAL VARIABLES /*Potmeter input*/ //AnalogIn potmeter(PTC2); //AnalogIn potmeter2(PTB0); //EMG input AnalogIn emg0(PTB0); AnalogIn emg1(PTB1); AnalogIn emg2(PTB2); AnalogIn emg3(PTB3); /* Encoder, using my encoder library */ /* First pin should be PTDx or PTAx */ /* because those pins can be used as */ /* InterruptIn */ Encoder motor1(PTD0,PTC9); Encoder motor2(PTD2,PTC8); /* MODSERIAL to get non-blocking Serial*/ MODSERIAL pc(USBTX,USBRX); /* PWM control to motor */ PwmOut pwm_motor(PTA12); PwmOut pwm_motor2(PTA5); /* Direction pin */ DigitalOut motordir(PTD3); DigitalOut motordir2(PTD1); Ticker emgtimer; /*floats van de filters van het emg*/ pc.baud(115200); // Defining the Baud rate for comunication with PC /*EMG Variables 1*/ float x,y,z,x1,x2,y1,z1,y2,z2,f,f1,f2,s,s1 ; //Defining variables for filter x1=0; // setting the variables to zero so they don't x2=0; // have any value jet and are used to store older values later y1=0; y2=0; z1=0; z2=0; f1=0; f2=0; s=0; s1=0; const float Ts = 0.01; //Defining time steps /*EMG Variables 2*/ float xx,yy,zz,xx1,xx2,yy1,zz1,yy2,zz2,ff,ff1,ff2 ; //Defining variables for filter xx1=0; // setting the variables to zero so they don't xx2=0; // have any value jet and are used to store older values later yy1=0; yy2=0; zz1=0; zz2=0; ff1=0; ff2=0; /*EMG Variables 3*/ float xxx,yyy,zzz,xxx1,xxx2,yyy1,zzz1,yyy2,zzz2,fff,fff1,fff2 ; //Defining variables for filter xxx1=0; // setting the variables to zero so they don't xxx2=0; // have any value jet and are used to store older values later yyy1=0; yyy2=0; zzz1=0; zzz2=0; fff1=0; fff2=0; /*EMG Variables 4*/ float xxxx,yyyy,zzzz,xxxx1,xxxx2,yyyy1,zzzz1,yyyy2,zzzz2,ffff,ffff1,ffff2 ; //Defining variables for filter xxxx1=0; // setting the variables to zero so they don't xxxx2=0; // have any value jet and are used to store older values later yyyy1=0; yyyy2=0; zzzz1=0; zzzz2=0; ffff1=0; ffff2=0; /*Variables to store direction in dependent on the EMG*/ float emg_dir; float emg_dir2; float emgPos1, emgPos2, emgPos10, emgPos20; emgPos10 = 0; emgPos20 = 0; /* variable to store setpoint in */ float setpoint; float setpoint2; /* variable to store pwm value in*/ float pwm_to_motor; float pwm_to_motor2; /* Alle waardes voor de regelaar benoemen. Moet nog afgesteld worden*/ float u, u1, e, e1, ui, ei, up, ei1, ed, ud; float u2, u12, e2, e12, ui2, ei2, up2, ei12, ed2, ud2; const float kp = 0.001;//0.02438; //test value 0.001 const float ki = 0.00001;//0.11661; //test value 0.00001 const float kd = 0.00001;//0.00071; //test value 0.00001 //const float Ts = 0.01; const float kp2 = 0.001;//0.02274; //test value 0.001 const float ki2 = 0.00001;//0.10879; //test value 0.00001 const float kd2 = 0.00001;//0.00065; //test value 0.00001 //const float Ts2 = 0.1; e1 = 0; u1 = 0; ei1 = 0; e12 = 0; u12 = 0; ei12 = 0; //START OF CODE /*Set the baudrate (use this number in RealTerm too! */ //pc.baud(115200); /*Create a ticker, and let it call the */ /*function 'setlooptimerflag' every 0.01s */ Ticker looptimer; looptimer.attach(setlooptimerflag,Ts); //INFINITE LOOP while(1) { /* Wait until looptimer flag is true. */ /* '!=' means not-is-equal */ while(looptimerflag != true); /* Clear the looptimerflag, otherwise */ /* the program would simply continue */ /* without waitingin the next iteration*/ looptimerflag = false; /*while loop voor de emg*/ /* EMG Filter 1*/ x = emg0.read(); //Reading EMG value 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 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 z = abs(z); //Rectify the signal f = z*0.0036+z1*0.0072+z2*0.0036-f1*-1.8227-f2*0.8372; // low pass filter at 5Hz z1 = z; // Store older values of variables z2 = z1; y2 = y1; y1 = y; x1 = x; x2 = x1; f1 = f; f2 = f1; if (f<0.035) f=0; //pc.printf("%f \n \r",(s1*f)); /* EMG Filter 2*/ xx = emg1.read(); //Reading EMG value 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 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 zz = abs(zz); //Rectify the signal ff = zz*0.0036+zz1*0.0072+zz2*0.0036-ff1*-1.8227-ff2*0.8372; // low pass filter at 5Hz zz1 = zz; // Store older values of variables zz2 = zz1; yy2 = yy1; yy1 = yy; xx1 = xx; xx2 = xx1; ff1 = ff; ff2 = ff1; if (ff<0.035) ff=0; /* EMG Filter 3*/ xxx = emg2.read(); //Reading EMG value 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 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 zzz = abs(zzz); //Rectify the signal fff = zzz*0.0036+zzz1*0.0072+zzz2*0.0036-fff1*-1.8227-fff2*0.8372; // low pass filter at 5Hz zzz1 = zzz; // Store older values of variables zzz2 = zzz1; yyy2 = yyy1; yyy1 = yyy; xxx1 = xxx; xxx2 = xxx1; fff1 = fff; fff2 = fff1; /* EMG Filter 4*/ xxxx = emg3.read(); //Reading EMG value 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 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 zzzz = abs(zzzz); //Rectify the signal ffff = zzzz*0.0036+zzzz1*0.0072+zzzz2*0.0036-ffff1*-1.8227-ffff2*0.8372; // low pass filter at 5Hz zzzz1 = zzzz; // Store older values of variables zzzz2 = zzzz1; yyyy2 = yyyy1; yyyy1 = yyyy; xxxx1 = xxxx; xxxx2 = xxxx1; ffff1 = ffff; ffff2 = ffff1; //Printing the Filtered singnal //pc.printf("%f \n \r",f); wait(Ts); /*Defining the direction of the Motor*/ emg_dir2=f-ff; /*bijvoorbeeld de x richting of hoe we de motor willen draaien. Dit moeten we kiezen*/ emg_dir=fff-ffff; emgPos1 = emg_dir*Ts + emgPos10; emgPos2 = emg_dir2*Ts + emgPos20; emgPos10 = emgPos1; emgPos20 = emgPos2; //pc.printf("%f \n \r",emgPos1); /* Read EMDG values, apply some math */ /* to get useful setpoint value */ /*setpoint = ((ff)-0.2255)*1226.55;*/ /* SHOUDER kan van -23 tot 79 graden draaien*/ setpoint = (emgPos1*1226.55); setpoint2 = (emgPos2*1226.55); /*setpoint2 = (potmeter2.read())*1226,55; ELLEBOOG kan van 0 tot 102 graden draaien*/ pc.printf("%f, %f \n \r",emgPos1, setpoint); // Print setpoint and current position to serial terminal //pc.printf("%f,%f,%f,%d \n \r",f,ff,emg_dir,motordir.read()); /*pc.printf("%f, %f \r\n", motor2.getSpeed(),f);*/ /*pc.printf(" %f, %d, %f, %d \n\r", setpoint, motor1.getPosition(), setpoint2, motor2.getPosition());*/ /*pc.printf("s: %f, %d \n\r", setpoint2, motor2.getPosition());*/ /*This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor*/ /* De PID berekeningen*/ e = setpoint - motor1.getPosition(); up = kp*e; ei = e*Ts + ei1; ui = ki*ei; keep_in_range(&ui, -0.5*setpoint,0.5*setpoint); ed = (e-e1)/Ts; ud = ed*kd; u = up + ui + ud; /*pwm_to_motor = 0;*/ pwm_to_motor = u; u1= u; e1= e; ei1 = ei; /* Het is nu een PID actie geworden voor motor 2*/ e2 = setpoint2 - motor2.getPosition(); up2 = kp2*e2; ei2 = e2*Ts + ei12; ui2 = ki2*ei2; keep_in_range(&ui, -0.5*setpoint2,0.5*setpoint2); ed2 = (e2-e12)/Ts; ud2 = ed2*kd2; u2 = up2 + ui2 + ud2; /*u = (kp+ki*Ts)*e-kp*e1+u1;*/ pwm_to_motor2 = u2; /*pwm_to_motor2 = 0;*/ u12= u2; e12= e2; ei12 = ei2; //Make sure the PWM stays in range keep_in_range(&pwm_to_motor, -1,1); keep_in_range(&pwm_to_motor2, -1,1); /* Control the motor direction pin. based on */ /* the sign of your pwm value. If your */ /* motor keeps spinning when running this code */ /* you probably need to swap the motor wires, */ /* or swap the 'write(1)' and 'write(0)' below */ if(pwm_to_motor > 0){ //if (pwm_to_motor > 0) emg_dir > 0{ motordir.write(1); } else{ motordir.write(0); } //WRITE VALUE TO MOTOR /* Take the absolute value of the PWM to send */ /* to the motor. */ pwm_motor.write(abs(pwm_to_motor));//pwm_motor.write(0); if(pwm_to_motor2 > 0){ //if (pwm_to_motor2 > 0)emg_dir2 > 0{ motordir2.write(1); } else{ motordir2.write(0); } //pwm_motor2.write(abs(pwm_to_motor2)); pwm_motor2.write(0); //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()); } } //coerces value 'in' to min or max when exceeding those values //if you'd like to understand the statement below take a google for //'ternary operators'. void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; }