2022_gorobo_Ateam
2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
Diff: library/odometry.hpp
- Revision:
- 4:867c2b5cf81a
- Child:
- 6:e089fda81b74
diff -r 6fb5c3218608 -r 867c2b5cf81a library/odometry.hpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/library/odometry.hpp Sat Jan 22 16:00:04 2022 +0000
@@ -0,0 +1,37 @@
+#ifndef odometry_hpp
+#define odometry_hpp
+#include "mbed.h"
+#include "rotary_inc.hpp"
+
+double x_position;
+double y_position;
+double odometer_ensyu;
+double read_interval = 0.05;
+
+RotaryInc odometer[4] = {
+ RotaryInc(PC_2 ,PC_3 ,1,256,4),
+ RotaryInc(PA_12,PC_5 ,1,256,4),
+ RotaryInc(PC_0 ,PC_1 ,1,256,4),
+ RotaryInc(PC_4 ,PA_13,1,256,4)
+};
+
+void get_position(double pltheta){
+ double Vx[2] = {0,0};
+ double Vy[2] = {0,0};
+ double Vod[4];
+
+ for(int i = 0;i < 4;i++){
+ Vod[i] = (odometer[i].getSpeed()) * odometer_ensyu / 10;
+ }
+
+ Vx[1] = ((Vod[0] * cos(pltheta)) + (Vod[1] * cos(pltheta + PI / 2)) + (Vod[2] * cos(pltheta + PI)) + (Vod[3] * cos(pltheta + 3 * (PI / 2)))) / 2;
+ Vy[1] = ((Vod[3] * cos(pltheta)) + (Vod[0] * cos(pltheta + PI / 2)) + (Vod[1] * cos(pltheta + PI)) + (Vod[2] * cos(pltheta + 3 * (PI / 2)))) / 2;
+
+ x_position += ((Vx[1] + Vx[0]) / 2) * read_interval;
+ y_position -= ((Vy[1] + Vy[0]) / 2) * read_interval;
+
+ Vx[0] = Vx[1];
+ Vy[0] = Vy[1];
+}
+
+#endif
\ No newline at end of file