Diff: MotorControl.cpp

Revision:

10:3fcaf50f528f

Parent:

9:7d6fa62f9022

Child:

11:5f05b14649ee

diff -r 7d6fa62f9022 -r 3fcaf50f528f MotorControl.cpp
--- a/MotorControl.cpp	Tue Jun 25 02:10:31 2019 +0000
+++ b/MotorControl.cpp	Tue Jul 02 04:44:35 2019 +0000
@@ -23,10 +23,9 @@
 extern bool                 mkOn;
 extern float                errorPrevious;
 
-#define parA 343.75f        // parmeter of equation (values of distance sensor)
-#define parB -1*508.08f
-#define parC 166.67f
-
+extern float parA;
+extern float parB;
+extern float parC;
 
 void MotorButton()
 {
@@ -54,6 +53,7 @@
                 nreset = 0;
                 p1 = 0;     
                 p2 = 0;
+                
                 break;
         
             case 1 : // up
@@ -108,6 +108,9 @@
         case 0 : //stop
             
             enable = 0;
+            nreset = 0;
+            p1 = 0;     
+            p2 = 0;
 
             break;
     
@@ -145,6 +148,7 @@
         if(ref_d > 0.3f)
         {
                 enable = 1;
+                nreset = 1;
                 p1 = 0;
                 p2 = 1;
 //                p2 = (kp<0.4?0.4:kp);
@@ -154,8 +158,9 @@
         
         else if(ref_d < -0.3f)
         {
-                    
+                
                 enable = 1;
+                nreset = 1;
 //                p1 = -1*(ref_d<-1?-1:ref_d);
                 p1 = 1;
 //                p1 = -1*(kp>0.4?0.4:kp);
@@ -165,6 +170,7 @@
         else
         {
                 enable = 0;
+                nreset = 0;
                 timer1.detach();
                   
         }
@@ -174,7 +180,6 @@
 void MkAction()
 {
     
-
     float d1 = disSensor.read();
     float d2 = parA*d1*d1+parB*d1+parC;
     
@@ -182,7 +187,7 @@
     float s;
     float abs_error;
     
-    curTarget = (onewayNum%2==0)? targetDis: 0.0f ; // initial angle
+    curTarget = (onewayNum%2==0)? targetDis: 0.2f ; // initial angle
     
     s = (curTarget-d2)/targetDis;
     abs_error = abs(curTarget-d2);
@@ -191,27 +196,37 @@
     if(s>=0)
     {
         enable = 1;
+        nreset = 1;
             p1 = 0;
-            p2 = 1; //(s>0.7f)?s:0.7f;
+            p2 = 0; //(s>0.7f)?s:0.7f;
+            wait(0.01);
+            p2 = 1;
     }
     
     else
     {
         enable = 1;
-            p1 = 1; //(s<-0.7f)?(-1*s):0.7f;
+        nreset = 1;
+            p1 = 0; //(s<-0.7f)?(-1*s):0.7f;
             p2 = 0;
+            wait(0.01);
+            p1 = 1;
     }
     
-    if (abs_error <0.7f)
+   
+
+    if (abs_error <1.5f)
     {
-          
         onewayNum = onewayNum +1;
+        enable = 0; 
+        nreset = 0; //Hbridge ouput reset! 190627 nreset problem
         
         if(onewayNum >=milLoop*2)
             {
                 onewayNum = 0;
                 milLoop = 0;
                 enable = 0;
+                nreset = 0;
                 timer3.detach();
                 bt.putc('<');
                 bt.putc('M');
@@ -232,6 +247,7 @@
         {
             mkOn = 0;
             enable = 0;
+            nreset = 0;
             timer3.detach();
         }