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