2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
library/odometry.hpp
- Committer:
- guesta
- Date:
- 2022-03-11
- Revision:
- 10:ad8fced7d6b6
- Parent:
- 6:e089fda81b74
File content as of revision 10:ad8fced7d6b6:
#ifndef odometry_hpp #define odometry_hpp #include "mbed.h" #include "rotary_inc.hpp" double x_position; double y_position; double odometer_ensyu = 50; double read_interval = 0.05; RotaryInc odometer[4] = { RotaryInc(PC_10 ,PC_11,1,256,4),//前 RotaryInc(PA_7,PA_6,1,256,4),//左 RotaryInc(PA_8 ,PA_9,1,256,4),//後 RotaryInc(PA_13,PC_4,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