2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

library/odometry.hpp@4:867c2b5cf81a, 2022-01-22 (annotated)

Committer:
guesta
Date:
Sat Jan 22 16:00:04 2022 +0000
Revision:
4:867c2b5cf81a
Child:
6:e089fda81b74
add odometry_program;

Who changed what in which revision?

UserRevisionLine numberNew 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 4:867c2b5cf81a 8 double odometer_ensyu;
guesta 4:867c2b5cf81a 9 double read_interval = 0.05;
guesta 4:867c2b5cf81a 10
guesta 4:867c2b5cf81a 11 RotaryInc odometer[4] = {
guesta 4:867c2b5cf81a 12 RotaryInc(PC_2 ,PC_3 ,1,256,4),
guesta 4:867c2b5cf81a 13 RotaryInc(PA_12,PC_5 ,1,256,4),
guesta 4:867c2b5cf81a 14 RotaryInc(PC_0 ,PC_1 ,1,256,4),
guesta 4:867c2b5cf81a 15 RotaryInc(PC_4 ,PA_13,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