2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
library/odometry.hpp@10:ad8fced7d6b6, 2022-03-11 (annotated)
- Committer:
- guesta
- Date:
- Fri Mar 11 04:24:27 2022 +0000
- Revision:
- 10:ad8fced7d6b6
- Parent:
- 6:e089fda81b74
NewVer;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
guesta | 4:867c2b5cf81a | 1 | #ifndef odometry_hpp |
guesta | 4:867c2b5cf81a | 2 | #define odometry_hpp |
guesta | 4:867c2b5cf81a | 3 | #include "mbed.h" |
guesta | 4:867c2b5cf81a | 4 | #include "rotary_inc.hpp" |
guesta | 4:867c2b5cf81a | 5 | |
guesta | 4:867c2b5cf81a | 6 | double x_position; |
guesta | 4:867c2b5cf81a | 7 | double y_position; |
guesta | 6:e089fda81b74 | 8 | double odometer_ensyu = 50; |
guesta | 4:867c2b5cf81a | 9 | double read_interval = 0.05; |
guesta | 4:867c2b5cf81a | 10 | |
guesta | 4:867c2b5cf81a | 11 | RotaryInc odometer[4] = { |
guesta | 10:ad8fced7d6b6 | 12 | RotaryInc(PC_10 ,PC_11,1,256,4),//前 |
guesta | 10:ad8fced7d6b6 | 13 | RotaryInc(PA_7,PA_6,1,256,4),//左 |
guesta | 10:ad8fced7d6b6 | 14 | RotaryInc(PA_8 ,PA_9,1,256,4),//後 |
guesta | 10:ad8fced7d6b6 | 15 | RotaryInc(PA_13,PC_4,1,256,4)//右 |
guesta | 4:867c2b5cf81a | 16 | }; |
guesta | 4:867c2b5cf81a | 17 | |
guesta | 4:867c2b5cf81a | 18 | void get_position(double pltheta){ |
guesta | 4:867c2b5cf81a | 19 | double Vx[2] = {0,0}; |
guesta | 4:867c2b5cf81a | 20 | double Vy[2] = {0,0}; |
guesta | 4:867c2b5cf81a | 21 | double Vod[4]; |
guesta | 4:867c2b5cf81a | 22 | |
guesta | 4:867c2b5cf81a | 23 | for(int i = 0;i < 4;i++){ |
guesta | 4:867c2b5cf81a | 24 | Vod[i] = (odometer[i].getSpeed()) * odometer_ensyu / 10; |
guesta | 4:867c2b5cf81a | 25 | } |
guesta | 4:867c2b5cf81a | 26 | |
guesta | 4:867c2b5cf81a | 27 | 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; |
guesta | 4:867c2b5cf81a | 28 | 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; |
guesta | 4:867c2b5cf81a | 29 | |
guesta | 4:867c2b5cf81a | 30 | x_position += ((Vx[1] + Vx[0]) / 2) * read_interval; |
guesta | 4:867c2b5cf81a | 31 | y_position -= ((Vy[1] + Vy[0]) / 2) * read_interval; |
guesta | 4:867c2b5cf81a | 32 | |
guesta | 4:867c2b5cf81a | 33 | Vx[0] = Vx[1]; |
guesta | 4:867c2b5cf81a | 34 | Vy[0] = Vy[1]; |
guesta | 4:867c2b5cf81a | 35 | } |
guesta | 4:867c2b5cf81a | 36 | |
guesta | 4:867c2b5cf81a | 37 | #endif |