22
Fork of StewartPlatform by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Mon Jul 25 2022 15:47:41 by
1.7.2
