jacco ton
groep6 K9
Dependencies: Encoder MODSERIAL mbed
Diff: main.cpp
- Revision:
- 0:605ed03bf57b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Nov 04 13:51:50 2013 +0000
@@ -0,0 +1,334 @@
+#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;
+}