Revision 1:d44a866de64f, committed 2014-10-31

Comitter:

Hooglugt

Date:

Fri Oct 31 13:22:09 2014 +0000

Parent:

0:99cbc87af37c

Child:

2:10dd6a88dfd7

Commit message:

verbetering op GUI, factor 0.6, led timing, etc.

Changed in this revision

--- a/PROJECT_main.cpp	Fri Oct 31 10:31:02 2014 +0000
+++ b/PROJECT_main.cpp	Fri Oct 31 13:22:09 2014 +0000
@@ -6,16 +6,17 @@
 
 #define TSAMP 0.001 // sample freq encoder motor
 #define TIMEB4NEXTCHOICE 1  // sec keuzelampje blijft aan
-#define TIMEBETWEENBLINK 200 // sec voor volgende blink
+#define TIMEBETWEENBLINK 100 // 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 FACTOR 0.6 //factor*max_waarde = threshold emg
 //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 blink;           //ledjes aan/uit
+Ticker blink2;          //extra tikker zodat kalbi en kaltri tegelijkertijd aankunnen
 Ticker looptimer;       //motor regelaar
 
 MODSERIAL pc(USBTX,USBRX);
@@ -96,13 +97,10 @@
 float pwm = 0;
 
 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
-float omrekenfactor2 =  0.0015213178; // 6.28/(24*172);
+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 setpoint1 = 0; //te behalen speed van motor1 (37D)
+float setpoint2 = 0; //te behalen hoek van motor2 (25D)
 
 float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
 float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag
@@ -272,7 +270,7 @@
 
     //motorarm naar nul-positie
     blink.attach(kalbi, 0.2);
-    blink.attach(kaltri, 0.2);
+    blink2.attach(kaltri, 0.2);
 
     //calibration motor 2
     pwm_motor2.write(0.6); //lage PWM
@@ -301,6 +299,7 @@
     }
 emgcal:
     blink.detach();
+    blink2.detach();
     dir1 = dir2 = dir3 = 1;
     for1 = for2 = for3 = 1;
     pc.printf("kalmoarm ");
@@ -314,29 +313,28 @@
             bi_max = bi_result;
         }
         wait (0.01);
-
-        blink.detach();
-        dir1 = dir2 = dir3 = 1;
-        pc.printf("kalbi ");
-        wait (1);
+    }
+    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);
+    //triceps kalibratie
+    blink.attach(kaltri, 0.2);
+    for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
+        if (tri_max < tri_result) {
+            tri_max = tri_result;
         }
-        blink.detach();
-        for1 = for2 = for3 = 1;
-        pc.printf("kaltri ");
-        wait (1);
-        for1 = for2 = for3 = 0;
+        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);
+    log_timer.attach(looper, TSAMP_EMG);
 
     while(1) { //Loop keuze DIRECTION
         for(int i=1; i<4; i++) {
@@ -345,7 +343,7 @@
                 dir2=0;
                 dir3=0;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(bi_result>0.8*bi_max) {
+                    if(bi_result>FACTOR*bi_max) {
                         direction = 1;
                         pc.printf("links ");
                         wait(TIMEB4NEXTCHOICE);                 // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
@@ -360,7 +358,7 @@
                 dir2 =1;
                 dir3 =0;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(bi_result>0.8*bi_max) {
+                    if(bi_result>FACTOR*bi_max) {
                         direction = 2;
                         pc.printf("mid ");
                         wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
@@ -375,7 +373,7 @@
                 dir2 =0;
                 dir3 =1;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(bi_result>0.8*bi_max) {
+                    if(bi_result>FACTOR*bi_max) {
                         direction = 3;
                         pc.printf("rechts ");
                         wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
@@ -395,12 +393,12 @@
                 for2=0;
                 for3=0;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(tri_result>0.8*tri_max) {
+                    if(tri_result>FACTOR*tri_max) {
                         for1 = for2 = for3 = 0;
                         pc.printf("reset ");
                         goto directionchoice;
                     } else {
-                        if(bi_result>0.8*bi_max) {
+                        if(bi_result>FACTOR*bi_max) {
                             force = 1;
                             pc.printf("zwak ");
                             wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
@@ -416,12 +414,12 @@
                 for2=1;
                 for3=0;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(tri_result>0.8*tri_max) {
+                    if(tri_result>FACTOR*tri_max) {
                         for1 = for2 = for3 = 0;
                         pc.printf("reset ");
                         goto directionchoice;
                     } else {
-                        if(bi_result>0.8*bi_max) {
+                        if(bi_result>FACTOR*bi_max) {
                             force = 2;
                             pc.printf("normaal ");
                             wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
@@ -437,12 +435,12 @@
                 for2=0;
                 for3=1;
                 for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
-                    if(tri_result>0.8*tri_max) {
+                    if(tri_result>FACTOR*tri_max) {
                         for1 = for2 = for3 = 0;
                         pc.printf("reset ");
                         goto directionchoice;
                     } else {
-                        if(bi_result>0.8*bi_max) {
+                        if(bi_result>FACTOR*bi_max) {
                             force = 3;
                             pc.printf("sterk ");
                             wait(TIMEB4NEXTCHOICE);                            // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
@@ -459,13 +457,31 @@
 choicesmade:
     blink.attach(okay, 0.2);
     while(1) {
-        if(tri_result>0.8*tri_max) {
+        if(tri_result>FACTOR*tri_max) {
             blink.detach();
             pc.printf("reset ");
+            switch (direction) {
+                case 1:
+                    dir1 = 1;
+                    for1 = 1;
+                    for2 = for3 = 0;
+                    break;
+                case 2:
+                    dir2 = 1;
+                    for1 = 1;
+                    for2 = for3 = 0;
+                    break;
+                case 3:
+                    dir3 = 1;
+                    for1 = 1;
+                    for2 = for3 = 0;
+                    break;
+            }
+
             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)) {
+            if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
                 blink.detach();
                 log_timer.detach();
                 goto motorcontrol;
@@ -479,33 +495,28 @@
 
     /* 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 ");
+    switch (direction) {
+        case 1:
+            setpoint2 = 3.14;
+            break;
+        case 2:
+            setpoint2 = 3.14;
+            break;
+        case 3:
+            setpoint2 = 3.14;
+            break;
     }
-    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 ");
+
+    switch (force) {
+        case 1:
+            setpoint1 = 8;
+            break;
+        case 2:
+            setpoint1 = 8;
+            break;
+        case 3:
+            setpoint1 = 8;
+            break;
     }
 
     while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
@@ -583,7 +594,7 @@
         pwm = Kp3*controlerror + Ki3*integral;
 
         keep_in_range(&pwm, -1,1);
-        if(pwm > 0) { 
+        if(pwm > 0) {
             motor1dir = 1;
         } else {
             motor1dir = 0; //1 = rechtsom, 0 = linksom