Diff: src/motor.cpp

Revision:

3:0cba1132c8a3

Parent:

2:50062ac8646d

--- a/src/motor.cpp	Sat May 07 12:27:09 2016 +0000
+++ b/src/motor.cpp	Tue May 31 00:26:35 2016 +0000
@@ -1,15 +1,18 @@
 #include "motor.h"
 
-PwmOut motor[6] = {PwmOut(D8), PwmOut(D9), PwmOut(D10), PwmOut(D11), PwmOut(D12), PwmOut(D13)};
+//PwmOut motor[6] = {PwmOut(D8), PwmOut(D9), PwmOut(D10), PwmOut(D11), PwmOut(D12), PwmOut(D13)};
 extern char position[6];
 
+/*
 void motor_drive() {
     for(int index = 0; index < 6; index++) {
         float duty = (0.000417 * position[index] + 0.05);
         motor[index] = duty;            
     }
 }
+*/
 
+/*
 void motor_init() {
         for (int index = 0; index < 6; index++) {
                 motor[index].period(0.020);                                 //cycle period is 20ms
@@ -18,4 +21,58 @@
 //      motor[0].period(0.020);
 //      motor[0] = 0.05;
 }
- 
\ No newline at end of file
+*/
+
+
+DigitalOut xian[4][6]={{PA_1,PA_2,PA_3,PA_4,PA_5,PA_6},{PB_1,PB_2,PB_3,PB_4,PB_5,PB_6},{PC_1,PC_2,PC_3,PC_4,PC_5,PC_6},{PA_7,PA_8,PA_9,PB_7,PB_8,PB_9}};    
+const int zpai[4][4]={{1,1,0,0},{0,1,1,0},{0,0,1,1},{1,0,0,1}};           //正转，前面是拍(k)，后面是线(j)
+const int fpai[4][4]={{1,1,0,0},{1,0,0,1},{0,0,1,1},{0,1,1,0}};           //反转
+const float steplong=1.8;                  //步长待补充
+int step[6];
+
+void getstep()
+{
+    for(int i=0;i<6;i++)
+        step[i]=position[i]/steplong;         //待补充计算方法，缺少步长steplong
+}
+
+int max(int a[],int n)                      //计算最大值
+{
+    int m=a[0];
+    for(int t=1;t<n;t++)
+        m=(m>a[t]?m:a[t]);
+    return m;
+}
+
+void motor_drive()                         //平台完成一次移动
+{
+    int m;
+    m=max(step,6);
+    for(int n=0;n<m;n++)                    //最大步数
+    {
+        for(int k=0;k<4;k++)                    //确定拍数（k）
+        {
+            for(int j=0;j<4;j++)                  //确定信号线(j)
+            {
+                for(int i=0;i<6;i++)              //对于一个确定的信号线逐一设置每个杆(i)的数值
+                {
+                    if(0<step[i])     //正转
+                    {
+                        xian[j][i]=zpai[k][j];
+                    }
+                    if(0>step[i])     //反转
+                    {
+                        xian[j][i]=fpai[k][j];
+                    }
+                }
+            }
+        }
+        for(int i=0;i<6;i++)           //一步完成之后，减少步数
+        {
+            if(0<step[i])
+                step[i]--;
+            if(0>step[i])
+                step[i]++;
+        }
+    }
+}