2022_gorobo_Ateam / gourobo2022_undercarriage_new

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

Dependencies:   SBDBT arrc_mbed BNO055

library/odometry.hpp@13:369f4abc1f36, 2022-04-08 (annotated)

Committer:
guesta
Date:
Fri Apr 08 09:14:20 2022 +0000
Revision:
13:369f4abc1f36
Parent:
10:ad8fced7d6b6 
2022/04/08

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 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