Diff: NR_method_1.cpp

Revision:

11:1eee224b54d8

Parent:

10:86c810be889a

Child:

12:56775c151fee

diff -r 86c810be889a -r 1eee224b54d8 NR_method_1.cpp
--- a/NR_method_1.cpp	Thu Nov 01 20:05:45 2018 +0000
+++ b/NR_method_1.cpp	Fri Nov 02 09:20:12 2018 +0000
@@ -15,6 +15,8 @@
 AnalogIn   emg2(A1); // emg2 input
 InterruptIn sw2(SW2);
 InterruptIn button(SW3);
+InterruptIn emgbutton(D0);
+InterruptIn emgbutton2(D1);
 DigitalOut Led(LED1);
 DigitalOut Led2(LED2);
 PwmOut PMW1(D5); // Motor 1
@@ -23,7 +25,7 @@
 DigitalOut M2(D7); // direction of motor 2
 
 // hidscope
-HIDScope    scope( 4 ); // 4 to check the different 
+HIDScope    scope( 4 ); // 4 to check the different
 
 // tickers
 Ticker sample_timer;
@@ -54,10 +56,10 @@
 double ey = -30; // current position
 double Cxx = -35; // Goal position
 double Cyy = 27; // Goal position
-double Kp = 1;
-double Ki = 1;
-double Kd = 0.3;
-double Ts = 0.001;
+double Kp = 4;
+double Ki = 5;
+double Kd = 0.4;
+double Ts = 0.002;
 bool startCalc;
 bool calpos1;
 bool calpos2;
@@ -70,12 +72,17 @@
 float counts_a;
 float counts_b;
 int counts = 8400;
-volatile float x1;
-volatile float y1;
+float x1;
+float y1;
 double emgFiltered3;
 double emgFiltered23;
 bool dir = true;
-
+int countsemg = 0;
+bool calibration;
+double sumemgx;
+double sumemgy;
+double threshholdx = 5;
+double threshholdy = 5;
 // filtering
 //filter coeffiecents
 // highpass
@@ -135,7 +142,7 @@
 
 void Position1x(double b)
 {
-    if (b > 0.20) {
+    if (b > threshholdx) {
         Led2 = 0;
         i = 0;
         Cxx =x1;
@@ -143,7 +150,7 @@
         if (dir == true) {
             if(x1 > -46.3) {
                 x1 = x1-4.2;
- 
+
 
             } else if ( x1 <= -46.3) {
                 x1 =-17;
@@ -159,12 +166,13 @@
             } else {
             }
         }
+        pc.printf("%f",x1);
     }
 }
 
 void Position1y(double b)
 {
-    if (b > 0.18) {
+    if (b > threshholdy) {
         Led2 = 0;
         i = 0;
         Cyy=y1;
@@ -188,6 +196,7 @@
             } else {
             }
         }
+        pc.printf("%f",y1);
     }
 }
 void change()
@@ -247,18 +256,18 @@
         if (ey >= Cyy - 0.01 && ey <= Cyy + 0.01) {
         } else {
             if (ey > Cyy) {
-                ey = ey - 0.008;
+                ey = ey - 0.004;
             }
             if (ey < Cyy) {
-                ey = ey + 0.008;
+                ey = ey + 0.004;
             }
         }
     } else {
         if (ex > Cxx) {
-            ex = ex - 0.008;
+            ex = ex - 0.004;
         }
         if (ex < Cxx) {
-            ex = ex + 0.008;
+            ex = ex + 0.004;
         }
     }
 }
@@ -326,7 +335,7 @@
 
             if(waiting == 2) {
                 Cxx = -45.8;
-                Cyy = 7;
+                Cyy = 3;
                 position_define();
                 angle_define();
                 motor_controler();
@@ -368,6 +377,18 @@
     }
 }
 
+void emgCalibration()
+{
+    calibration = true;
+    sumemgx = 0;
+    sumemgy = 0;
+}
+
+void emgprint()
+{
+    pc.printf("%f %f %f %f ", threshholdx,threshholdy,sumemgx,sumemgy) ;
+}
+
 
 int main()
 {
@@ -391,7 +412,8 @@
     wait(0.1);
     PMW2.write(0);
     count3 = Enc2.getPulses();
-    
+    emgbutton.fall(&emgCalibration);
+    emgbutton2.fall(&emgprint);
     setCalibration();
     pc.printf("Calibration is done\r\n");
     pc.printf("Please press button SW3\n\r");
@@ -417,35 +439,65 @@
     i = 251;
     while(true) {
 
+
+
         if(bas == true) {
-            Led = 1;
-            filteren();
-            position_define();
-            angle_define();
-            motor_controler();
+            if(calibration == true) {
+                Led = 1;
+                Led2 = 0;
+                filteren();
 
-            scope.set(0, ex); // filtered 1
-            scope.set(1, Cxx); //filtered signal 2
-            scope.set(2, ey);
-            scope.set(3, Cyy);
-            scope.send();
+                if (countsemg <= 2500) {
+                    sumemgx = sumemgx + emgFiltered3;
+                    sumemgy = sumemgy + emgFiltered23;
+                    countsemg ++;
+                scope.set(0, emgFiltered3); // filtered 1
+                scope.set(1, emgFiltered23); //filtered signal 2
+                
+                scope.send();
+                } else {
+                    calibration = false;
+                    countsemg = 0;
+                }
 
-            if (i <= 250) {
-                emgFiltered3 = 0;
-                emgFiltered23 = 0;
-                i++;
             }
+                if (countsemg == 2500)
+                {
+                threshholdx = (sumemgx / 2500) * 0.9;
+                threshholdy = (sumemgy / 2500)  * 0.9;
+                }
 
 
-            sw2.fall(change);
-            Position1x(emgFiltered3);
-            Position1y(emgFiltered23);
-            Cxx = x1;
-            Cyy = y1;
-            Led2 = 1;
-            Led = 0;
-            bas= false;
+                Led2 = 1;
+                filteren();
+                position_define();
+                angle_define();
+                motor_controler();
+
+                //scope.set(0, ex); // filtered 1
+                //scope.set(1, Cxx); //filtered signal 2
+                scope.set(2, emgFiltered3);
+                scope.set(3, emgFiltered23);
+                scope.send();
+                
+
+                if (i <= 250) {
+                    emgFiltered3 = 0;
+                    emgFiltered23 = 0;
+                    i++;
+                }
+
+
+                sw2.fall(change);
+                Position1x(emgFiltered3);
+                Position1y(emgFiltered23);
+                Cxx = x1;
+                Cyy = y1;
+                Led2 = 1;
+                Led = 0;
+                bas= false;
+            
         }
     }
+}
 
-}