Revision 0:605ed03bf57b, committed 2013-11-04

Comitter:

jaccoton

Date:

Mon Nov 04 13:51:50 2013 +0000

Commit message:

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?

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.lib	Mon Nov 04 13:51:50 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/vsluiter/code/Encoder/#2dd7853c911a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Mon Nov 04 13:51:50 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MODSERIAL/#b04ce87dc424

--- /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;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Nov 04 13:51:50 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file