2022_gorobo_Ateam / gourobo2022_undercarriage_new

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

Dependencies:   SBDBT arrc_mbed BNO055

library/odometry.hpp

Committer:
guesta
Date:
2022-01-24
Revision:
6:e089fda81b74
Parent:
4:867c2b5cf81a
Child:
10:ad8fced7d6b6

File content as of revision 6:e089fda81b74:

#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_12,PC_5 ,1,256,4),//左
    RotaryInc(PA_8 ,PA_9 ,1,256,4),//後
    RotaryInc(PA_6 ,PA_7,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