Ryu Kaz
/
767zi
t6est
foot.cpp
- Committer:
- kazuryu
- Date:
- 2019-09-24
- Revision:
- 0:3dc012104243
- Child:
- 4:9ba47e5db1e2
File content as of revision 0:3dc012104243:
#include "mbed.h" #include "foot.h" /* PwmOut RF(D3); PwmOut LF(D5); PwmOut RB(D6); PwmOut LB(D9); PwmOut pwms[4] = {RF,LF,RB,LB}; */ // DigitalOut dig(PG_9); PwmOut RFA(PB_4);//3/1 PwmOut RFB(PB_5);//3/2 PwmOut LFA(PC_8);//3/3 PwmOut LFB(PC_9);//3/4 PwmOut RBA(PD_12);//4/1 PwmOut RBB(PD_13);//4/2 PwmOut LBA(PD_14);//4/3 PwmOut LBB(PD_15);//4/4 PwmOut pwms[8] = {RFA,RFB,LFA,LFB,RBA,RBB,LBA,LBB}; PwmOut lockR(PB_10); PwmOut lockL(PB_11); PwmOut lockpwms[2] = {lockR,lockL}; //new //PB_6 -> PD_12 //PC_7 -> PB_5 /* // PB_4 -> PA_6///// // PB_6 -> PD_12 */ /* DigitalOut dig(PG_9); PwmOut RFA(PB_4);//3/1 PwmOut RFB(PC_7);//3/2 PwmOut LFA(PC_8);//3/3 PwmOut LFB(PC_9);//3/4 PwmOut RBA(PB_6);//4/1 PwmOut RBB(PD_13);//4/2 PwmOut LBA(PD_14);//4/3 PwmOut LBB(PD_15);//4/4 */ int Write[][4] { {255, 255, 255, 255}, //0前 { -255, -255, -255, -255}, //1後ろ { -255, 255, 255, -255}, //2右 {255, -255, -255, 255}, //3左 {0, 255, 255, 0}, //4右前 {255, 0, 0, 255}, //5左前 { -255, 0, 0, -255}, //6右後ろ {0, -255, -255, 0}, //7左後ろ { -255, 255, -255, 255}, //8右旋回 {255, -255, 255, -255},//9左旋回 {500,500,500,500} }; float pwm_g[] = {1.1052f,1,1.0421f,1.0526f}; void Move(int way,float pwm){ int num; for(int i =0;i<4;i++){ num = Write[way][i]; if(num > 0)dig = 0; else if(num < 0)dig = 1; if(num != 0)pwms[i] = pwm; else pwms[i] = 0; } } int count_Move = 0; int count_Move_lock = 0; void Move2(int way,float pwm){ int num; if(count_Move == 0){ for(int i =0;i<8;i++){ pwms[i].period_ms(1); } count_Move = 1; } for(int i =0;i<4;i++){ num = Write[way][i]; if(num == 255){ pwms[i*2] = pwm*pwm_g[i]; pwms[i*2+1] = 0; }else if(num == -255){ pwms[i*2] = 0; pwms[i*2+1] = pwm*pwm_g[i]; }else if(num == 500){ pwms[i*2] = 1; pwms[i*2+1] = 1; } } } void Move_lock(int way,float pwm){ int num; if(count_Move_lock == 0){ for(int i =0;i<2;i++){ lockpwms[i].period_ms(1); } count_Move_lock = 1; } for(int i =0;i<1;i++){ num = Write[way][i]; if(num == 255){ pwms[i*2] = pwm; pwms[i*2+1] = 0; }else if(num == -255){ pwms[i*2] = 0; pwms[i*2+1] = pwm; } } } void trans(int a,int dis,float *x,float *y,int *sign){ if(a == 0){ *y=dis; *x = 0; }else if(a == 1){ *y = -dis; *x = 0; }else if(a == 2){ *y = 0; *x = dis; }else if(a == 3){ *y = 0; *x = -dis; }else if(a == 4){ *y = dis/1.414; *x = dis/1.414; }else if(a == 5){ *y = dis/1.414; *x = -dis/1.414; }else if(a == 6){ *y = -dis/1.414; *x = -dis/1.414; }else if(a == 7){ *y = -dis/1.414; *x = dis/1.414; } *sign = 0; if(*x < 0)*sign = 0b10; if(*y < 0)*sign = *sign | 0b01; }