Diff: MotorControl.cpp

Revision:

4:bf4ad2079096

Child:

5:6bb52b2a79bf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorControl.cpp	Fri Dec 21 20:39:24 2018 +0000
@@ -0,0 +1,210 @@
+#include "mbed.h"
+
+extern                      Serial bt;
+extern                      Serial pc;
+
+DigitalOut                  enable(PB_12);           
+PwmOut                      p1(PB_13);               
+PwmOut                      p2(PB_14);               
+
+extern                      InterruptIn up;               
+extern                      InterruptIn down;
+
+extern                      AnalogIn disSensor;
+
+extern                      Serial bt;
+extern                      Ticker timer1;               
+extern                      Ticker timer3;
+
+extern int                  targetDis;
+extern int                  milLoop;
+extern int                  onewayNum;
+extern bool                 mkOn;
+
+
+#define parA 347.44f        // parmeter of equation (values of distance sensor)
+#define parB -1*481.16f
+#define parC 155.42f
+
+
+void MotorButton()
+{
+        
+        milLoop = 0;
+        targetDis = 0;
+        onewayNum = 0; 
+        timer3.detach(); 
+        
+        int a = up.read();
+        int b = down.read();
+        int c = a*2 + b;
+        
+        switch(c)
+        {
+            case 0 : //stop
+        
+                enable = 0;
+                p1 = 0;     
+                p2 = 0;
+                break;
+        
+            case 1 : // up
+        
+                enable = 1;
+                p1 = 0;
+                p2 = 1;
+        
+                break;
+        
+            case 2 : // down
+            
+                enable = 1;
+                p1 = 1;
+                p2 = 0;
+            
+                break;
+            
+            case 3 : // stop
+            
+                enable = 0;
+                p1 = 0;
+                p2 = 0;
+            
+                break;
+                
+        }
+       
+            
+    
+}
+
+void MotorTest(int mode)
+{
+    
+    timer3.detach();
+    
+    switch (mode)
+    
+    {
+        case 0 : //stop
+            
+            enable = 0;
+
+            break;
+    
+        case 1 : // up
+    
+            enable = 1;
+            p1 = 0;
+            p2 = 1;
+    
+            break;
+    
+        case 2 : // down
+        
+            enable = 1;
+            p1 = 1;
+            p2 = 0;
+        
+            break;       
+            
+    }
+    
+              
+}
+
+void ControlAng()
+{
+        
+        float d1 = disSensor.read();
+        float d2 = parA*d1*d1+parB*d1+parC;
+        float ref_d = targetDis-d2;
+        
+        if(ref_d > 0.9f)
+        {
+                enable = 1;
+                p1 = 0;
+                p2 = 1;
+                
+        }
+        
+        else if(ref_d < -0.9f)
+        {
+                    
+                enable = 1;
+                p1 = 1;
+                p2 = 0;
+                     
+        }
+        
+        else
+        {
+                enable = 0;
+                timer1.detach();
+                  
+        }
+}
+
+
+void MkAction()
+{
+    
+
+    float d1 = disSensor.read();
+    float d2 = parA*d1*d1+parB*d1+parC;
+    
+    static float curTarget;
+    float s;
+    float abs_error;
+    
+    curTarget = (onewayNum%2==0)? targetDis: 1.2f ; // initial angle
+    
+    s = (curTarget-d2)/targetDis;
+    abs_error = abs(curTarget-d2);
+    
+      
+    if(s>=0)
+    {
+        enable = 1;
+            p1 = 0;
+            p2 = 1; //(s>0.7f)?s:0.7f;
+    }
+    
+    else
+    {
+        enable = 1;
+            p1 = 1; //(s<-0.7f)?(-1*s):0.7f;
+            p2 = 0;
+    }
+    
+    if (abs_error <0.5f)
+    {
+          
+        onewayNum = onewayNum +1;
+        
+        if(onewayNum >=milLoop*2)
+            {
+                onewayNum = 0;
+                milLoop = 0;
+                enable = 0;
+                timer3.detach();             
+            }
+    }
+    
+    
+}
+
+void MkActionManual()
+{
+    
+    float d1 = disSensor.read();
+    float d2 = parA*d1*d1+parB*d1+parC;
+
+        enable = 0;
+        timer3.detach();
+        onewayNum=0; 
+        targetDis = d2;
+        milLoop = 99;
+        timer3.attach(&MkAction,0.1);
+
+}
\ No newline at end of file