22

Dependencies:   mbed

Fork of StewartPlatform by Guojun Jiang

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers motor.cpp Source File

motor.cpp

00001 #include "motor.h"
00002 
00003 //PwmOut motor[6] = {PwmOut(D8), PwmOut(D9), PwmOut(D10), PwmOut(D11), PwmOut(D12), PwmOut(D13)};
00004 extern char position[6];
00005 
00006 /*
00007 void motor_drive() {
00008     for(int index = 0; index < 6; index++) {
00009         float duty = (0.000417 * position[index] + 0.05);
00010         motor[index] = duty;            
00011     }
00012 }
00013 */
00014 
00015 /*
00016 void motor_init() {
00017         for (int index = 0; index < 6; index++) {
00018                 motor[index].period(0.020);                                 //cycle period is 20ms
00019                 motor[index] = 0.075;                                               //control pulse width is 1ms~2ms? should be confirmed!!
00020         }
00021 //      motor[0].period(0.020);
00022 //      motor[0] = 0.05;
00023 }
00024 */
00025 
00026 
00027 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}};    
00028 const int zpai[4][4]={{1,1,0,0},{0,1,1,0},{0,0,1,1},{1,0,0,1}};           //正转,前面是拍(k),后面是线(j)
00029 const int fpai[4][4]={{1,1,0,0},{1,0,0,1},{0,0,1,1},{0,1,1,0}};           //反转
00030 const float steplong=1.8;                  //步长待补充
00031 int step[6];
00032 
00033 void getstep()
00034 {
00035     for(int i=0;i<6;i++)
00036         step[i]=position[i]/steplong;         //待补充计算方法,缺少步长steplong
00037 }
00038 
00039 int max(int a[],int n)                      //计算最大值
00040 {
00041     int m=a[0];
00042     for(int t=1;t<n;t++)
00043         m=(m>a[t]?m:a[t]);
00044     return m;
00045 }
00046 
00047 void motor_drive()                         //平台完成一次移动
00048 {
00049     int m;
00050     m=max(step,6);
00051     for(int n=0;n<m;n++)                    //最大步数
00052     {
00053         for(int k=0;k<4;k++)                    //确定拍数(k)
00054         {
00055             for(int j=0;j<4;j++)                  //确定信号线(j)
00056             {
00057                 for(int i=0;i<6;i++)              //对于一个确定的信号线逐一设置每个杆(i)的数值
00058                 {
00059                     if(0<step[i])     //正转
00060                     {
00061                         xian[j][i]=zpai[k][j];
00062                     }
00063                     if(0>step[i])     //反转
00064                     {
00065                         xian[j][i]=fpai[k][j];
00066                     }
00067                 }
00068             }
00069         }
00070         for(int i=0;i<6;i++)           //一步完成之后,减少步数
00071         {
00072             if(0<step[i])
00073                 step[i]--;
00074             if(0>step[i])
00075                 step[i]++;
00076         }
00077     }
00078 }