念のためパブリッシュ
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); } */