yuki honma / UnderBody

下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用

Dependents:   TOUTEKI_all_mbed mbed_test_program

Mecanum.cpp

Committer:
yuki17100
Date:
2018-09-04
Revision:
3:9c3f2662974e
Parent:
2:f1085de93385

File content as of revision 3:9c3f2662974e:

#include "Mecanum.h"

Mecanum::Mecanum() : MD(115200, PIN_MD, NC){
}


void Mecanum::command(double (&duty)[4]){
    MD.SpeedM1(ADR_MD1, (int)(duty[0]*CMD));
    MD.SpeedM2(ADR_MD1, (int)(duty[2]*CMD));
    MD.SpeedM1(ADR_MD2, (int)(duty[3]*CMD));
    MD.SpeedM2(ADR_MD2, (int)(duty[1]*CMD));
}

void Mecanum::move(double direction, double speed, double revolution){
    double duty[4];
    
    duty[0] = (cos(direction) - sin(direction))*speed - revolution;//右前
    duty[1] = (cos(direction) + sin(direction))*speed + revolution;//左前
    duty[2] = (cos(direction) + sin(direction))*speed - revolution;//右後
    duty[3] = (cos(direction) - sin(direction))*speed + revolution;//左後
    
    command(duty);
    
}

void Mecanum::rectang(double x,double y,double rotation){
    double duty[4];
    
    duty[0] = y - x - rotation;
    duty[1] = y + x + rotation;
    duty[2] = y + x - rotation;
    duty[3] = y - x + rotation;
    
    command(duty);
}