Diff: Triangle_omni.cpp

Revision:

0:5c64198c6a96

Child:

1:d36ea95c83f2

--- /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