Revision 0:f3c5ba43a1aa, committed 2017-09-24

Comitter:

echo_piyo

Date:

Sun Sep 24 05:24:12 2017 +0000

Commit message:

????2??

Changed in this revision

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mecanum.h	Sun Sep 24 05:24:12 2017 +0000
@@ -0,0 +1,55 @@
+/*
+Mecanum mecanum
+
+Sbdbtの入力 +y↑ -y↓ +x→ -x←
+.setupdeg([float]Deg)   //角度センサの初期値設定
+.sbdbt_cal([float]Vx, [float]Vy, [int]con_rl, [int]con_l1, [float]w, [float]angle) //使わない部分は0を代入
+.xy_cal([float]Vx, [float]Vy) //xyの代入値(-1.0 ~ 1.0) 
+
+.v1 //(返り値[float] -1.0 ~ 1.0) モータ1用出力
+.v2 //(返り値[float] -1.0 ~ 1.0) モータ2用出力
+.v3 //(返り値[float] -1.0 ~ 1.0) モータ3用出力
+.v4 //(返り値[float] -1.0 ~ 1.0) モータ4用出力
+
+メカナム設置図
+    ↑→   →↓
+     O---O
+     O---O
+    ←↑   ↓←
+
+
+*/
+
+#define rotaPower 0.6
+#define rotaStickPower 0.35
+#define boost_A 0.5
+#define boost_K 0.9
+#define PI 3.141592
+
+#ifndef MBED_MECANUM_H
+#define MBED_MECANUM_H
+
+#include "mbed.h"
+
+class Mecanum{
+    public:
+        void sbdbt_cal(float Vx, float Vy, int r1, int l1,float w, float angle);
+        void setupdeg(float Deg);
+        void boost();
+        void boost_forward();
+        void boost_back();
+        void boost_right();
+        void boost_left();
+        void xy_cal(float Vx, float Vy);
+        float v1();
+        float v2();
+        float v3();
+        float v4();
+        float VX();
+        float VY();
+    private:
+        float V1, V2, V3, V4, vx, vy;
+        float first_deg, rad;
+};
+
+#endif
\ No newline at end of file