Diff: main.cpp

Revision:

11:97759ceeb638

Parent:

10:e628a87d18aa

Child:

12:e30bc8446379

--- a/main.cpp	Mon Nov 03 22:44:19 2014 +0000
+++ b/main.cpp	Tue Nov 04 10:53:04 2014 +0000
@@ -17,21 +17,21 @@
 #include "encoder.h"
 #include "HIDScope.h"
 
-#define THRESHOLD 0.04
+#define THRESHOLD 0.05
 #define NOSAMPL 500
 
 #define TSAMP1 0.01
-#define K_P1 (0.004)
-#define K_I1 (0.00001*TSAMP1)
-#define K_D1 (0.0001/TSAMP1)
+#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.05)
-#define KSLA_I (0.000001 *TSAMP1)
+#define KSLA_P (0.06)
+#define KSLA_I (0.00002 *TSAMP1)
 #define KSLA_D (0.0005 /TSAMP1)
 
-#define MAXPOS 700
+#define MAXPOS 750
 
 // Constantes voor de filters definiëren:
 // Constantes voor de Low Pass filter
@@ -68,6 +68,8 @@
 DigitalOut rood(LED1);
 DigitalOut blauw(LED3);
 DigitalOut groen(LED2);
+
+//emg input
 AnalogIn    emg1(PTB1);
 AnalogIn    emg2(PTB2);
 
@@ -348,7 +350,7 @@
         /*rood = 1;
         groen = 0;*/
         cout<<"ga naar mode 2"<<endl;
-        wait(0.2);
+        wait(1);
     } else if (y1>0 && y2>0) {
         check=1;
     } else if (y1>0) {
@@ -397,25 +399,25 @@
     switch(k)
     {
         case 1:
-            maxpwm=0.2; //DE MAX PWM'en AANPASSEN VOOR DE ANDERE CASES!!!!
+            maxpwm=0.6; //DE MAX PWM'en AANPASSEN VOOR DE ANDERE CASES!!!!
             break;
         case 2:
-            maxpwm=0.2;
+            maxpwm=0.8;
             break;
         case 3:
-            maxpwm=0.2;
+            maxpwm=1.0;
             break;
-        case 4:
+        /*case 4:
             maxpwm=0.2;
-            break;
+            break;*/
         default:
             maxpwm=0;
             break;
     }
-    while((encoder1.getPosition()<MAXPOS)||getv(0.01)>0.1)
+    while(abs(encoder1.getPosition()<MAXPOS))
     {
         new_pwm=pidarm(MAXPOS,encoder1.getPosition());
-        clamp(&new_pwm,-maxpwm,maxpwm);
+        clamp(&new_pwm,-1,maxpwm);
         if (new_pwm>0)
         {
             dir1=1;
@@ -426,12 +428,16 @@
         }
         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;
     }
-      
- pwm1.write(0);
- wait(2);
- //cout<< 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)
@@ -445,7 +451,6 @@
     clamp(&out_i1,-I_LIMIT1,I_LIMIT1);
     prev_error = error;
     out  = out_i1+out_p+out_d;
-    cout<<"out: "<<out<<endl;
     return out;
 }
 
@@ -474,10 +479,8 @@
         check=0;
         /*rood = 0;
         groen = 1;*/
-        cout<<"ga naar mode 1"<<endl;
     } else if (y1>0 && y2>0) {
         check=1;
-        cout<<"zo naar mode 1"<<endl;
     } else if (y1>0) {
         stand=stand+1;
         check=0;
@@ -508,22 +511,28 @@
             break;
 
         case 3:
-            pos = 250;
+            pos = 300;
+            break;
+        default:
             break;
     }
     
-    while((abs(pos-encoder1.getPosition()) >6)|| (v1!= 0)) {
+    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) {
+        } else if(out1<0) {  
             dir1 = 0;
         }
+        else{
+            }
         pwm1 = fabs(out1);
         v1 = getv(0.001);
     }
@@ -544,7 +553,7 @@
     wait(2);
     cout<<"Begin programma"<<endl;
     while(1) {
-        //Per TSAMP word EMG uitgelzen, gefilterd en gemiddeld
+        //Per TSAMP word EMG uitgelezen, gefilterd en gemiddeld
         while(!looptimerflag);
         looptimerflag = false;
         emg_value1 =  readEMG1();
@@ -573,6 +582,7 @@
                     cout<<"fase1"<<endl;
                     badjestand=badje(y1,y2);
                     batposition(badjestand);
+                    armstand=1;
                     break;
 
                 case 1: