Diff: mecanum.cpp

Revision:

0:f3c5ba43a1aa

--- /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