Diff: NR_method_1.cpp

Revision:

8:364ea64ae86b

Parent:

7:fcb20c3ccee9

Child:

9:4fc2659cfb26

diff -r fcb20c3ccee9 -r 364ea64ae86b NR_method_1.cpp
--- a/NR_method_1.cpp	Thu Nov 01 14:56:40 2018 +0000
+++ b/NR_method_1.cpp	Thu Nov 01 18:10:54 2018 +0000
@@ -13,7 +13,7 @@
 //#include  <MODSERIAL.h>
 
 // hidscope
-HIDScope    scope( 2 );
+HIDScope    scope( 4 );
 bool startCalc;
 bool calpos1;
 bool calpos2;
@@ -33,8 +33,8 @@
 // tickers
 Ticker sample_timer;
 
-volatile int x1;
-volatile int y1;
+volatile float x1;
+volatile float y1;
 double emgFiltered3;
 double emgFiltered23;
 bool dir = true;
@@ -80,6 +80,7 @@
 
 int counts = 8400;
 DigitalOut Led(LED1);
+DigitalOut Led2(LED2);
 PwmOut PMW1(D5); // Motor 1
 DigitalOut M1(D4); // direction of motor 1
 PwmOut PMW2(D6); // Motor 2
@@ -148,26 +149,26 @@
 void Position1x(double b)
 {
     if (b > 0.20) {
+        Led2 = 0;
         i = 0;
         Cxx =x1;
         if (dir == true) {
-            if(x1 > -46) {
-                x1 = x1-4.1;
-                pc.printf(" posx is %f\n\r",Cxx);
+            if(x1 > -46.3) {
+                x1 = x1-4.2;
                 //return x1;
 
-            } else if ( x1 <= -46) {
-                x1 =-14;
+            } else if ( x1 <= -46.3) {
+                x1 =-17;
                 //return x1;
             } else {
             }
         } else {
-            if(x1 < -14) {
-                x1 = x1+4.1;
+            if(x1 < -17) {
+                x1 = x1+4.2;
                 //return x1;
 
-            } else if ( x1 >= -14) {
-                x1 = -46;
+            } else if ( x1 >= -17) {
+                x1 = -46.3;
                 //return x1;
             } else {
             }
@@ -177,24 +178,25 @@
 
 void Position1y(double b)
 {
-    if (b > 0.20) {
+    if (b > 0.18) {
+        Led2 = 0;
         i = 0;
         Cyy=y1;
         if(dir == true) {
-            if(y1 < 43) {
-                y1 = y1+4.1;
+            if(y1 < 32.4) {
+                y1 = y1+4.2;
                 //return y1;
-            } else if ( y1 >= 43) {
-                y1 = 11;
+            } else if ( y1 >= 32.4) {
+                y1 = 3;
                 //return y1;
             } else {
             }
         } else {
-            if(y1 > 11) {
-                y1 = y1-4.1;
+            if(y1 > 3) {
+                y1 = y1-4.2;
                 //return y1;
-            } else if ( y1 <= 11) {
-                y1 = 43;
+            } else if ( y1 <= 3) {
+                y1 = 32.4;
                 //return y1;
             } else {
             }
@@ -258,18 +260,18 @@
         if (ey >= Cyy - 0.01 && ey <= Cyy + 0.01) {
         } else {
             if (ey > Cyy) {
-                ey = ey - 0.004;
+                ey = ey - 0.008;
             }
             if (ey < Cyy) {
-                ey = ey + 0.004;
+                ey = ey + 0.008;
             }
         }
     } else {
         if (ex > Cxx) {
-            ex = ex - 0.004;
+            ex = ex - 0.008;
         }
         if (ex < Cxx) {
-            ex = ex + 0.004;
+            ex = ex + 0.008;
         }
     }
 }
@@ -326,8 +328,8 @@
     while(waiting <=2)
         if (bas == true) {
             if (waiting == 1) {
-                Cxx = -20;
-                Cyy = 10;
+                Cxx = -17;
+                Cyy = 3;
 
                 position_define();
                 angle_define();
@@ -336,8 +338,8 @@
             }
 
             if(waiting == 2) {
-                Cxx = -45;
-                Cyy = 10;
+                Cxx = -45.8;
+                Cyy = 7;
                 position_define();
                 angle_define();
                 motor_controler();
@@ -356,7 +358,6 @@
                 PMW1.write(0);
                 count2 = count1;
                 count1 = Enc1.getPulses();
-                wait(0.1);
             }
             Enc1.reset();
             bb= Enc1.getPulses();
@@ -365,15 +366,13 @@
         if(calpos2 == false) {
             while(abs(count4-count3) > 0) {
                 PMW2.write(0.1f);
-                wait(0.1);
-                PMW2.write(0);
                 M1=0;
                 PMW1.write(0.1f);
                 wait(0.1);
+                PMW2.write(0);
                 PMW1.write(0);
                 count4 = count3;
                 count3 = Enc2.getPulses();
-                wait(0.1);
             }
             Enc2.reset();
             bc= Enc2.getPulses();
@@ -386,6 +385,7 @@
 int main()
 {
     Led = 1;
+    Led2 = 0;
     M1 = 1;
     M2 = 1;
     startCalc = false;
@@ -393,7 +393,7 @@
     calpos2 = false;
     count2 = 10000;
     count4 = 10000;
-    Led = 0;
+    Led = 1;
     PMW1.write(0.1f);
     wait(0.1);
     PMW1.write(0);
@@ -404,6 +404,7 @@
     count3 = Enc2.getPulses();
     
     setCalibration();
+    Led2 = 1;
     button.fall(&change_wait);
     PMW1.period_us(60);
     PMW2.period_us(60);
@@ -432,8 +433,10 @@
             angle_define();
             motor_controler();
 
-            scope.set(0, Cxx); // filtered 1
-            scope.set(1, Cyy); //filtered signal 2
+            scope.set(0, ex); // filtered 1
+            scope.set(1, Cxx); //filtered signal 2
+            scope.set(2, ey);
+            scope.set(3, Cyy);
             scope.send();
 
             if (i <= 250) {
@@ -443,10 +446,12 @@
             }
 
 
-            //sw2.fall(change);
+            sw2.fall(change);
             Position1x(emgFiltered3);
             Position1y(emgFiltered23);
-
+            Cxx = x1;
+            Cyy = y1;
+            Led2 = 1;
             Led = 0;
             bas= false;
         }