File content as of revision 4:9ba47e5db1e2:

#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);

//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}
};
//1.1052f,1,1.0421f,1.0726f
float pwm_g[] = {1,0.904f,0.982f,0.970f};
    //1,0.904,0.942,0.970
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] = 0;
            pwms[i*2+1] = 0;
            }
        }
    }
/*
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;
    }