Revision 1:8a0a39740897, committed 2013-11-04

Comitter:

Esther

Date:

Mon Nov 04 19:30:07 2013 +0000

Parent:

0:e9e50c0a6502

Child:

2:103de31c8803

Commit message:

begintotaalscriptbegin

Changed in this revision

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

--- a/begintotaalscript.cpp	Thu Oct 31 14:10:59 2013 +0000
+++ b/begintotaalscript.cpp	Mon Nov 04 19:30:07 2013 +0000
@@ -1,57 +1,438 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "encoder.h"
+
+volatile bool looptimerflag;
+
+void setlooptimerflag(void)
+{
+    looptimerflag=true;
+}
 
 
 int main()
 {
-
+    //Communicatie met de pc
     MODSERIAL pc(USBTX,USBRX,64,1024);
     pc.baud(115200);
 
-
+    //Benoemen inputs
     DigitalIn knop1(PTC2);
     DigitalIn knop2(PTB3);
     DigitalIn knop3(PTB2);
 
+    AnalogIn    emg1(PTB0); //Analog input emg1
+    AnalogIn    emg2(PTB1); //Analog input emg2
+
+    Encoder motor1(PTD0,PTC9);
+    Encoder motor2(PTD2,PTC8);
+
+    /* PWM control to motor */
+    PwmOut pwm_motor1(PTA12);
+    PwmOut pwm_motor2(PTA5);
+    /* Direction pin */
+    DigitalOut motor1dir(PTD3);
+    DigitalOut motor2dir(PTD1);
+
     knop1.mode(PullUp);
     knop2.mode(PullUp);
     knop3.mode(PullUp);
 
+    //Variabelen voor menu
     int state = 1;
 
-    while (true) {
+    bool calibratie_rust = false;
+    bool calibratie_max  = false;
+    bool calibratie_motor = false;
+    bool meting = false;
+
+    //Variabelen EMG/motoren
+    double yy,z,zabs,w,y1,z1,zabs1,w1,y2,z2,zabs2,w2,e1,e2,e3,f1,f2,g1,g2,g3,h1,h2,byy,bz,bzabs,bw,by1,bz1,bzabs1,bw1,by2,bz2,bzabs2,bw2,be1,be2,be3,bf1,bf2,bg1,bg2,bg3,bh1,bh2;
+    double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay;
+    double theta, r, dtheta, dr, theta_pen, r_pen, theta_pwm, r_pwm, pi;
+    
+    pi=3.14159265359;
+
+    e1= 0.855930601814815;
+    e2=  -1.711861203629630;
+    e3= 0.855930601814815;
+    f1=-1.690996376887443;
+    f2= 0.732726030371816;
+
+    g1= 0.098259168204717*pow(10.0,-4.0);
+    g2= 0.196518336409435*pow(10.0,-4.0);
+    g3=0.098259168204717*pow(10.0,-4.0);
+    h1= -1.991114292201654;
+    h2=0.991153595868935;
+
+    be1=0.914969144113083;
+    be2= -1.829938288226165;
+    be3= 0.914969144113083;
+    bf1=-1.822694925196308;
+    bf2=0.837181651256022;
+
+    bg1= 0.246193004641015*pow(10.0,-5.0);
+    bg2= 0.492386009282031 *pow(10.0,-5.0);
+    bg3= 0.246193004641015*pow(10.0,-5.0);
+    bh1= -1.995557124345789;
+    bh2= 0.995566972065975;
+
+
+    y1 = 0; /*Initialize y(n-1) = 0*/
+    y2 = 0; /*Initialize y(n-2) = 0*/
+    z1 = 0; /*Initialize z(n-1) = 0*/
+    z2 = 0; /*Initialize z(n-2) = 0*/
+    zabs1 = 0; /*Initialize zabs(n-1) = 0*/
+    zabs2 = 0; /*Initialize zabs(n-2) = 0*/
+    w1 = 0; /*Initialize w(n-1) = 0*/
+    w2 = 0; /*Initialize w(n-2) = 0*/
 
-        if (state == 1)
-            pc.printf("state 1 rust| 1=cal motor 2=cal emg 3=tekenen /n/r");
+    by1 = 0; /*Initialize by(n-1) = 0*/
+    by2 = 0; /*Initialize by(n-2) = 0*/
+    bz1 = 0; /*Initialize bz(n-1) = 0*/
+    bz2 = 0; /*Initialize bz(n-2) = 0*/
+    bzabs1 = 0; /*Initialize bzabs(n-1) = 0*/
+    bzabs2 = 0; /*Initialize bzabs(n-2) = 0*/
+    bw1 = 0; /*Initialize bw(n-1) = 0*/
+    bw2 = 0; /*Initialize bw(n-2) = 0*/
+    const float Ts = 0.001; /*Sample Time, const(ant)*/
+
+//Constanten voor tekening: EMGdeel
+    xbegin= 0; //beginpositie
+    ybegin=0; //beginpositie
+    qbegin=315;  //beginhoek (in graden) vanaf de y-as, rechtsom gedraaid is positief
+    MaxsnelheidV=15;     //in cm/s
+    MaxsnelheidD=15;    //in graden/s
+
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,Ts);
+
+    while (true) {              //oneindige while loop
+
+        while(looptimerflag != true);
+        looptimerflag = false;
+
+
+// state 1 RUST
+        if (state == 1) {
+
+            pc.printf("state 1 RUST  |     knop1 = cal_motor     knop2 = MENU EMG     knop3 = Tekenen \n\r");
+
 
-        if (knop1 == false ) {
-            state=2;
-            wait(0.05);
+            if (knop1 == false ) {  //als knop 1 is ingedrukt
+                state=2;
+                wait(0.05);        //wacht goed contact knopje
+
+                while(knop1 == false) {}    //wacht tot knop is losgelaten
+                wait(0.05);                 // wacht goed contact knopje
+
+            }
+
+            if (knop2 == false ) {  //als knop 2 wordt ingedrukt: state is 3
+                state=3;
+                wait(0.05);
+
+                while(knop2 == false) {}
+                wait(0.05);
+            }
+            if (knop3 == false && calibratie_rust == true && calibratie_max  == true && calibratie_motor == true) {
+                state=6;    //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn. 
+                wait(0.05);
+
+                while(knop3 == false) {}
+                wait(0.05);
+                meting=true;
+
+            }
+            if (knop3 == false && state !=6) {
+                wait(0.05);
+                pc.printf("Niet alle calibraties uitgevoerd! \n");
+                wait(1);
+            }
+
+
         }
 
-        if (knop2 == false ) {
-            state=3;
-            wait(0.05);
+
+//state2 CAL MOTOR
+        if (state == 2) {
+            pc.printf("state 2 cal_motor    |    knop1 = terug naar rust \n\r");
+
+
+            if (knop1 == false ) {
+                state=1;
+                wait(0.05);
+
+                while(knop1 == false) {}
+                wait(0.05);
+
+                motor1.setPosition(0);
+                motor2.setPosition(0);
+
+
+                calibratie_motor = true;
+
+
+            }
         }
-        if (knop3 == false ) {
-            state=6;
-            wait(0.05);
+// state 3 MENU EMG
+        if (state == 3) {
+            pc.printf("state 3 MENU EMG | knop1= cal EMG rust  knop2= rust   knop3= cal EMG max  \n\r");
+
+
+            if (knop2 == false ) {  // terug naar state 1
+                state=1;
+                wait(0.05);
+
+                while(knop2 == false) {}
+                wait(0.05);
+            }
+
+            if (knop1 == false ) {  // naar state 4
+                state=4;
+                wait(0.05);
+
+                while(knop1 == false) {}
+                wait(0.05);
+
+                // begingrenswaarden stellen:
+                GrenswaardeV=0;
+                GrenswaardeD=0;
+                meting=true;
+            }
+
+            if (knop3 == false ) {  // naar state 5
+                state=5;
+                wait(0.05);
+
+                while(knop3 == false) {}
+                wait(0.05);
+
+                // begingrenswaarden stellen:
+                MaxwaardeV=0;
+                MaxwaardeD=0;
+                meting=true;
+
+            }
+
+
         }
-        
+
+
+        //METEN VAN EMG
+        if (meting == true) {
+            //  pc.printf("TEST CODE: Meting in gang\n");
+            yy = emg1.read(); //<measurement function, read from pin>
+            byy= emg2.read();
+
+            z=-f1*z1-f2*z2+e1*yy+e2*y1+e3*y2; /*hoogdoorlaatfilteroverdacht*/
+            bz=-bf1*bz1-bf2*bz2+be1*byy+be2*by1+be3*by2; /*hoogdoorlaatfilteroverdacht van biceps*/
+
+            y2 = y1; /*y(n-1) will be y(n-2) on next loop*/
+            y1= yy; /*y(n) will be y(n-1) on next loop*/
+            z2 = z1; /*z(n-1) will be z(n-2) on next loop*/
+            z1 = z; /*z(n) will be z(n-1) on next loop*/
+
+            by2 = by1; /*y(n-1) will be y(n-2) on next loop*/
+            by1= byy; /*y(n) will be y(n-1) on next loop*/
+            bz2 = bz1; /*z(n-1) will be z(n-2) on next loop*/
+            bz1 = bz; /*z(n) will be z(n-1) on next loop*/
+
+            zabs=abs(z);/*rectify*/
+            bzabs=abs(bz);/*rectify*/
+
+            w=-h1*w1-h2*w2+g1*zabs+g2*zabs1+g3*zabs2; /*laagdoorlaatfilteroverdracht*/
+            bw=-bh1*bw1-bh2*bw2+bg1*bzabs+bg2*bzabs1+bg3*bzabs2; /*laagdoorlaatfilteroverdracht van biceps*/
+
+            zabs2 = zabs1; /*z(n-1) will be z(n-2) on next loop*/
+            zabs1 = zabs; /*z(n) will be z(n-1) on next loop*/
+            w2 = w1; /*w(n-1) will be w(n-2) on next loop*/
+            w1 = w; /*w(n) will be w(n-1) on next loop*/
+
+            bzabs2 = bzabs1; /*z(n-1) will be z(n-2) on next loop*/
+            bzabs1 = bzabs; /*z(n) will be z(n-1) on next loop*/
+            bw2 = bw1; /*w(n-1) will be w(n-2) on next loop*/
+            bw1 = bw; /*w(n) will be w(n-1) on next loop*/
+
+            //pc.printf("%f,%f\n",w,bw);
+
+            wait(Ts); /*When finished wait (maybe a bit shorter?)*/
+
+            if (w>1.0) {
+                w=1.0;
+            }
+            if (bw>1.0) {
+                bw=1.0;
+            }
+
+        }
 
 
 
-        if (state == 2)
-            pc.printf("state 2 cal robot| 2= rust /n/r");
-        if (state == 3)
-            pc.printf("state 3 cal EMG | 1=cal emg laag 3=cal emg hoog /n/r");
-        //if (state == 4)
-          //  pc.printf("state 1 | 1=cal motor 2=cal emg 3=tekenen /n/r");
-        //if (state == 5)
-          //  pc.printf("state 1 | 1=cal motor 2=cal emg 3=tekenen /n/r");
-        if (state == 6)
-            pc.printf("state 6 tekenen| 6=rust /n/r");
+// state 4 EMG CAL IN RUST
+        if (state == 4) {
+            pc.printf("state 4 cal EMG RUST    |    knop1= MEMNU EMG \n\r");
+            pc.printf("%f,%f\n",GrenswaardeV,GrenswaardeD);
+            // Nieuwe grenswaarden bepalen
+            if (w > GrenswaardeV) {
+                GrenswaardeV = w;
+            }
+            if (bw > GrenswaardeD) {
+                GrenswaardeD = bw;
+            }
+            if (knop1 == false ) {  // terug naar state 1
+                state=3;
+                wait(0.05);
+
+                while(knop1 == false) {}
+                wait(0.05);
+                calibratie_rust = true;
+                meting=false;
+
+            }
+        }
+// state 5 EMG CAL MAX
+        if (state == 5) {
+            pc.printf("state 5 cal EMG MAX |    knop3= EMG MENU \n\r");
+            pc.printf("%f,%f\n",MaxwaardeV,MaxwaardeD);
+            // Nieuwe maxwaarden bepalen
+            if (w > MaxwaardeV) {
+                MaxwaardeV = w;
+            }
+            if (bw > MaxwaardeD) {
+                MaxwaardeD = bw;
+            }
+
+            // einde maxwaarde bepalen:
+            if (knop3 == false ) {  // terug naar state 3
+                state=3;
+                wait(0.05);
+
+                while(knop3 == false) {}
+                wait(0.05);
+                calibratie_max  = true;
+                meting=false;
+
+            }
+
+        }
+
+// state 6  TEKENEN
+
+        if (state == 6) {
+            // pc.printf("state 6 tekenen  |    knop3= rust \n\r");
+
+            //  EMG
+            BereikD = MaxwaardeD-GrenswaardeD;    //Bereik van grafiek EMG (Voltage t.o.v. tijd)
+            BereikV = MaxwaardeV-GrenswaardeV;      //Bereik van grafiek EMG (Voltage t.o.v. tijd)
+
+            SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV);  // Snelheid van pen (in cm/sec), lineair verband met het Bereik
+            SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik
+
+            if (SnelheidV<MaxsnelheidD) {
+                SnelheidV=MaxsnelheidD;
+            }
+
+            if (SnelheidD<MaxsnelheidD) {
+                SnelheidD=MaxsnelheidD;
+            }
+
+            if (SnelheidV>MaxsnelheidV) {
+                SnelheidV=MaxsnelheidV;
+            }
+
+            if (SnelheidD>MaxsnelheidV) {
+                SnelheidD=MaxsnelheidV;
+            }
+
+            deltas = SnelheidV*Ts;  //afstand tussen twee meetpunten
+            deltaq = SnelheidD*Ts;  //verandering hoek tov. de rechtdoor
+            q = deltaq+qbegin;           // hoekverandering tov. het assenstelsel
+
+            deltay = deltas*cos(q);  //veranding van pen in de y-richting
+            deltax = deltas*sin(q);  // verandering van pen in de x-richting
+
+            x = xbegin+deltax;       // nieuwe postie pen, x-coördinaat
+            y = ybegin+deltay;       // nieuwe positie pen, y-coöridnaat
+
+            if (x>29.7) {       //grenzen A4
+                x=29.7;
+            }
+            if (y>21.0) {
+                y=21.0;
+            }
+
+            if (x<0) {       //grenzen A4
+                x=0;
+            }
+            if (y<0) {
+                y=0;
+            }
+
+            xbegin=x;    // x zal xbegin worden in de volgende loop oftewel:(x(n) will be x(n-1) on next loop)
+            ybegin=y;    // y zal ybgin worden in de volgende loop  oftewel:(y(n) will be y(n-1) on next loop)
+            qbegin=q;   // q zal qbegin worden in de volgende loop oftewel:(q(n) will be q(n-1) on next loop)
+
+            pc.printf("%f,%f,%f,%f\n",SnelheidV,SnelheidD,x,y);
+
+
+// motoren
+            x = x*10.0 + 69.8;
+            y = y*10.0 + 69.8;
+            theta   = atan(y/x)       ;// *   (400.0/(.5*pi));
+            r       = (sqrt(x*x+y*y)) ;// *   (2577/461.335);
+
+            theta_pen   = motor1.getPosition()  *   ((.5*pi)/400);
+            r_pen       = motor2.getPosition()  *   (363.0/2577.0);
+
+
+            dtheta  = (theta - theta_pen);
+            dr      = (r - r_pen);
+            theta_pwm   = (dtheta)*0.5;
+            r_pwm       = (dr)*0.001;
+            //NAAR MOTOR
+
+            //Zorgen dat pwm tussen -1 en 1 blijft.
+            if(theta_pwm > 1) {
+                theta_pwm=1;
+            }
+            if(theta_pwm < -1) {
+                theta_pwm=-1;
+            }
+            if(r_pwm > 1) {
+                r_pwm=1;
+            }
+            if(r_pwm < -1) {
+                r_pwm=-1;
+            }
+            // Bepaal richting waarin motoren moeten draaien
+            if(theta_pwm > 0)
+                motor1dir.write(1);
+            else
+                motor1dir.write(0);
+            if(r_pwm > 0)
+                motor2dir.write(1);
+            else
+                motor2dir.write(0);
+
+            // print naar pc
+            pc.printf("t=%.3f   dt=%.3f  tpwm=%.3f  |  r=%.3f   dr=%.3f  rpwm=%.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm);
+            //schrijf PWM naar motor
+            pwm_motor1.write(abs(theta_pwm));
+            pwm_motor2.write(abs(r_pwm));
+
+
+            // terug naar state 1 knop
+            if (knop3 == false ) {  // terug naar state 1
+                state=1;
+                wait(0.05);
+
+                while(knop3 == false) {}
+                wait(0.05);
+            }
+
+        }
 
     } //sluiten oneindige while loop
 
-}//afsluiten main
\ No newline at end of file
+}   //afsluiten main
\ No newline at end of file