Diff: NR_method_1.cpp

Revision:

7:fcb20c3ccee9

Parent:

6:e492bc8fc3fb

Child:

8:364ea64ae86b

--- a/NR_method_1.cpp	Thu Nov 01 12:59:27 2018 +0000
+++ b/NR_method_1.cpp	Thu Nov 01 14:56:40 2018 +0000
@@ -14,14 +14,17 @@
 
 // hidscope
 HIDScope    scope( 2 );
-
+bool startCalc;
+bool calpos1;
+bool calpos2;
 bool bas;
 int waiting;
+int count1;
+int count2;
+int count3;
+int count4;
 InterruptIn button(SW3);
 
-InterruptIn xxx(D0);
-InterruptIn yyy(D1);
-
 Serial pc(USBTX, USBRX);
 // emg signals input
 AnalogIn   emg1(A0);
@@ -120,8 +123,8 @@
 double C = 20;
 double D = 27;
 double E = 35;
-double ex = -35; // current position
-double ey = 27; // current position
+double ex = -40; // current position
+double ey = -30; // current position
 double Cxx = -35; // Goal position
 double Cyy = 27; // Goal position
 
@@ -149,7 +152,7 @@
         Cxx =x1;
         if (dir == true) {
             if(x1 > -46) {
-                x1 = x1-4;
+                x1 = x1-4.1;
                 pc.printf(" posx is %f\n\r",Cxx);
                 //return x1;
 
@@ -160,7 +163,7 @@
             }
         } else {
             if(x1 < -14) {
-                x1 = x1+4;
+                x1 = x1+4.1;
                 //return x1;
 
             } else if ( x1 >= -14) {
@@ -179,7 +182,7 @@
         Cyy=y1;
         if(dir == true) {
             if(y1 < 43) {
-                y1 = y1+4;
+                y1 = y1+4.1;
                 //return y1;
             } else if ( y1 >= 43) {
                 y1 = 11;
@@ -188,7 +191,7 @@
             }
         } else {
             if(y1 > 11) {
-                y1 = y1-4;
+                y1 = y1-4.1;
                 //return y1;
             } else if ( y1 <= 11) {
                 y1 = 43;
@@ -285,8 +288,8 @@
 
 void motor_controler()
 {
-    bb = -(Enc1.getPulses()) - 201;
-    bc = Enc2.getPulses() + 2100;
+    bb = -(Enc1.getPulses()) - 816;
+    bc = Enc2.getPulses() + 4316;
 
     if (bb >= counts_a) {
         z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
@@ -321,25 +324,61 @@
 {
     waiting = 1;
     while(waiting <=2)
-    if (bas == true)
-    {
-        if (waiting == 1) {
-            Cxx = -20;
-            Cyy = 10;
+        if (bas == true) {
+            if (waiting == 1) {
+                Cxx = -20;
+                Cyy = 10;
+
+                position_define();
+                angle_define();
+                motor_controler();
+
+            }
 
-            position_define();
-            angle_define();
-            motor_controler();
+            if(waiting == 2) {
+                Cxx = -45;
+                Cyy = 10;
+                position_define();
+                angle_define();
+                motor_controler();
+            }
+        }
+}
+
+void setCalibration()
+{
+    if (startCalc == false) {
 
+        if (calpos1 == false) {
+            while(abs(count2-count1) > 0) {
+                PMW1.write(0.1f);
+                wait(0.1);
+                PMW1.write(0);
+                count2 = count1;
+                count1 = Enc1.getPulses();
+                wait(0.1);
+            }
+            Enc1.reset();
+            bb= Enc1.getPulses();
+            calpos1 = true;
         }
-
-    if(waiting == 2) {
-        Cxx = -45;
-        Cyy = 10;
-        position_define();
-        angle_define();
-        motor_controler();
-    }
+        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);
+                PMW1.write(0);
+                count4 = count3;
+                count3 = Enc2.getPulses();
+                wait(0.1);
+            }
+            Enc2.reset();
+            bc= Enc2.getPulses();
+            calpos2 = true;
+        }
     }
 }
 
@@ -347,8 +386,27 @@
 int main()
 {
     Led = 1;
+    M1 = 1;
+    M2 = 1;
+    startCalc = false;
+    calpos1 = false;
+    calpos2 = false;
+    count2 = 10000;
+    count4 = 10000;
+    Led = 0;
+    PMW1.write(0.1f);
+    wait(0.1);
+    PMW1.write(0);
+    count1 = Enc1.getPulses();
+    PMW2.write(0.1f);
+    wait(0.1);
+    PMW2.write(0);
+    count3 = Enc2.getPulses();
+    
+    setCalibration();
+    button.fall(&change_wait);
     PMW1.period_us(60);
-    button.fall(&change_wait);
+    PMW2.period_us(60);
     position_controll.attach(position_controll_void,0.002);
     X[0][0] = X0[0][0];
     X[1][0] = X0[1][0];
@@ -373,23 +431,22 @@
             position_define();
             angle_define();
             motor_controler();
-            
+
             scope.set(0, Cxx); // filtered 1
             scope.set(1, Cyy); //filtered signal 2
             scope.send();
-            
-            if (i <= 250)
-            {
+
+            if (i <= 250) {
                 emgFiltered3 = 0;
                 emgFiltered23 = 0;
-                i++;   
+                i++;
             }
-            
-            
+
+
             //sw2.fall(change);
             Position1x(emgFiltered3);
             Position1y(emgFiltered23);
-            
+
             Led = 0;
             bas= false;
         }