yuki honma / UnderBody

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

Dependents:   TOUTEKI_all_mbed mbed_test_program

Mecanum.cpp@3:9c3f2662974e, 2018-09-04 (annotated)

Committer:
yuki17100
Date:
Tue Sep 04 14:22:13 2018 +0000
Revision:
3:9c3f2662974e
Parent:
2:f1085de93385 
Mecanum????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki17100 0:62707e16531a 1 #include "Mecanum.h"
yuki17100 0:62707e16531a 2
yuki17100 0:62707e16531a 3 Mecanum::Mecanum() : MD(115200, PIN_MD, NC){
yuki17100 0:62707e16531a 4 }
yuki17100 0:62707e16531a 5
yuki17100 0:62707e16531a 6
yuki17100 3:9c3f2662974e 7 void Mecanum::command(double (&duty)[4]){
yuki17100 3:9c3f2662974e 8 MD.SpeedM1(ADR_MD1, (int)(duty[0]*CMD));
yuki17100 3:9c3f2662974e 9 MD.SpeedM2(ADR_MD1, (int)(duty[2]*CMD));
yuki17100 3:9c3f2662974e 10 MD.SpeedM1(ADR_MD2, (int)(duty[3]*CMD));
yuki17100 3:9c3f2662974e 11 MD.SpeedM2(ADR_MD2, (int)(duty[1]*CMD));
yuki17100 3:9c3f2662974e 12 }
yuki17100 3:9c3f2662974e 13
yuki17100 0:62707e16531a 14 void Mecanum::move(double direction, double speed, double revolution){
yuki17100 0:62707e16531a 15 double duty[4];
yuki17100 0:62707e16531a 16
yuki17100 3:9c3f2662974e 17 duty[0] = (cos(direction) - sin(direction))*speed - revolution;//右前
yuki17100 3:9c3f2662974e 18 duty[1] = (cos(direction) + sin(direction))*speed + revolution;//左前
yuki17100 3:9c3f2662974e 19 duty[2] = (cos(direction) + sin(direction))*speed - revolution;//右後
yuki17100 3:9c3f2662974e 20 duty[3] = (cos(direction) - sin(direction))*speed + revolution;//左後
yuki17100 3:9c3f2662974e 21
yuki17100 3:9c3f2662974e 22 command(duty);
yuki17100 0:62707e16531a 23
yuki17100 3:9c3f2662974e 24 }
yuki17100 3:9c3f2662974e 25
yuki17100 3:9c3f2662974e 26 void Mecanum::rectang(double x,double y,double rotation){
yuki17100 3:9c3f2662974e 27 double duty[4];
yuki17100 0:62707e16531a 28
yuki17100 3:9c3f2662974e 29 duty[0] = y - x - rotation;
yuki17100 3:9c3f2662974e 30 duty[1] = y + x + rotation;
yuki17100 3:9c3f2662974e 31 duty[2] = y + x - rotation;
yuki17100 3:9c3f2662974e 32 duty[3] = y - x + rotation;
yuki17100 3:9c3f2662974e 33
yuki17100 3:9c3f2662974e 34 command(duty);
yuki17100 0:62707e16531a 35 }