Diff: main.cpp

Revision:

5:8a0663298e2c

Parent:

3:8790e69f8c59

Child:

6:cd2671c141cc

--- a/main.cpp	Sat Jan 25 00:40:35 2020 +0000
+++ b/main.cpp	Sat Jan 25 06:38:16 2020 +0000
@@ -3,7 +3,8 @@
 #include "MCP.h"
 #include "XBee.h"
 #include <stdint.h>
-
+#include <stdio.h>
+#include <math.h>
 #define SDA PB_7 
 #define SCL PB_6
 #define MCP_ADDRESS 0x40
@@ -14,8 +15,88 @@
 
 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
     
+#define omni 0
+#define mekanamuR 1
+#define mekanamuL 2
     
-    
+long map(long x, long in_min, long in_max, long out_min, long out_max) {
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+void outputMecani(double right_value, double left_value, double omni_value);
+// メカニ平行移動数
+void driveMecani(double power, double degree) {
+  //powerには、 出力の大きさを0～255で入れてください。255で最大です。
+  //degreeには、進みたい角度を0～360で入れてください。0で右進です。
+
+  double right_value;//右メカナムの値
+  double left_value;//左メカナムの値
+  double omni_value;//オムニの値
+  double pa;//前後の速度
+  double pb;//左右の速度
+
+  pa = -(sin((degree)) * fabs(sin((degree))) * power);
+  pb = cos((degree)) * fabs(cos((degree))) * power;
+  right_value = -(pa + pb);
+  left_value = -(pa - pb);
+  omni_value = -pb * 0.75;
+  //if (left_value < 0) {
+  //  left_value = left_value * 0.95;
+  // } else {
+  right_value = right_value * 0.95;
+  //  }
+  outputMecani(right_value, left_value, omni_value);
+
+}
+
+// メカニ旋回関数
+void turnMecani(double power, double center) {
+  //powerには、 出力の大きさを-255～255で入れてください。255で最大です。
+  //centerには、回転の中心の位置を-255~255で入れてください。中心の位置は、値が大きいとメカナム寄り、小さいとオムニ寄りになります。
+
+  double right_value; //右メカナムの値
+  double left_value;  //左メカナムの値
+  double omni_value;  //オムニの値
+
+  if (center <= 0) {
+    omni_value = - power * fabs(center);
+    right_value = power;
+    left_value = -power;
+  } else {
+    omni_value = -power;
+    right_value = power * (center);
+    left_value = -power * (center);
+  }
+
+  outputMecani(right_value, left_value, omni_value);
+}
+
+// メカニピン出力関数Arduino用
+void outputMecani(double right_value, double left_value, double omni_value) {
+
+  if (right_value < 0) {
+    motor[mekanamuR].dir=BACK;
+  } else {
+    motor[mekanamuR].dir=FOR;
+  }
+
+  if (left_value < 0) {
+    motor[mekanamuL].dir=FOR;
+  } else {
+    motor[mekanamuL].dir=BACK;
+  }
+
+  if (omni_value > 0) {
+    motor[omni].dir=BACK;
+  } else {
+    motor[omni].dir=FOR;
+  }
+    motor[mekanamuR].pwm=fabs(right_value);
+    motor[mekanamuL].pwm=fabs(left_value);
+    motor[omni].pwm=fabs(omni_value);
+ //横移動時に曲がってしまう場合は調整してください。
+
+}
+
 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
 
 int main() {
@@ -38,7 +119,25 @@
         
         /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
         
+        int A=controller->AnalogL.X-7;
+        int B=controller->AnalogL.Y-7;
+        float shita=atan(double(B)/double(A));
+        double power=sqrt(double(A)*double(A)+double(B)*double(B));
+        power=map(power,0,9.90,0,255);
         
+        if(A<0&&B<0){
+            shita=shita+3.141592f;
+            }else if(A<0&&B>0){
+            shita=shita+3.141592/2;
+            }
+        if(A==0){
+            if(B>0){
+                shita=3.141592;
+                }else if(B<0){shita=-3.141592;
+                }else if(B==0){shita=0;
+                }
+                }
+            
         
         /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */