yuki honma / UnderBody

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

Dependents:   TOUTEKI_all_mbed mbed_test_program

Mecanum.cpp@0:62707e16531a, 2018-09-01 (annotated)

Committer:
yuki17100
Date:
Sat Sep 01 09:07:35 2018 +0000
Revision:
0:62707e16531a
Child:
1:698586aa0c5a
???????????????MD??????????

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 0:62707e16531a 7 void Mecanum::move(double direction, double speed, double revolution){
yuki17100 0:62707e16531a 8 double duty[4];
yuki17100 0:62707e16531a 9
yuki17100 0:62707e16531a 10 duty[0] = (sin(direction) - cos(direction))*speed - revolution;//右前
yuki17100 0:62707e16531a 11 duty[1] = (sin(direction) + cos(direction))*speed + revolution;//左前
yuki17100 0:62707e16531a 12 duty[2] = (sin(direction) - cos(direction))*speed + revolution;//右後
yuki17100 0:62707e16531a 13 duty[3] = (sin(direction) + cos(direction))*speed - revolution;//左後
yuki17100 0:62707e16531a 14
yuki17100 0:62707e16531a 15 MD.SpeedM1(ADR_MD1, (int)(duty[0]*CMD));//右前
yuki17100 0:62707e16531a 16 MD.SpeedM2(ADR_MD1, (int)(duty[2]*CMD));//右後
yuki17100 0:62707e16531a 17 MD.SpeedM1(ADR_MD2, (int)(duty[3]*CMD));//左後
yuki17100 0:62707e16531a 18 MD.SpeedM2(ADR_MD2, (int)(duty[1]*CMD));//左前
yuki17100 0:62707e16531a 19
yuki17100 0:62707e16531a 20 }