2018_Project-R / RoboClaw_mine_

念のためパブリッシュ

Triangle_omni.cpp

Committer:
kouika748
Date:
2017-10-30
Revision:
0:5c64198c6a96
Child:
1:d36ea95c83f2

File content as of revision 0:5c64198c6a96:

/*試験オリジナルライブラリ

double d_r(double degree){
    return degree*Pi/180.0;
    }

double r_d(double radian){
    return radian*180.0/Pi;
    }

double odo_way(int mode){
    if(mode==1) return odo1.getPulses()*((omni1*Pi)/(resolution*en_bai));
    else if(mode==2) return odo2.getPulses()*((omni2*Pi)/(resolution*en_bai));
    else if(mode==3) return odo3.getPulses()*((omni3*Pi)/(resolution*en_bai));
    else return 0;
    }

void odometry(){
    double radian=0.0;
    radian=((odo_way(1)+odo_way(2)+odo_way(3))/(3*gaisetuen));
    way_x=(odo_way(1)*sin(radian-d_r(90.0)))+((-1)*odo_way(2)*sin(radian-d_r(120.0-90.0)))+(odo_way(3)*sin(radian-d_r(240.0+90.0)));
    way_y=(odo_way(1)*sin(radian))+(odo_way(2)*sin(radian-d_r(120.0)))+(odo_way(3)*sin(radian-d_r(240.0)));
    angle=r_d(radian);
    }
    */