Diff: PROJECT_main.cpp

Revision:

0:99cbc87af37c

Child:

1:d44a866de64f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PROJECT_main.cpp	Fri Oct 31 10:31:02 2014 +0000
@@ -0,0 +1,644 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "HIDScope.h"
+#include "arm_math.h"
+#include "encoder.h"
+
+#define TSAMP 0.001 // sample freq encoder motor
+#define TIMEB4NEXTCHOICE 1  // sec keuzelampje blijft aan
+#define TIMEBETWEENBLINK 200 // sec voor volgende blink
+#define TSAMP_EMG 0.002 //sample frequency emg
+#define KALIBRATIONTIME 1000 // 10 sec voor bepalen van maximale biceps/triceps waarde
+
+//Define objects
+AnalogIn    emg0(PTB1);         //Analog input biceps
+AnalogIn    emg1(PTB2);         //Analog input triceps
+
+Ticker log_timer;       //sample emg
+Ticker blink;           //ledjes aan/uit   
+Ticker looptimer;       //motor regelaar
+
+MODSERIAL pc(USBTX,USBRX);
+
+arm_biquad_casd_df1_inst_f32 bihighpass;
+float bihighpass_const[] = {0.8751821104711265,  -1.750364220942253,  0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
+float bihighpass_states[4];
+
+arm_biquad_casd_df1_inst_f32 binotch;
+float binotch_const[] = {0.9714498065192796,  -1.5718388053127037,  0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
+float binotch_states[4];
+
+arm_biquad_casd_df1_inst_f32 trihighpass;
+float trihighpass_const[] = {0.8751821104711265,  -1.750364220942253,  0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071
+float trihighpass_states[4];
+
+arm_biquad_casd_df1_inst_f32 trinotch;
+float trinotch_const[] = {0.9714498065192796,  -1.5718388053127037,  0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10
+float trinotch_states[4];
+
+float bi_result = 0;
+float tri_result = 0;
+
+float bi_max = 0;
+float tri_max = 0;
+
+// variables for biceps MAF
+float y0 = 0;
+float y1 = 0;
+float y2 = 0;
+float y3 = 0;
+float y4 = 0;
+float y5 = 0;
+float y6 = 0;
+float y7 = 0;
+float y8 = 0;
+float y9 = 0;
+
+// variables for triceps MAF
+float x0 = 0;
+float x1 = 0;
+float x2 = 0;
+float x3 = 0;
+float x4 = 0;
+float x5 = 0;
+float x6 = 0;
+float x7 = 0;
+float x8 = 0;
+float x9 = 0;
+
+//LED interface
+DigitalOut dir1(PTA1);
+DigitalOut dir2(PTA2);
+DigitalOut dir3(PTD4);
+DigitalOut for1(PTA12);
+DigitalOut for2(PTA13);
+DigitalOut for3(PTD1);
+
+uint8_t         direction = 0;
+uint8_t         force = 0;
+
+//motorcontrol objects
+
+//motor 1, voltage pins op M2
+Encoder motor1(PTD3, PTD5);
+DigitalOut motor1dir(PTC9);
+PwmOut pwm_motor1(PTC8);
+
+//motor 2, voltage pins op M1
+Encoder motor2(PTD2,PTD0);
+DigitalOut motor2dir(PTA4);
+PwmOut pwm_motor2(PTA5);
+
+float integral = 0;
+float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
+float balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
+float controlerror = 0;
+float pwm = 0;
+
+float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
+float omrekenfactor2 =  0.0015213178; // 6.28/(24*172);
+
+float setpoint1 = 8; // te behalen speed van motor1 (37D)
+float setpoint2 = 3.14; // te behalen hoek van motor2 (25D)
+
+//float setpoint1 = 0; eigenlijk moeten deze waarden later in de if-statement bij motorcontrol bepaald worden
+//float setpoint2 = 0;
+
+float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
+float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag
+
+float Kp2 = 0.30; //Kp en Ki van motor2, voor in het positie brengen en voor de return
+float Ki2 = 0.20;
+
+float Kp3 = 0.09; //Kp en Ki van motor1, voor de return
+float Ki3 = 0.05;
+
+volatile bool looptimerflag; //voor motorcontrol TSAMP
+
+//functies
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+
+}
+
+void keep_in_range(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = max;
+}
+
+void looper()
+{
+    //put raw emg value of biceps and triceps in emg_biceps and emg_triceps, respectively
+    float emg_biceps;         //Float voor EMG-waarde biceps
+    float emg_triceps;        //Float voor EMG-waarde triceps
+
+    emg_biceps = emg0.read();         // read float value (0..1 = 0..3.3V) biceps
+    emg_triceps = emg1.read();        // read float value (0..1 = 0..3.3V) triceps
+
+    //process emg biceps
+    arm_biquad_cascade_df1_f32(&bihighpass, &emg_biceps, &emg_biceps, 1 );
+    arm_biquad_cascade_df1_f32(&binotch, &emg_biceps, &emg_biceps, 1 );
+    y0 = fabs(emg_biceps);
+    bi_result = (y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1);
+    y9=y8;
+    y8=y7;
+    y7=y6;
+    y6=y5;
+    y5=y4;
+    y4=y3;
+    y3=y2;
+    y2=y1;
+    y1=y0;
+
+    //process emg triceps
+    arm_biquad_cascade_df1_f32(&trihighpass, &emg_triceps, &emg_triceps, 1 );
+    arm_biquad_cascade_df1_f32(&trinotch, &emg_triceps, &emg_triceps, 1 );
+    x0 = fabs(emg_triceps);
+    tri_result = (x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1);
+    x9=x8;
+    x8=x7;
+    x7=x6;
+    x6=x5;
+    x5=x4;
+    x4=x3;
+    x3=x2;
+    x2=x1;
+    x1=x0;
+}
+
+void kalbi() //blinking three lights, first row - 2nd row unlit
+{
+    if(dir1==0) {
+        dir1 = dir2 = dir3 = 1;
+    } else    {
+        dir1 = dir2 = dir3 = 0;
+    }
+}
+
+void kaltri() //blinking three lights, 2nd row - first row lit
+{
+    if(for1==0) {
+        for1 = for2 = for3 = 1;
+    } else    {
+        for1 = for2 = for3 = 0;
+    }
+}
+
+void okay()  //blinking the two lights you have chosen (misschien is hier een betere manier van coderen voor :P)
+{
+    if(direction == 1 && force == 1) {            // links zwak
+        if(for1 == 0 && dir1 == 0) {
+            for1 = dir1 = 1;
+        } else    {
+            for1 = dir1 = 0;
+        }
+    }
+    if(direction == 1 && force == 2) {            // links normaal
+        if(for2 == 0 && dir1 == 0) {
+            for2 = dir1 = 1;
+        } else    {
+            for2 = dir1 = 0;
+        }
+    }
+    if(direction == 1 && force == 3) {            // links sterk
+        if(for3 == 0 && dir1 == 0) {
+            for3 = dir1 = 1;
+        } else    {
+            for3 = dir1 = 0;
+        }
+    }
+    if(direction == 2 && force == 1) {            // mid zwak
+        if(for1 == 0 && dir2 == 0) {
+            for1 = dir2 = 1;
+        } else    {
+            for1 = dir2 = 0;
+        }
+    }
+    if(direction == 2 && force == 2) {            // mid normaal
+        if(for2 == 0 && dir2 == 0) {
+            for2 = dir2 = 1;
+        } else    {
+            for2 = dir2 = 0;
+        }
+    }
+    if(direction == 2 && force == 3) {            // mid sterk
+        if(for3 == 0 && dir2 == 0) {
+            for3 = dir2 = 1;
+        } else    {
+            for3 = dir2 = 0;
+        }
+    }
+    if(direction == 3 && force == 1) {            // rechts zwak
+        if(for1 == 0 && dir3 == 0) {
+            for1 = dir3 = 1;
+        } else    {
+            for1 = dir3 = 0;
+        }
+    }
+    if(direction == 3 && force == 2) {            // rechts normaal
+        if(for2 == 0 && dir3 == 0) {
+            for2 = dir3 = 1;
+        } else    {
+            for2 = dir3 = 0;
+        }
+    }
+    if(direction == 3 && force == 3) {            // rechts sterk
+        if(for3 == 0 && dir3 == 0) {
+            for3 = dir3 = 1;
+        } else    {
+            for3 = dir3 = 0;
+        }
+    }
+}
+
+int main()
+{
+    pc.baud(115200);                        //baudrate instellen
+    log_timer.attach(looper, TSAMP_EMG);    //EMG, Fsample 500 Hz
+    looptimer.attach(setlooptimerflag,TSAMP);
+    pwm_motor1.period_us(100);              //10kHz PWM frequency
+    pwm_motor2.period_us(100);              //10kHz PWM frequency
+
+    //set up filters
+    arm_biquad_cascade_df1_init_f32(&binotch, 1, binotch_const, binotch_states);
+    arm_biquad_cascade_df1_init_f32(&bihighpass, 1, bihighpass_const, bihighpass_states);
+
+    arm_biquad_cascade_df1_init_f32(&trinotch, 1, trinotch_const, trinotch_states);
+    arm_biquad_cascade_df1_init_f32(&trihighpass, 1, trihighpass_const, trihighpass_states);
+
+    //kalibratie
+
+    //motorarm naar nul-positie
+    blink.attach(kalbi, 0.2);
+    blink.attach(kaltri, 0.2);
+
+    //calibration motor 2
+    pwm_motor2.write(0.6); //lage PWM
+    motor2dir = 1;
+    wait(1);                // anders wordt de while(1) meteen onderbroken
+    while(1) {
+        if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
+            pwm_motor2.write(0);
+            motor2.setPosition(0);
+            goto motor1cal;
+        }
+        wait(0.01);
+    }
+motor1cal:
+    //calibration motor 1
+    pwm_motor1.write(0.55); //lage PWM
+    motor1dir = 1;
+    wait(1);                // anders wordt de while(1) meteen onderbroken
+    while(1) {
+        if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
+            pwm_motor1.write(0);
+            motor1.setPosition(0);
+            goto emgcal;
+        }
+        wait(0.01);
+    }
+emgcal:
+    blink.detach();
+    dir1 = dir2 = dir3 = 1;
+    for1 = for2 = for3 = 1;
+    pc.printf("kalmoarm ");
+    wait (1);
+    for1 = for2 = for3 = 0;
+
+    //biceps kalibratie
+    blink.attach(kalbi, 0.2);
+    for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
+        if (bi_max < bi_result) {
+            bi_max = bi_result;
+        }
+        wait (0.01);
+
+        blink.detach();
+        dir1 = dir2 = dir3 = 1;
+        pc.printf("kalbi ");
+        wait (1);
+
+        //triceps kalibratie
+        blink.attach(kaltri, 0.2);
+        for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
+            if (tri_max < tri_result) {
+                tri_max = tri_result;
+            }
+            wait (0.01);
+        }
+        blink.detach();
+        for1 = for2 = for3 = 1;
+        pc.printf("kaltri ");
+        wait (1);
+        for1 = for2 = for3 = 0;
+    }
+
+directionchoice:
+log_timer.attach(looper, TSAMP_EMG);
+
+    while(1) { //Loop keuze DIRECTION
+        for(int i=1; i<4; i++) {
+            if(i==1) {           //red
+                dir1=1;
+                dir2=0;
+                dir3=0;
+                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+                    if(bi_result>0.8*bi_max) {
+                        direction = 1;
+                        pc.printf("links ");
+                        wait(TIMEB4NEXTCHOICE);                 // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
+                        goto forcechoice;                       // goes to second while(1) for the deciding the force
+                    } else {
+                        wait(0.01);
+                    }
+                }
+            }
+            if(i==2) {           //green
+                dir1 =0;
+                dir2 =1;
+                dir3 =0;
+                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+                    if(bi_result>0.8*bi_max) {
+                        direction = 2;
+                        pc.printf("mid ");
+                        wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
+                        goto forcechoice;
+                    } else {
+                        wait(0.01);
+                    }
+                }
+            }
+            if(i==3) {           //blue
+                dir1 =0;
+                dir2 =0;
+                dir3 =1;
+                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+                    if(bi_result>0.8*bi_max) {
+                        direction = 3;
+                        pc.printf("rechts ");
+                        wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
+                        goto forcechoice;
+                    } else {
+                        wait(0.01);
+                    }
+                }
+            }
+        }
+    }
+forcechoice:
+    while(1) { //Loop keuze FORCE
+        for(int j=1; j<4; j++) {
+            if(j==1) {           //red
+                for1=1;
+                for2=0;
+                for3=0;
+                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+                    if(tri_result>0.8*tri_max) {
+                        for1 = for2 = for3 = 0;
+                        pc.printf("reset ");
+                        goto directionchoice;
+                    } else {
+                        if(bi_result>0.8*bi_max) {
+                            force = 1;
+                            pc.printf("zwak ");
+                            wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
+                            goto choicesmade;
+                        } else {
+                            wait(0.01);
+                        }
+                    }
+                }
+            }
+            if(j==2) {           //green
+                for1=0;
+                for2=1;
+                for3=0;
+                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+                    if(tri_result>0.8*tri_max) {
+                        for1 = for2 = for3 = 0;
+                        pc.printf("reset ");
+                        goto directionchoice;
+                    } else {
+                        if(bi_result>0.8*bi_max) {
+                            force = 2;
+                            pc.printf("normaal ");
+                            wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
+                            goto choicesmade;
+                        } else {
+                            wait(0.01);
+                        }
+                    }
+                }
+            }
+            if(j==3) {           //blue
+                for1=0;
+                for2=0;
+                for3=1;
+                for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
+                    if(tri_result>0.8*tri_max) {
+                        for1 = for2 = for3 = 0;
+                        pc.printf("reset ");
+                        goto directionchoice;
+                    } else {
+                        if(bi_result>0.8*bi_max) {
+                            force = 3;
+                            pc.printf("sterk ");
+                            wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
+                            goto choicesmade;
+                        } else {
+                            wait(0.01);
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+choicesmade:
+    blink.attach(okay, 0.2);
+    while(1) {
+        if(tri_result>0.8*tri_max) {
+            blink.detach();
+            pc.printf("reset ");
+            wait(1);                                // 1 sec wait, anders reset je meteen ook de biceps keuze
+            goto forcechoice;
+        } else {
+            if(bi_result>0.8*bi_max && (dir1==1||dir2==1||dir3==1)) {
+                blink.detach();
+                log_timer.detach();
+                goto motorcontrol;
+            } else {
+                wait(0.01);                          // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
+            }
+        }
+    }
+
+motorcontrol:
+
+    /* Vanaf hier komt de aansturing van de motor */
+
+    if(direction == 1 && force == 1) {            // links zwak
+        pc.printf("links zwak ");
+        // hier komt setpoint motor 1, setpoint motor2
+    }
+    if(direction == 1 && force == 2) {            // links normaal
+        pc.printf("links normaal ");
+    }
+    if(direction == 1 && force == 3) {            // links sterk
+        pc.printf("links sterk ");
+    }
+    if(direction == 2 && force == 1) {            // mid zwak
+        pc.printf("mid zwak ");
+    }
+    if(direction == 2 && force == 2) {            // mid normaal
+        pc.printf("mid normaal ");
+    }
+    if(direction == 2 && force == 3) {            // mid sterk
+        pc.printf("mid sterk ");
+    }
+    if(direction == 3 && force == 1) {            // rechts zwak
+        pc.printf("rechts zwak ");
+    }
+    if(direction == 3 && force == 2) {            // rechts normaal
+        pc.printf("rechts normaal ");
+    }
+    if(direction == 3 && force == 3) {            // rechts sterk
+        pc.printf("rechts sterk ");
+    }
+
+    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
+
+        //regelaar motor2, bepaalt positie
+        controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
+        integral = integral + controlerror*TSAMP;
+        pwm = Kp2*controlerror + Ki2*integral;
+
+        keep_in_range(&pwm, -1,1);
+        pwm_motor2.write(abs(pwm));
+        if(pwm > 0) {
+            motor2dir = 1;
+        } else {
+            motor2dir = 0;
+        }
+
+        //controleert of batje positie heeft bepaald
+        if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+            if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
+                batjeset = 0;
+            } else {
+                batjeset++;
+            }
+        } else {
+            pwm_motor2.write(0);
+            batjeset = integral = 0;
+            wait(1);
+            goto motor1control;
+        }
+    }
+
+motor1control:
+    while(1) {      // loop voor het slaan mbv motor1 (batje snelheid)
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
+
+        if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
+            controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
+            integral = integral + controlerror*TSAMP;
+            pwm = Kp1*controlerror + Ki1*integral;
+        } else {            //regelaar motor1, bepaalt positie
+            pwm_motor1.write(0);
+            balhit = integral = 0;
+            wait(1); // wait voordat arm weer naar beginpositie terugkeert
+            goto resetpositionmotor1;
+        }
+
+        keep_in_range(&pwm, -1,1);
+        pwm_motor1.write(abs(pwm));
+
+        if(pwm > 0) {
+            motor1dir = 1;
+        } else {
+            motor1dir = 0;
+        }
+
+        //controleert of batje balletje heeft bereikt
+        //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
+        if (motor1.getPosition()*omrekenfactor1 > 1.10) {
+            balhit = 1;
+        }
+    }
+
+resetpositionmotor1:
+    while(1) {      // slagarm wordt weer in oorspronkelijke positie geplaatst
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
+
+        //regelaar motor1, bepaalt positie
+        controlerror = -1*motor1.getPosition()*omrekenfactor1;
+        integral = integral + controlerror*TSAMP;
+        pwm = Kp3*controlerror + Ki3*integral;
+
+        keep_in_range(&pwm, -1,1);
+        if(pwm > 0) { 
+            motor1dir = 1;
+        } else {
+            motor1dir = 0; //1 = rechtsom, 0 = linksom
+        }
+
+        pwm_motor1.write(abs(pwm));
+
+        //controleert of arm terug in positie is
+        if(batjeset < 200) {
+            if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
+                batjeset = 0;
+            } else {
+                batjeset++;
+            }
+        } else {
+            pwm_motor1.write(0);
+            batjeset = integral = 0;
+            wait(1);
+            goto resetpositionmotor2;
+        }
+    }
+
+resetpositionmotor2:
+    while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
+
+        //regelaar motor2, bepaalt positie
+        controlerror = -1*motor2.getPosition()*omrekenfactor2;
+        integral = integral + controlerror*TSAMP;
+        pwm = Kp2*controlerror + Ki2*integral;
+
+        keep_in_range(&pwm, -1,1);
+
+        if(pwm > 0) {
+            motor2dir = 1;
+        } else {
+            motor2dir = 0;
+        }
+
+        pwm_motor2.write(abs(pwm));
+
+        //controleert of batje positie heeft bepaald
+        if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+            if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
+                batjeset = 0;
+            } else {
+                batjeset++;
+            }
+        } else {
+            pwm_motor2.write(0);
+            batjeset = integral = 0;
+            wait(1);
+            direction = force = 0;
+            goto directionchoice;
+        }
+    }
+} // end main
\ No newline at end of file