2018_Project-R / RoboClaw_mine_

念のためパブリッシュ

Triangle_omni.cpp@0:5c64198c6a96, 2017-10-30 (annotated)

Committer:
kouika748
Date:
Mon Oct 30 09:09:42 2017 +0000
Revision:
0:5c64198c6a96
Child:
1:d36ea95c83f2
?????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kouika748 0:5c64198c6a96 1 /*試験オリジナルライブラリ
kouika748 0:5c64198c6a96 2
kouika748 0:5c64198c6a96 3 double d_r(double degree){
kouika748 0:5c64198c6a96 4 return degree*Pi/180.0;
kouika748 0:5c64198c6a96 5 }
kouika748 0:5c64198c6a96 6
kouika748 0:5c64198c6a96 7 double r_d(double radian){
kouika748 0:5c64198c6a96 8 return radian*180.0/Pi;
kouika748 0:5c64198c6a96 9 }
kouika748 0:5c64198c6a96 10
kouika748 0:5c64198c6a96 11 double odo_way(int mode){
kouika748 0:5c64198c6a96 12 if(mode==1) return odo1.getPulses()*((omni1*Pi)/(resolution*en_bai));
kouika748 0:5c64198c6a96 13 else if(mode==2) return odo2.getPulses()*((omni2*Pi)/(resolution*en_bai));
kouika748 0:5c64198c6a96 14 else if(mode==3) return odo3.getPulses()*((omni3*Pi)/(resolution*en_bai));
kouika748 0:5c64198c6a96 15 else return 0;
kouika748 0:5c64198c6a96 16 }
kouika748 0:5c64198c6a96 17
kouika748 0:5c64198c6a96 18 void odometry(){
kouika748 0:5c64198c6a96 19 double radian=0.0;
kouika748 0:5c64198c6a96 20 radian=((odo_way(1)+odo_way(2)+odo_way(3))/(3*gaisetuen));
kouika748 0:5c64198c6a96 21 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)));
kouika748 0:5c64198c6a96 22 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)));
kouika748 0:5c64198c6a96 23 angle=r_d(radian);
kouika748 0:5c64198c6a96 24 }
kouika748 0:5c64198c6a96 25 */