Diff: main.cpp

Revision:

64:21e3e832f127

Parent:

63:90cc947c3d77

Child:

65:e1bc4d5cb5d2

--- a/main.cpp	Tue Nov 04 08:48:21 2014 +0000
+++ b/main.cpp	Tue Nov 04 11:22:53 2014 +0000
@@ -5,8 +5,8 @@
 #include "arm_math.h"
 #include "TouchButton.h"
 
-#define K_P (0.5)
-#define K_I (0.0  *TSAMP1)
+#define K_P (0.005)
+#define K_I (0.00  *TSAMP1)
 #define K_D (0  /TSAMP1)
 #define I_LIMIT 1.
 #define K_Pp (0.003)
@@ -85,7 +85,7 @@
 float new_pwm;
 float PWM2 = 0.3; //PWM voor instellen hoek batje
 int toestand = WACHTEN;
-float setspeed = 0, V3=150, V2=1, V1 =10;//V in counts/s
+float setspeed = 0, V3=150, V2=70, V1 =50;//V in counts/s
 
 Encoder motor1(PTD5,PTD3, true);
 Encoder motor2(PTD0,PTD2);
@@ -142,6 +142,10 @@
     pc.printf("key 3 START\r\n");
 
     while(true) {
+        rood=0;
+        wit=0;
+        blauw=0;
+        groen=0;
 
         key = TButton.PressedButton();
 
@@ -155,7 +159,7 @@
             pc.printf("calibratie tricep aan\n");
             wait(2);
 
-            /*Calibratie_Triceps();
+            Calibratie_Triceps();
             drempelwaardeT=MOVAVG_T-1;
             pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT);
 
@@ -171,19 +175,20 @@
             myled2=1;
             myled3=1;
             rood=0;
-            wit=0;*/
+            wit=0;
         }
         if (key==2) {
             //green
             myled1 = 1;
             myled2 = 0;
             myled3 = 1;
+            rood=0;
             groen=1;
 
             pc.printf("calibratie bicep snelheid 1 aan\n");
             wait(2);
 
-            /*Calibratie_Biceps();
+            Calibratie_Biceps();
             drempelwaardeB1=MOVAVG_B-1;
             pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1);
             myled1 = 0;
@@ -196,6 +201,7 @@
             myled1 = 1;
             myled2 = 0;
             myled3 = 1;
+            rood=0;
 
             pc.printf("calibratie biceps snelheid 2 aan\n");
             wait(2);
@@ -214,6 +220,7 @@
             myled1 = 1;
             myled2 = 0;
             myled3 = 1;
+            rood=0;
 
             pc.printf("calibratie biceps snelheid 3 aan\n");
             wait(2);
@@ -232,15 +239,17 @@
             groen=0;
             myled1=1;
             myled2=1;
-            myled3=1;*/
+            myled3=1;
         }
         if (key==3) {
+            motor2.setPosition(0);
             //blue
+
             myled1 = 1;
             myled2 = 1;
             myled3 = 0;
             blauw=1;
-            /*wait(3);
+            wait(2);
 
             if(drempelwaardeT==0) {
                 pc.printf("geen waarde calibratie TRICEPS \n");
@@ -248,7 +257,8 @@
                 myled2 = 0;
                 myled3 = 0;
                 wit=1;
-                rood=1;
+                wait(0.5);
+                wit=0;
             }
             if (drempelwaardeB1==0) {
                 pc.printf("geen waarde calibratie BICEPS 1 \n");
@@ -256,7 +266,8 @@
                 myled2 = 0;
                 myled3 = 0;
                 wit=1;
-                groen=1;
+                wait(0.5);
+                wit=0;
             }
             if (drempelwaardeB2==0) {
                 pc.printf("geen waarde calibratie BICEPS 2 \n");
@@ -264,7 +275,8 @@
                 myled2 = 0;
                 myled3 = 0;
                 wit=1;
-                groen=1;
+                wait(0.5);
+                wit=0;
             }
             if (drempelwaardeB3==0) {
                 pc.printf("geen waarde calibratie BICEPS 3 \n");
@@ -272,12 +284,13 @@
                 myled2 = 0;
                 myled3 = 0;
                 wit=1;
-                groen=1;
+                wait(0.5);
+                wit=0;
             } else {
-                pc.printf("eerst positie dan snelheid aangeven /n"); */
+                pc.printf("eerst positie dan snelheid aangeven /n");
 
                 //bepaling van positie met triceps 1
-                /*Ticker log_timerT1;
+                Ticker log_timerT1;
 
                 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
                 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
@@ -306,7 +319,7 @@
                 myled3 = 0;
                 rood=0;
 
-                wait(3);
+                wait(1);
 
                 //bepaling van positie met tricep 2
                 Ticker log_timerT2;
@@ -354,14 +367,14 @@
 
                 Ticker looptimer2;
                 looptimer2.attach(motor2aansturing,TSAMP1);
-                wait(8);
+                wait(3);
                 looptimer2.detach();
                 pc.printf("Detach Motor 2\n");
 
-                //   eind aansturing motor 2*/
+                //   eind aansturing motor 2
 
-                wait(2);
-                /*Ticker log_timerB;
+                wait(1);
+                Ticker log_timerB;
 
                 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
                 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
@@ -370,6 +383,7 @@
                 myled1 = 1;
                 myled2 = 0;
                 myled3 = 1;
+                rood=0;
                 groen=1;
 
                 log_timerB.attach(Biceps,0.005);
@@ -401,9 +415,9 @@
                 groen=0;
 
                 // *** INPUT MOTOR 1 ***
-                snelheidsstand=yB1+yB2+yB3; */
-                snelheidsstand=2;
-                               //controle snelheidsstand op scherm
+                snelheidsstand=yB1+yB2+yB3;
+
+                //controle snelheidsstand op scherm
                 if (snelheidsstand==0) {
                     pc.printf("Motor 1 beweegt niet \n");
                 } else {
@@ -445,7 +459,7 @@
                 groen=0;
                 blauw=0;
             }
-        //}
+        }
     }
 }//end int main
 
@@ -669,7 +683,7 @@
                 motordir2 = 1;
                 pwm_motor2.write(PWM2);
             }
-            if (motor2.getPosition()<= 20) {
+            if (motor2.getPosition()<= 40) {
                 motordir2 = 0;
                 pwm_motor2.write(PWM2);
             } else {