Diff: MotorControl.cpp

Revision:

8:a435e7aa7a02

Parent:

5:6bb52b2a79bf

Child:

9:7d6fa62f9022

diff -r 8fb5513a2231 -r a435e7aa7a02 MotorControl.cpp
--- a/MotorControl.cpp	Wed Dec 26 11:04:24 2018 +0000
+++ b/MotorControl.cpp	Fri Mar 29 01:29:39 2019 +0000
@@ -1,4 +1,4 @@
-#include "mbed.h"
+ #include "mbed.h"
 
 extern                      Serial bt;
 extern                      Serial pc;
@@ -20,6 +20,7 @@
 extern int                  milLoop;
 extern int                  onewayNum;
 extern bool                 mkOn;
+extern float                errorPrevious;
 
 #define parA 343.75f        // parmeter of equation (values of distance sensor)
 #define parB -1*508.08f
@@ -28,11 +29,15 @@
 
 void MotorButton()
 {
-        
+        mkOn=0;
         milLoop = 0;
-        targetDis = 0;
+//        targetDis = 0;
         onewayNum = 0; 
-        timer3.detach(); 
+        timer3.detach();
+    
+        float d1 = disSensor.read();
+        float d2 = parA*d1*d1+parB*d1+parC;
+        targetDis = d2;      // setup to target distance in MkActionManual
         
         int a = up.read();
         int b = down.read();
@@ -40,6 +45,8 @@
         
         switch(c)
         {
+            
+            
             case 0 : //stop
         
                 enable = 0;
@@ -48,20 +55,28 @@
                 break;
         
             case 1 : // up
+            {
         
                 enable = 1;
                 p1 = 0;
                 p2 = 1;
+                
         
                 break;
+            }
         
             case 2 : // down
-            
+            {
+                
+                
+                
                 enable = 1;
                 p1 = 1;
                 p2 = 0;
+                
             
                 break;
+            }
             
             case 3 : // stop
             
@@ -118,22 +133,26 @@
         float d1 = disSensor.read();
         float d2 = parA*d1*d1+parB*d1+parC;
         float ref_d = targetDis-d2;
+//        float kp = ref_d/(errorPrevious);
         
-        if(ref_d > 0.9f)
+        if(ref_d > 0.3f)
         {
                 enable = 1;
                 p1 = 0;
                 p2 = 1;
+//                p2 = (kp<0.4?0.4:kp);
+//                p2 = (ref_d>1?1:ref_d);
                 
         }
         
-        else if(ref_d < -0.9f)
+        else if(ref_d < -0.3f)
         {
                     
                 enable = 1;
+//                p1 = -1*(ref_d<-1?-1:ref_d);
                 p1 = 1;
+//                p1 = -1*(kp>0.4?0.4:kp);
                 p2 = 0;
-                     
         }
         
         else
@@ -186,7 +205,12 @@
                 onewayNum = 0;
                 milLoop = 0;
                 enable = 0;
-                timer3.detach();             
+                timer3.detach();
+                bt.putc('<');
+                bt.putc('M');
+                bt.putc('E');
+                bt.putc('>');
+                bt.putc('\r');             
             }
     }
     
@@ -196,14 +220,21 @@
 void MkActionManual()
 {
     
-    float d1 = disSensor.read();
-    float d2 = parA*d1*d1+parB*d1+parC;
+        
+        if( 1 == mkOn)
+        {
+            mkOn = 0;
+            enable = 0;
+            timer3.detach();
+        }
+        
+        else
+        {
+            mkOn = 1;
+            onewayNum=0; 
+            milLoop = 99;
+            timer3.attach(&MkAction,0.1);
+        }
 
-        enable = 0;
-        timer3.detach();
-        onewayNum=0; 
-        targetDis = d2;
-        milLoop = 99;
-        timer3.attach(&MkAction,0.1);
 
 }
\ No newline at end of file