first
Dependents: 17robo_fuzi 17robo_tokyo_kaede
Diff: mecanum.cpp
- Revision:
- 0:f3c5ba43a1aa
diff -r 000000000000 -r f3c5ba43a1aa mecanum.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mecanum.cpp Sun Sep 24 05:24:12 2017 +0000 @@ -0,0 +1,78 @@ +#include "mecanum.h" +#include "math.h" + +void Mecanum::setupdeg(float Deg){ + first_deg = Deg; +} +void Mecanum::sbdbt_cal(float Vx, float Vy, int r1, int l1,float w, float angle){ + rad = (angle - first_deg)/180*PI; + + vx = Vx / 2 * cos(rad) - Vy / 2 * sin(rad); + vy = Vx / 2 * sin(rad) + Vy / 2 * cos(rad); + + V1 = vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w; + V2 = -vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w; + V3 = -vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w; + V4 = vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w; +} +void Mecanum::boost(){ + if(vy > 0.4) { + V1 += boost_A; + V2 += -boost_A; + V3 += boost_A; + V4 += -boost_A; + } else if(vy < -0.4) { + V1 += -boost_A; + V2 += boost_A; + V3 += -boost_A; + V4 += boost_A; + } +} +void Mecanum::boost_forward(){ + V1 = -boost_K; + V2 = boost_K; + V3 = -boost_K; + V4 = boost_K; +} +void Mecanum::boost_back(){ + V1 = boost_K; + V2 = -boost_K; + V3 = boost_K; + V4 = -boost_K; +} +void Mecanum::boost_right(){ + V1 = boost_K; + V2 = -boost_K; + V3 = -boost_K; + V4 = boost_K; +} +void Mecanum::boost_left(){ + V1 = -boost_K; + V2 = boost_K; + V3 = boost_K; + V4 = -boost_K; +} +void Mecanum::xy_cal(float Vx, float Vy){ + V1 = Vx / 2 + Vy / 2; + V2 = Vx / 2 - Vy / 2; + V3 = -Vx / 2 + Vy / 2; + V4 = -Vx / 2 - Vy / 2; +} +float Mecanum::v1(){ + return V1; +} +float Mecanum::v2(){ + return V2; +} +float Mecanum::v3(){ + return V3; +} +float Mecanum::v4(){ + return V4; +} +float Mecanum::VX(){ + return vx; +} +float Mecanum::VY(){ + return vy; +} \ No newline at end of file