念のためパブリッシュ
Diff: Triangle_omni.cpp
- Revision:
- 0:5c64198c6a96
- Child:
- 1:d36ea95c83f2
diff -r 000000000000 -r 5c64198c6a96 Triangle_omni.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Triangle_omni.cpp Mon Oct 30 09:09:42 2017 +0000 @@ -0,0 +1,25 @@ +/*試験オリジナルライブラリ + +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); + } + */ \ No newline at end of file