Diff: MotorControl.cpp

Revision:

8:2a4298cbae3f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorControl.cpp	Sat May 04 18:51:43 2019 +0000
@@ -0,0 +1,83 @@
+#include "mbed.h"
+#include "QEI.h"
+ 
+extern              Serial bt;
+DigitalOut          Enable(A0);
+DigitalOut          M1(PA_1);
+DigitalOut          M2(PF_1);
+
+
+extern              QEI re;
+extern              Ticker      timer1;
+extern int          targetDis;
+
+void MotorTest(int mode)
+{
+    
+    switch (mode)
+    
+    {
+        case 0 : //stop
+            
+            Enable = 0;
+            timer1.detach();
+
+            break;
+    
+        case 1 : // right
+    
+            Enable = 1;
+            M1 = 1;
+            M2 = 0;            
+    
+            break;
+    
+        case 2 : // left
+        
+            Enable = 1;
+            M1 = 0;
+            M2 = 1;
+        
+            break;       
+            
+    }
+    
+              
+}
+
+void ControlPos()
+{
+        
+        int pulse = re.getPulses();
+        int d = pulse/200;
+       
+        float ref_d = (targetDis-d);
+        float abs_ref_d = abs(ref_d);
+//        float s;
+//        s = abs_ref_d; 
+        
+        if(-0.8f > ref_d)
+        {
+                Enable = 1;
+                M1 = 0;
+                M2 = 1; //(s<3)?s/5:1;
+                
+        }
+        
+        else if ( 0.8f <ref_d)
+        {
+                    
+                Enable = 1;
+                M1 = 1; //(s<3)?s/5:1;
+                M2 = 0;
+                        
+        }
+        
+        if (abs_ref_d < 0.8f)
+        {
+            
+                Enable = 0;
+                timer1.detach();
+                  
+        }
+}
\ No newline at end of file