Diff: main.cpp

Revision:

0:ba744421930f

Child:

1:5deb5092d487

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 05 09:21:38 2013 +0000
@@ -0,0 +1,410 @@
+#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);
+    
+
+    /*floats van de filters van het emg*/
+    pc.baud(115200);   // Defining the Baud rate for comunication with PC
+    /*EMG Variables 1*/
+    float t;
+        t = 0;
+        
+    float hoek1, hoek2;
+    hoek1 = 0;
+    hoek2 = 0;
+    /*Floats voor Maximal value*/
+    
+    float h,h1,hh,hh1,b,bb;
+    h=0;
+    h1=0;
+    hh=0;
+    hh1=0;
+    b=0;
+    bb=0;
+    
+    
+    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.2)
+        f=0;
+        else 
+        f=f;
+        
+         //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.2)
+        ff=0;
+        else 
+        ff=ff;
+        /* 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;
+        if (fff<0.2)
+        fff=0;
+        else 
+        fff=fff;
+        /* 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;
+        if (ffff<0.2)
+        ffff=0;
+        
+        
+        else 
+        ffff=ffff;
+        //Printing the Filtered singnal
+        //pc.printf("%f \n \r",f);
+
+        wait(Ts);
+        /*Defining the direction of the Motor*/
+        emg_dir=f;/*-ff; snelheid in de x richting. Of draaiing motor 1*/
+        emg_dir2=fff;/*-ffff; /*snelheid in de y richting. Of draaiing motor2*/
+        
+        //omzetten naar hoeksnelheden
+        //hoekV1 = emg_dir*29.5*cos(hoek2)-emg_dir2*29,5*sin(hoek2); //nu heb ik beide hoeksnelheden
+        //hoekV2 = -emg_dir*21,5*cos(hoek1)+emg_dir2*21,5*sin(hoek1);
+        
+         
+        /*emgPosx1 = emg_dir2*Ts + emgPos10;
+        emgPos2 = emg_dir2*Ts + emgPos20;
+        emgPos10 = emgPos1;
+        emgPos20 = emgPos2;*/
+        
+        /* transformatie van xy naar hoek1 en hoek2*/
+        
+        //t+=Ts;
+        /*Defining which way to turn*/
+        
+        
+        if (f>0.2){         //Defining a threshhold abvoe which theres unlikely to measure any activity of other muscles
+        if (f>h);           //Looking for the biggest value of f
+        h=f;                //Storing it in h 
+        if (h1<h)           //if h is a value below the set maximum
+        h1=h1+0.001;        //the variable of the setpoint will try to narrow it in steps of 0.005
+        }
+        
+        if (ff>0.2){
+        if (ff>b)
+        b=(1-ff);
+        if (h1>b)
+        h1=h1-0.001;
+        }
+        keep_in_range(&h1,0,1);
+        
+        if (fff>0.2){
+        if (fff>hh);
+        hh=fff;
+        if (hh1<hh)
+        hh1=hh1+0.005;
+        }
+        
+        if (ffff>0.2){
+        if (ffff>bb)
+        bb=(1-ffff);
+        if (hh1>bb)
+        hh1=hh1-0.005;
+        }
+        keep_in_range(&hh1,0,1);
+        //emgPosx = 0.5*sin(t);
+        //emgPosy = 0.5*cos(t);
+        
+        //emgPos1 = 
+        //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*/
+        pc.printf("%f \n \r",f);
+        setpoint = (h1*1226.55); //emgPos moet wel tussen 0 en 1 zitte?
+        setpoint2 = (hh1*1226.55);
+        /*setpoint2 = (potmeter2.read())*1226,55; ELLEBOOG kan van 0 tot 102 graden draaien*/
+       
+        // 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(abs(pwm_to_motor2));
+        //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());
+         //pc.printf("f:%f, ff:%f, h1:%f, h:%f, b:%f, Pos:%f \n \r",f,ff,h1,h,b,motor2.getPosition());
+    }
+}
+
+
+//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;
+}
\ No newline at end of file