first
Dependents: 17robo_fuzi 17robo_tokyo_kaede
mecanum.cpp
- Committer:
- echo_piyo
- Date:
- 2017-09-24
- Revision:
- 0:f3c5ba43a1aa
File content as of revision 0:f3c5ba43a1aa:
#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; }