jy z / Mbed 2 deprecated StewartPlatform

22

Dependencies:   mbed

Fork of StewartPlatform by Guojun Jiang

src/motor.cpp@3:0cba1132c8a3, 2016-05-31 (annotated)

Committer:
zjyporridge
Date:
Tue May 31 00:26:35 2016 +0000
Revision:
3:0cba1132c8a3
Parent:
2:50062ac8646d 
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wheels 2:50062ac8646d 1 #include "motor.h"
wheels 2:50062ac8646d 2
zjyporridge 3:0cba1132c8a3 3 //PwmOut motor[6] = {PwmOut(D8), PwmOut(D9), PwmOut(D10), PwmOut(D11), PwmOut(D12), PwmOut(D13)};
wheels 2:50062ac8646d 4 extern char position[6];
wheels 2:50062ac8646d 5
zjyporridge 3:0cba1132c8a3 6 /*
wheels 2:50062ac8646d 7 void motor_drive() {
wheels 2:50062ac8646d 8 for(int index = 0; index < 6; index++) {
wheels 2:50062ac8646d 9 float duty = (0.000417 * position[index] + 0.05);
wheels 2:50062ac8646d 10 motor[index] = duty;
wheels 2:50062ac8646d 11 }
wheels 2:50062ac8646d 12 }
zjyporridge 3:0cba1132c8a3 13 */
wheels 2:50062ac8646d 14
zjyporridge 3:0cba1132c8a3 15 /*
wheels 2:50062ac8646d 16 void motor_init() {
wheels 2:50062ac8646d 17 for (int index = 0; index < 6; index++) {
wheels 2:50062ac8646d 18 motor[index].period(0.020); //cycle period is 20ms
wheels 2:50062ac8646d 19 motor[index] = 0.075; //control pulse width is 1ms~2ms? should be confirmed!!
wheels 2:50062ac8646d 20 }
wheels 2:50062ac8646d 21 // motor[0].period(0.020);
wheels 2:50062ac8646d 22 // motor[0] = 0.05;
wheels 2:50062ac8646d 23 }
zjyporridge 3:0cba1132c8a3 24 */
zjyporridge 3:0cba1132c8a3 25
zjyporridge 3:0cba1132c8a3 26
zjyporridge 3:0cba1132c8a3 27 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}};
zjyporridge 3:0cba1132c8a3 28 const int zpai[4][4]={{1,1,0,0},{0,1,1,0},{0,0,1,1},{1,0,0,1}}; //正转，前面是拍(k)，后面是线(j)
zjyporridge 3:0cba1132c8a3 29 const int fpai[4][4]={{1,1,0,0},{1,0,0,1},{0,0,1,1},{0,1,1,0}}; //反转
zjyporridge 3:0cba1132c8a3 30 const float steplong=1.8; //步长待补充
zjyporridge 3:0cba1132c8a3 31 int step[6];
zjyporridge 3:0cba1132c8a3 32
zjyporridge 3:0cba1132c8a3 33 void getstep()
zjyporridge 3:0cba1132c8a3 34 {
zjyporridge 3:0cba1132c8a3 35 for(int i=0;i<6;i++)
zjyporridge 3:0cba1132c8a3 36 step[i]=position[i]/steplong; //待补充计算方法，缺少步长steplong
zjyporridge 3:0cba1132c8a3 37 }
zjyporridge 3:0cba1132c8a3 38
zjyporridge 3:0cba1132c8a3 39 int max(int a[],int n) //计算最大值
zjyporridge 3:0cba1132c8a3 40 {
zjyporridge 3:0cba1132c8a3 41 int m=a[0];
zjyporridge 3:0cba1132c8a3 42 for(int t=1;t<n;t++)
zjyporridge 3:0cba1132c8a3 43 m=(m>a[t]?m:a[t]);
zjyporridge 3:0cba1132c8a3 44 return m;
zjyporridge 3:0cba1132c8a3 45 }
zjyporridge 3:0cba1132c8a3 46
zjyporridge 3:0cba1132c8a3 47 void motor_drive() //平台完成一次移动
zjyporridge 3:0cba1132c8a3 48 {
zjyporridge 3:0cba1132c8a3 49 int m;
zjyporridge 3:0cba1132c8a3 50 m=max(step,6);
zjyporridge 3:0cba1132c8a3 51 for(int n=0;n<m;n++) //最大步数
zjyporridge 3:0cba1132c8a3 52 {
zjyporridge 3:0cba1132c8a3 53 for(int k=0;k<4;k++) //确定拍数（k）
zjyporridge 3:0cba1132c8a3 54 {
zjyporridge 3:0cba1132c8a3 55 for(int j=0;j<4;j++) //确定信号线(j)
zjyporridge 3:0cba1132c8a3 56 {
zjyporridge 3:0cba1132c8a3 57 for(int i=0;i<6;i++) //对于一个确定的信号线逐一设置每个杆(i)的数值
zjyporridge 3:0cba1132c8a3 58 {
zjyporridge 3:0cba1132c8a3 59 if(0<step[i]) //正转
zjyporridge 3:0cba1132c8a3 60 {
zjyporridge 3:0cba1132c8a3 61 xian[j][i]=zpai[k][j];
zjyporridge 3:0cba1132c8a3 62 }
zjyporridge 3:0cba1132c8a3 63 if(0>step[i]) //反转
zjyporridge 3:0cba1132c8a3 64 {
zjyporridge 3:0cba1132c8a3 65 xian[j][i]=fpai[k][j];
zjyporridge 3:0cba1132c8a3 66 }
zjyporridge 3:0cba1132c8a3 67 }
zjyporridge 3:0cba1132c8a3 68 }
zjyporridge 3:0cba1132c8a3 69 }
zjyporridge 3:0cba1132c8a3 70 for(int i=0;i<6;i++) //一步完成之后，减少步数
zjyporridge 3:0cba1132c8a3 71 {
zjyporridge 3:0cba1132c8a3 72 if(0<step[i])
zjyporridge 3:0cba1132c8a3 73 step[i]--;
zjyporridge 3:0cba1132c8a3 74 if(0>step[i])
zjyporridge 3:0cba1132c8a3 75 step[i]++;
zjyporridge 3:0cba1132c8a3 76 }
zjyporridge 3:0cba1132c8a3 77 }
zjyporridge 3:0cba1132c8a3 78 }