first
Dependents: 17robo_fuzi 17robo_tokyo_kaede
mecanum.cpp@0:f3c5ba43a1aa, 2017-09-24 (annotated)
- Committer:
- echo_piyo
- Date:
- Sun Sep 24 05:24:12 2017 +0000
- Revision:
- 0:f3c5ba43a1aa
????2??
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
echo_piyo | 0:f3c5ba43a1aa | 1 | #include "mecanum.h" |
echo_piyo | 0:f3c5ba43a1aa | 2 | #include "math.h" |
echo_piyo | 0:f3c5ba43a1aa | 3 | |
echo_piyo | 0:f3c5ba43a1aa | 4 | void Mecanum::setupdeg(float Deg){ |
echo_piyo | 0:f3c5ba43a1aa | 5 | first_deg = Deg; |
echo_piyo | 0:f3c5ba43a1aa | 6 | } |
echo_piyo | 0:f3c5ba43a1aa | 7 | void Mecanum::sbdbt_cal(float Vx, float Vy, int r1, int l1,float w, float angle){ |
echo_piyo | 0:f3c5ba43a1aa | 8 | rad = (angle - first_deg)/180*PI; |
echo_piyo | 0:f3c5ba43a1aa | 9 | |
echo_piyo | 0:f3c5ba43a1aa | 10 | vx = Vx / 2 * cos(rad) - Vy / 2 * sin(rad); |
echo_piyo | 0:f3c5ba43a1aa | 11 | vy = Vx / 2 * sin(rad) + Vy / 2 * cos(rad); |
echo_piyo | 0:f3c5ba43a1aa | 12 | |
echo_piyo | 0:f3c5ba43a1aa | 13 | V1 = vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w; |
echo_piyo | 0:f3c5ba43a1aa | 14 | V2 = -vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w; |
echo_piyo | 0:f3c5ba43a1aa | 15 | V3 = -vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w; |
echo_piyo | 0:f3c5ba43a1aa | 16 | V4 = vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w; |
echo_piyo | 0:f3c5ba43a1aa | 17 | } |
echo_piyo | 0:f3c5ba43a1aa | 18 | void Mecanum::boost(){ |
echo_piyo | 0:f3c5ba43a1aa | 19 | if(vy > 0.4) { |
echo_piyo | 0:f3c5ba43a1aa | 20 | V1 += boost_A; |
echo_piyo | 0:f3c5ba43a1aa | 21 | V2 += -boost_A; |
echo_piyo | 0:f3c5ba43a1aa | 22 | V3 += boost_A; |
echo_piyo | 0:f3c5ba43a1aa | 23 | V4 += -boost_A; |
echo_piyo | 0:f3c5ba43a1aa | 24 | } else if(vy < -0.4) { |
echo_piyo | 0:f3c5ba43a1aa | 25 | V1 += -boost_A; |
echo_piyo | 0:f3c5ba43a1aa | 26 | V2 += boost_A; |
echo_piyo | 0:f3c5ba43a1aa | 27 | V3 += -boost_A; |
echo_piyo | 0:f3c5ba43a1aa | 28 | V4 += boost_A; |
echo_piyo | 0:f3c5ba43a1aa | 29 | } |
echo_piyo | 0:f3c5ba43a1aa | 30 | } |
echo_piyo | 0:f3c5ba43a1aa | 31 | void Mecanum::boost_forward(){ |
echo_piyo | 0:f3c5ba43a1aa | 32 | V1 = -boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 33 | V2 = boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 34 | V3 = -boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 35 | V4 = boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 36 | } |
echo_piyo | 0:f3c5ba43a1aa | 37 | void Mecanum::boost_back(){ |
echo_piyo | 0:f3c5ba43a1aa | 38 | V1 = boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 39 | V2 = -boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 40 | V3 = boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 41 | V4 = -boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 42 | } |
echo_piyo | 0:f3c5ba43a1aa | 43 | void Mecanum::boost_right(){ |
echo_piyo | 0:f3c5ba43a1aa | 44 | V1 = boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 45 | V2 = -boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 46 | V3 = -boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 47 | V4 = boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 48 | } |
echo_piyo | 0:f3c5ba43a1aa | 49 | void Mecanum::boost_left(){ |
echo_piyo | 0:f3c5ba43a1aa | 50 | V1 = -boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 51 | V2 = boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 52 | V3 = boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 53 | V4 = -boost_K; |
echo_piyo | 0:f3c5ba43a1aa | 54 | } |
echo_piyo | 0:f3c5ba43a1aa | 55 | void Mecanum::xy_cal(float Vx, float Vy){ |
echo_piyo | 0:f3c5ba43a1aa | 56 | V1 = Vx / 2 + Vy / 2; |
echo_piyo | 0:f3c5ba43a1aa | 57 | V2 = Vx / 2 - Vy / 2; |
echo_piyo | 0:f3c5ba43a1aa | 58 | V3 = -Vx / 2 + Vy / 2; |
echo_piyo | 0:f3c5ba43a1aa | 59 | V4 = -Vx / 2 - Vy / 2; |
echo_piyo | 0:f3c5ba43a1aa | 60 | } |
echo_piyo | 0:f3c5ba43a1aa | 61 | float Mecanum::v1(){ |
echo_piyo | 0:f3c5ba43a1aa | 62 | return V1; |
echo_piyo | 0:f3c5ba43a1aa | 63 | } |
echo_piyo | 0:f3c5ba43a1aa | 64 | float Mecanum::v2(){ |
echo_piyo | 0:f3c5ba43a1aa | 65 | return V2; |
echo_piyo | 0:f3c5ba43a1aa | 66 | } |
echo_piyo | 0:f3c5ba43a1aa | 67 | float Mecanum::v3(){ |
echo_piyo | 0:f3c5ba43a1aa | 68 | return V3; |
echo_piyo | 0:f3c5ba43a1aa | 69 | } |
echo_piyo | 0:f3c5ba43a1aa | 70 | float Mecanum::v4(){ |
echo_piyo | 0:f3c5ba43a1aa | 71 | return V4; |
echo_piyo | 0:f3c5ba43a1aa | 72 | } |
echo_piyo | 0:f3c5ba43a1aa | 73 | float Mecanum::VX(){ |
echo_piyo | 0:f3c5ba43a1aa | 74 | return vx; |
echo_piyo | 0:f3c5ba43a1aa | 75 | } |
echo_piyo | 0:f3c5ba43a1aa | 76 | float Mecanum::VY(){ |
echo_piyo | 0:f3c5ba43a1aa | 77 | return vy; |
echo_piyo | 0:f3c5ba43a1aa | 78 | } |