Diff: main.cpp

Revision:

0:1e91c3ae4e28

Child:

1:42fad02fb157

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 13 15:56:40 2014 +0000
@@ -0,0 +1,624 @@
+//ROBOTCONTROL BMT M9 - GROEP 2
+//Alex Overbeek
+//Tom Baumeister
+//Bas van Buuren
+//Bas Mattern
+//Thijs ruikes
+
+/*Het script bestaat uit drie delen: INCLUDE AND DEFINE, FUNCTIONS, MAINSCRIPT. Per gedeelte wordt uitgelegd wat gedaan wordt.
+We laten de beweging in fases lopen. Fase 1 werkt in dit script. Voor fase 2 kan de positie al worden ingesteld, de slagbeweging
+moet nog gemaakt worden. Dit gaan we doen met een PID regelaar die naar een snelheid toe regelt.
+*/
+
+//INCLUDE AND DEFINE ALL
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include <iostream>
+#include "encoder.h"
+#include "HIDScope.h"
+
+#define THRESHOLD 0.8
+#define NOSAMPL 500
+
+#define TSAMP1 0.01
+#define K_P1 (0.0015)
+#define K_I1 (0.0000001*TSAMP1)
+#define K_D1 (0.0003/TSAMP1)
+#define I_LIMIT1 1.
+
+//KPID voor slagfunctie
+#define KSLA_P (0.06)
+#define KSLA_I (0.00002 *TSAMP1)
+#define KSLA_D (0.0005 /TSAMP1)
+
+#define MAXPOS 750
+
+// Constantes voor de filters definiëren:
+// Constantes voor de Low Pass filter
+#define A1LP    0.018180963222803
+#define A0LP     0.016544013176248
+#define B1LP    -1.718913340044714
+#define B0LP     0.753638316443765
+// Constantes voor de High Pass Filter
+#define A1HP    -1.999801878951505
+#define A0HP   0.999801878951505
+#define B1HP   -1.971717601075000
+#define B0HP  0.972111984032897
+// Constantes voor het Notch Filter
+#define A0N 0.99436777112
+#define A1N -1.89139989664
+#define A2N 0.99436777112
+#define B1N 1.89070035439
+#define B2N -0.988036
+
+// Sample Time
+#define TSAMP 0.001
+
+// Outputs van de motor
+Encoder encoderA(PTD5,PTA13);
+Encoder encoder1(PTD0,PTD2);
+
+// Outputs naar de motor
+PwmOut pwm(PTC8);
+DigitalOut dir(PTC9);
+PwmOut pwm1(PTA5);
+DigitalOut dir1(PTA4);
+
+//lampjes
+DigitalOut rood(LED1);
+DigitalOut blauw(LED3);
+DigitalOut groen(LED2);
+
+//emg input
+AnalogIn    emg1(PTB1);
+AnalogIn    emg2(PTB2);
+
+// PC communicatie
+MODSERIAL pc(USBTX,USBRX);
+
+// Ticker voor de meetgegevens
+Ticker timer;
+volatile bool looptimerflag;
+
+// Waardes voor de filters reserveren en als float vaststellen
+float emg_value2, ylp2, yhp2, yn2,ysum1 = 0, yave1=0;
+float emg_value1, ylp1, yhp1, yn1,ysum2 = 0, yave2=0 ;
+
+//variabvelen voor positie motor1
+float v1=0,out_i1 = 0; //out_i1 globaal gedef om reset
+
+// 0 of 1 waardes gedefinieerd uit het EMG, met 0 is te lage activiteit, 1 is hoog genoeg, voor de zekerheid toch als int.
+uint8_t y1, y2;
+int check = 0;
+
+// integers reserveren voor het deel van het regelsysteem als we de boolean emg waardes hebben: hoek van het badje (0,1, of 2) en de gewenste snelheid (0,1, of 2)
+int badjestand, badjes=1,badjedone=0,speeding,armstand=0,armspeed=0;
+bool speeddone=0;
+
+// Teller voor hoeveel metingen er zijn gedaan
+uint16_t teller=0;
+
+//Definieer alle functies
+void clamp(float * in, float min, float max);
+void clampint(int * in, int min, int max);
+void setlooptimerflag(void);
+float readEMG1();
+float readEMG2();
+float notchfilter1(float ylp1);
+float notchfilter2(float ylp2);
+float hpfilter1(float yn1);
+float hpfilter2(float yn2);
+float lpfilter1(float yhp1);
+float lpfilter2(float yhp2);
+float filter1(float emg_value1);
+float filter2(float emg_value2);
+float threshold1 (float yave1);
+float threshold2 (float yave2);
+float getv(float delta_t);
+float resetarm();
+uint8_t badje (uint8_t y1, uint8_t y2);
+void batposition(int y);
+void sla(int k);
+float pidposition(float setpoint, float measurement);
+float pidarm(float rev_value, float mea_value);
+uint8_t armposition (uint8_t y1, uint8_t y2);
+
+/*FUNCTIES
+In deze sectie worden de functie geprogrammeerd, hieronder een uitleg per functie. De functies zijn verdeeld in drie groepen. In het script
+staat achter enkele functies een 1 of een 2. Er worden 2 signalen bewerkt, dus sommige functies staan dubbel in het script. Signaal 1 wordt
+door de functies met 1, en signaal 2 wordt door de functie met 2, bewerkt.
+
+GLOBAL FUNCTIONS
+Clamp - "clampt" een waarde binnen grenzen, als de geclampte variabele over de grens heen gaat krijgt de variabele de grenswaarde.
+setLooptimerflag - Hiermee wordt de ticker aangeroepen.
+
+EMG FUNCTIONS
+readEMG - Leest het voltage af van de geselecteerde pin, in ons geval een EMG waarde.
+(notch/lp/hp)filter -Er worden drie filters gebruikt: lowpass, highpass en notchfilter. De funtie krijgt een input en geeft dan een gefilterde output.
+filter - Voert de notch,lp,hp filters achter elkaar uit op een input en geeft het gemiddelde van 500 meetpunten als output.
+threshold - Vergelijkt de input met een treshold, als de waarde boven de treshold komt (zodra een spier wordt aangespannen) wordt de output 1, anders 0.
+
+MOTORCONTROLFUNCTIONS
+resetarm - Brengt arm terug naar de nul positie, zet de encoder op 0.
+getv(delta_t) - Geeft de huidige draaisnelheid van motor1, delta_t is het tijdsinterval waarover gemeten wordt
+badje - Vergelijkt met dubbele input, y1 en y2. Er zijn vier situaties, beide = 0, y1=1, y2=1, beide =1.
+badposition - Leest de output van badje, laat het badje links, recht, niet draaien.
+pid... - Pid regelaar, de k waardes wordenin de define sectie gedefineerd.
+armposition - Op basis van de emgsignalen wordt hier een positie ingesteld.
+armtopos - Regelt de arm naar de opgegeven positie toe.
+*/
+//HIDScope scope(5);
+//GLOBAL FUNCTIOMS
+/*void viewer(){
+    scope.set(0,yave1);
+    scope.set(1,y1);
+    scope.set(2,yave2);
+    scope.set(3,y2);
+  scope.send();
+    }*/
+
+void clamp(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+
+void clampint(int * in, int min, int max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+//EMG FUNCTIONS
+float readEMG1()
+{
+    emg_value1=emg1.read();
+    return emg_value1;
+}
+
+float readEMG2()
+{
+    emg_value2=emg2.read();
+    return emg_value2;
+}
+
+float notchfilter1(float ylp1)
+{
+    static float yn1,x1=0,x2=0,y1=0,y2=0,x;
+    x = ylp1;
+    yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yn1;
+    return yn1;
+}
+
+float notchfilter2(float ylp2)
+{
+    static float x1=0,x2=0,y1=0,y2=0,x;
+    x = ylp2;
+    yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yn2;
+    return yn2;
+}
+
+float hpfilter1(float yn1)
+{
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yn1;
+    yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yhp1;
+    return yhp1;
+}
+
+float hpfilter2(float yn2)
+{
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yn2;
+    yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yhp2;
+    return yhp2;
+}
+
+float lpfilter1(float yhp1)
+{
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yhp1;
+    ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = ylp1;
+    return ylp1;
+}
+
+float lpfilter2(float yhp2)
+{
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yhp2;
+    ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = ylp2;
+    return ylp2;
+}
+
+float filter1(float emg_value1)
+{
+    static uint16_t n;
+    yn1 = notchfilter1(emg_value1);
+    /*yhp1 = hpfilter1(yn1);
+    ylp1 = lpfilter1(yhp1);*/
+    ylp1 = fabs(yn1);
+    ysum1 = ysum1+ylp1;
+    n++;
+    if(n==500) {
+        yave1 = ysum1/500;
+        ysum1 = 0;
+        n = 0;
+    }
+    return yave1;
+}
+
+float filter2(float emg_value2)
+{
+    static uint16_t n;
+    yn2 = notchfilter2(emg_value2);
+    /*yhp2 = hpfilter2(yn2);
+    ylp2 = lpfilter2(yhp2);*/
+    ylp2 = fabs(yn2);
+    ysum2 = ysum2 + ylp2;
+    n++;
+
+    if(n==NOSAMPL) {
+        yave2 = ysum2/NOSAMPL;
+        ysum2 = 0;
+        n = 0;
+    }
+    return yave2;
+}
+
+float threshold1 (float yave1)
+{
+    if(yave1>THRESHOLD) {
+        y1 = 1;
+    } else {
+        y1 = 0;
+    }
+    return y1;
+}
+
+float threshold2 (float yave2)
+{
+    if(yave2>THRESHOLD) {
+        y2 = 1;
+    } else {
+        y2 = 0;
+    }
+    return y2;
+}
+
+//MOTORCONTROL FUNCTIONS
+float getv(float delta_t)
+{
+    int enca1=0,enca2=0,counts=0;
+    float v;
+    int n =0 ;
+    while(n<3) {
+        wait(delta_t);
+        enca2 = enca1;
+        enca1 = encoder1.getPosition();
+        n++;
+    }
+    counts = (enca1 - enca2)/delta_t;
+    v = (counts)*((2*3.14159265359)/1550)*0.5;
+    return v;
+}
+
+float resetarm()
+{
+    v1 = 1;
+    while(v1 !=0) {
+        dir1 = 0;
+        pwm1.write(0.1);
+        v1 =getv(0.1);
+    }
+    pwm1 = 0;
+    dir1 =1;
+    encoder1.setPosition(0);
+    return pwm1;
+}
+uint8_t badje (uint8_t y1, uint8_t y2)
+{
+    if (y1>0 && y2>0 && check>0) {
+        rood = 0;
+        groen = 0;
+        blauw = 1;
+        badjedone=1;
+        check=0;
+        cout<<"ga naar mode 2"<<endl;
+        wait(1);
+    } else if (y1>0 && y2>0) {
+        check=1;
+    } else if (y1>0) {
+        badjes=0;
+        check=0;
+    } else if (y2>0 ) {
+        badjes=1;
+        check=0;
+    } else {
+        check=0;
+        badjes = 2;
+    }
+    return badjes;
+}
+
+void batposition(int y)
+{
+    switch(y) {
+        case 0:
+
+            dir = 1;
+            pwm.write(0.4);
+            wait(0.04);
+            pwm.write(0);
+            cout<<"links"<<endl;
+            break;
+        case 1:
+            dir = 0;
+            pwm.write(0.4);
+            wait(0.04);
+            pwm.write(0);
+            cout<<"rechts"<<endl;
+            break;
+        case 2:
+            pwm.write(0);
+            break;
+
+    }
+
+}
+
+void sla(int k)
+{   
+    float maxpwm;
+    float new_pwm;
+    switch(k)
+    {
+        case 1:
+            maxpwm=0.6; //DE MAX PWM'en AANPASSEN VOOR DE ANDERE CASES!!!!
+            break;
+        case 2:
+            maxpwm=0.8;
+            break;
+        case 3:
+            maxpwm=1.0;
+            break;
+        /*case 4:
+            maxpwm=0.2;
+            break;*/
+        default:
+            maxpwm=0;
+            break;
+    }
+    while(abs(encoder1.getPosition()<MAXPOS))
+    {
+        new_pwm=pidarm(MAXPOS,encoder1.getPosition());
+        clamp(&new_pwm,-1,maxpwm);
+        if (new_pwm>0)
+        {
+            dir1=1;
+        }
+        else if (new_pwm<0)
+        {
+            dir1=0;
+        }
+        pwm1.write(fabs(new_pwm));
+        cout<<"pwm1: "<<new_pwm<<endl;
+        cout<<"dir: "<<dir<<endl;
+        cout<<"v: "<<getv(0.01)<<endl;
+        cout<<"pos: "<<encoder1.getPosition()<<endl;
+    }
+    dir1=0;
+    pwm1.write(.5);
+    wait(.1);
+    pwm1.write(0);
+    wait(1);
+    //cout<< encoder1.getPosition()<<endl;
+}
+
+float pidposition(float setpoint, float measurement)
+{
+    float error, out, out_p=0,out_d=0;
+    static float prev_error = 0;
+    error  = setpoint-measurement;
+    out_p  = error*K_P1;
+    out_i1 += error*K_I1;
+    out_d  = (error-prev_error)*K_D1;
+    clamp(&out_i1,-I_LIMIT1,I_LIMIT1);
+    prev_error = error;
+    out  = out_i1+out_p+out_d;
+    return out;
+}
+
+float pidarm(float rev_value, float mea_value)
+{
+    float error;
+    static float prev_error = 0;
+    float p_out = 0;
+    static float i_out = 0;
+    float d_out = 0;
+    error = rev_value - mea_value;
+    p_out = error * KSLA_P;
+    i_out += error * KSLA_I;
+    d_out = (error - prev_error) * KSLA_D;
+    clamp(&i_out,-I_LIMIT1,I_LIMIT1);
+    prev_error=error;
+    return p_out + i_out + d_out;
+}
+
+
+uint8_t armposition (uint8_t y1, uint8_t y2)
+{
+    static int stand=0;
+    if (y1>0 && y2>0 && check>0) {
+        rood=0;
+        groen=0;
+        blauw=1;
+        badjedone=2;
+        check=0;
+        /*rood = 0;
+        groen = 1;*/
+    } else if (y1>0 && y2>0) {
+        check=1;
+    } else if (y1>0) {
+        stand=stand+1;
+        check=0;
+        cout<<"stand "<<stand<<endl;
+    } else if (y2>0 ) {
+        stand=stand-1;
+        check=0;
+        cout<<"stand "<<stand<<endl;
+    } else {
+        
+        check =0;
+    }
+    clampint(&stand,1,3);
+    return stand;
+}
+
+float gotopos(int pos)
+{
+    float out1;
+    
+    switch(pos) {
+        case 1:
+            pos = 100;
+            break;
+
+        case 2:
+            pos = 200;
+            break;
+
+        case 3:
+            pos = 300;
+            break;
+        default:
+            break;
+    }
+    
+    while((abs(pos-encoder1.getPosition()) >15)|| (v1>0.1)) {
+
+        while(!looptimerflag);
+        looptimerflag = false;
+        cout << "Deltapos: " << abs(pos-encoder1.getPosition()) << endl;
+        out1 = pidposition(pos,encoder1.getPosition());
+        clamp(&out1,-1,1);
+        cout <<"out1: " << out1 <<endl;
+        if(out1>0) {
+            dir1 = 1;
+
+        } else if(out1<0) {  
+            dir1 = 0;
+        }
+        else{
+            }
+        pwm1 = fabs(out1);
+        v1 = getv(0.001);
+    }
+    pwm1 =0;
+
+    return pwm1;
+}
+
+// MAIN SCRIPT
+int main()
+{
+    /*rood = 0;
+    blauw = 1;
+    groen = 1;*/
+    resetarm();
+    pc.baud(115200);
+    timer.attach(setlooptimerflag,TSAMP);
+    wait(2);
+    cout<<"Begin programma"<<endl;
+    while(1) {
+        //Per TSAMP word EMG uitgelezen, gefilterd en gemiddeld
+        while(!looptimerflag);
+        looptimerflag = false;
+        emg_value1 =  readEMG1();
+        emg_value2 =  readEMG2();
+        yave1 = filter1(emg_value1);
+        yave2 = filter2(emg_value2);
+        y1=threshold1(yave1);
+        y2=threshold2(yave2);
+        teller++;
+
+        /*Dit gedeelte voert de bewegingen uit. In de eerste fase kan met de linker spier het batje naar links gedraaid worden,
+        en met de rechter spier het batje naar recht. Als het badje in de juiste positie staat kunnen beide spieren tegelijk
+        aangespannen worden om naar de volgende fase te gaan. In fase 2 wordt de armpositie ingesteld. Hoe groter de afstand van de
+        arm met de bal, hoe harder de arm gaat slaan. Zodra de arm in de goede positie staat kan door beide spieren aan te spannen worden
+        doorgegaan naar fase 3. In fase 3 slaat de arm de bal. De snelheid die ingesteld wordt waarmee de arm gaat slaan wordt bepaald aan de
+        hand van de positie waarin de arm in fase 2 is gezet. Zodra de arm de bal geslagen heeft moet de arm gereset worden en terug gaan naar
+        fase 1.*/
+
+        if (teller==NOSAMPL) {
+            teller=0;
+            switch(badjedone) {
+                case 0:
+                    rood=0;
+                    groen=1;
+                    blauw=1;
+                    cout<<"fase1"<<endl;
+                    badjestand=badje(y1,y2);
+                    batposition(badjestand);
+                    armstand=1;
+                    break;
+
+                case 1:
+                    rood=1;
+                    groen=0;
+                    blauw=1;
+                    cout<<"fase2"<<endl;
+                    armstand=armposition(y1,y2);
+                    gotopos(armstand);
+                    cout<<"armstand "<<armstand<<endl;
+                    
+                    //armtopos(armstand);
+                    //cout<<"badjedont: "<<badjedone<<endl;
+                    break;
+
+                case 2:
+                    rood=1;
+                    groen=1;
+                    blauw=0;
+                    resetarm();
+                    cout<<"sla"<<endl;
+                    sla(armstand);
+                    resetarm();
+                    badjedone=0;
+                    break;
+                default:
+                    break;
+            }
+
+        }
+    }
+   
+}
+
+