File content as of revision 0:772b2085fe23:

#include "mbed.h"
#include "Motor.h"

Motor::Motor(PinName p01,PinName p10,PinName p02,PinName p20,PinName p03,PinName p30,PinName p04,PinName p40):
_p01(p01),_p10(p10),_p02(p02),_p20(p20),_p03(p03),_p30(p30),_p04(p04),_p40(p40)
{
    _p01 = 0;
    _p10 = 0;
    _p02 = 0;
    _p20 = 0;
    _p03 = 0;
    _p30 = 0;
    _p04 = 0;
    _p40 = 0;
}

/*モーター周波数変更初期設定*/
void Motor::setPwmPeriod(float hz){
    _p01.period(hz);
    _p10.period(hz);
    _p02.period(hz);
    _p20.period(hz);
    _p03.period(hz);
    _p30.period(hz);
    _p04.period(hz);
    _p40.period(hz);
}

/*モーター制御*/
void Motor::setPower(float a,float b,float c,float d){
    
    a = a / 100;
    b = b / 100;
    c = c / 100;
    d = d / 100;
    
    if(a > 0) { //正回転
        _p01 = a;
        _p10 = 0;
    } else if(a == 0) { //ブレーキ
        _p01 = 1;
        _p10 = 1;//0か1だと思う
    } else { //負回転
        _p01 = 0;
        _p10 = -1*a;
    }
    if(b > 0) {
        _p02 = b;
        _p20 = 0;
    } else if(b == 0) {
        _p02 = 1;
        _p20 = 1;
    } else {
        _p02 = 0;
        _p20 = -1*b;
    }
    if(c > 0) {
        _p03 = c;
        _p30 = 0;
    } else if(c == 0) {
        _p03 = 1;
        _p30 = 1;
    } else {
        _p03 = 0;
        _p30 = -1*c;

    }
    if(d > 0) {
        _p04 = d;
        _p40 = 0;
    } else if(d == 0) {
        _p04 = 1;
        _p40 = 1;
    } else {
        _p04 = 0;
        _p40 = -1*d;
    }
}

//*********************************************************************************/////////
///////モーター制御θ(degree)に進行方向,powerにPWMの値(0～100),修正値をmodに代入,右回転基準//////////
//********************************************************************************://///////
/*4輪用*/
void Motor::omni4Wheels(int degree,int power,int mod){
    float motor[4];
    float Max[3];
    if(power == 0) { //パワー0のとき

        motor[0] = -mod; //モーター右前
        motor[1] = -mod; //モーター右後
        motor[2] = -mod; //モーター左後
        motor[3] = -mod; //モーター左前
    } else {

        motor[0] = sin((degree-45)*Pi/180) - mod; //モーター右前
        motor[1] = sin((degree-135)*Pi/180) - mod; //モーター右後
        motor[2] = sin((degree-225)*Pi/180) - mod; //モーター左後
        motor[3] = sin((degree-315)*Pi/180) - mod; //モーター左前

        if(motor[0]>1) {
            motor[0] = 1;
        } else if(motor[0] < -1) {
            motor[0] = -1;
        }
        if(motor[1]>1) {
            motor[1] = 1;
        } else if(motor[1] < -1) {
            motor[1] = -1;
        }
        if(motor[2]>1) {
            motor[2] = 1;
        } else if(motor[2] < -1) {
            motor[2] = -1;
        }
        if(motor[3]>1) {
            motor[3] = 1;
        } else if(motor[3] < -1) {
            motor[3] = -1;
        }


        if(fabs(motor[0]) >= fabs(motor[1])) { //モーターパワー最大値計算
            Max[0] = fabs(motor[0]);
        } else {
            Max[0] = fabs(motor[1]);
        }
        if(fabs(motor[2]) >= fabs(motor[3])) {
            Max[1] = fabs(motor[2]);
        } else {
            Max[1] = fabs(motor[3]);
        }
        if(Max[0] >= Max[1]) {
            Max[2] = Max[0];
        } else {
            Max[2] = Max[1];
        }

        motor[0] = (power*(motor[0]/Max[2]));//モータパワー最大値修正
        motor[1] = (power*(motor[1]/Max[2]));
        motor[2] = (power*(motor[2]/Max[2]));
        motor[3] = (power*(motor[3]/Max[2]));
    }
    
    setPower(motor[0],motor[1],motor[2],motor[3]);
}


/*3輪用*/
void Motor::omni3Wheels(int degree,int power,int mod){
    float motor[3];//モーター用変数
    float Max[2];//最大値用変数
    
    if(power == 0){//パワー0のとき
        
        motor[0] = -mod; //モーター右
        motor[1] = -mod; //モーター後
        motor[2] = -mod; //モーター左
    }
    else{ 
    
        motor[0] = sin((degree-60)*Pi/180) - mod; //モーター右
        motor[1] = sin((degree-180)*Pi/180) - mod; //モーター後
        motor[2] = sin((degree-300)*Pi/180) - mod; //モーター左
        
        if(motor[0]>1){
            motor[0] = 1;
        }
        else if(motor[0] < -1){
            motor[0] = -1;
        }
        if(motor[1]>1){
            motor[1] = 1;
        }
        else if(motor[1] < -1){
            motor[1] = -1;
        }
        if(motor[2]>1){
            motor[2] = 1;
        }
        else if(motor[2] < -1){
            motor[2] = -1;
        }
        
    
        if(fabs(motor[0]) >= fabs(motor[1])){ //モーターパワー最大値計算
            Max[0] = fabs(motor[0]);
        }
        else{
            Max[0] = fabs(motor[1]);
        }
        if(fabs(motor[2]) >= Max[0]){
            Max[1] = fabs(motor[2]);
        }
        else{
            Max[1] = Max[0];//最大値
        }
        
    
        motor[0] = (power*(motor[0]/Max[1]));//モータパワー最大値修正
        motor[1] = (power*(motor[1]/Max[1]));
        motor[2] = (power*(motor[2]/Max[1]));
    }
    
    setPower(motor[0],motor[1],motor[2],0);
    
}