kusano kiyoshige / mecanum

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?

UserRevisionLine numberNew 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 }