Diff: main.cpp

Revision:

6:cd2671c141cc

Parent:

5:8a0663298e2c

--- a/main.cpp	Sat Jan 25 06:38:16 2020 +0000
+++ b/main.cpp	Sat Feb 01 06:40:55 2020 +0000
@@ -5,142 +5,185 @@
 #include <stdint.h>
 #include <stdio.h>
 #include <math.h>
-#define SDA PB_7 
+#define SDA PB_7
 #define SCL PB_6
 #define MCP_ADDRESS 0x40
 
 MCP MCP(SDA, SCL, MCP_ADDRESS);
 XBEE::ControllerData *controller;
 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
-
+Serial pc(USBTX,USBRX);
 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
-    
+
 #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;
+
+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で右進です。
+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;//左右の速度
+    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);
+    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;
+    //if (left_value < 0) {
+    //  left_value = left_value * 0.95;
+    // } else {
+    left_value = left_value * 0.85;
+    right_value = right_value * 0.85;
+    //  }
+    outputMecani(right_value, left_value, omni_value);
 
 }
 
 // メカニ旋回関数
-void turnMecani(double power, double center) {
-  //powerには、 出力の大きさを-255～255で入れてください。255で最大です。
-  //centerには、回転の中心の位置を-255~255で入れてください。中心の位置は、値が大きいとメカナム寄り、小さいとオムニ寄りになります。
+void turnMecani(double power, double center)
+{
+    //powerには、 出力の大きさを-255～255で入れてください。255で最大です。
+    //centerには、回転の中心の位置を-255~255で入れてください。中心の位置は、値が大きいとメカナム寄り、小さいとオムニ寄りになります。
 
-  double right_value; //右メカナムの値
-  double left_value;  //左メカナムの値
-  double omni_value;  //オムニの値
+    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);
-  }
+    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);
+    outputMecani(right_value, left_value, omni_value);
 }
 
 // メカニピン出力関数Arduino用
-void outputMecani(double right_value, double left_value, double omni_value) {
+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 (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 (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;
+    }
+    
+    double R=fabs(right_value);
+    double L=fabs(left_value);
+    double om=fabs(omni_value);
+    R=map(R,0,255,0,100);
+    L=map(L,0,255,0,100);
+    om=map(om,0,255,0,100);
+    motor[mekanamuR].pwm=R;
+    motor[mekanamuL].pwm=L;
+    motor[omni].pwm=om;
 
-  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() {
-    
+int main()
+{
+
     XBEE::Controller::Initialize();
     MOTOR::Motor::Initialize();
-    
-     __enable_irq();
-    
+
+    __enable_irq();
+
     /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
-    
-    
-    
+
+
+
     /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
-    
+
     while(1) {
-        
+
         controller = XBEE::Controller::GetData();
         MCP.Update();
-        
+
         /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
-        
+
         int A=controller->AnalogL.X-7;
         int B=controller->AnalogL.Y-7;
-        float shita=atan(double(B)/double(A));
+        double 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;
+        power=map(power,0,7.0,0,255);
+        pc.printf("\r\n");
+
+        if(A<0) {
+            if(B>0) {
+                shita=3.141592-atan(fabs(double(B/A)));
+            } else if(B<0) {
+                shita=-3.141592+atan(fabs(double(B/A)));
+            } else if(B==0) {
+                shita=3.141592;
             }
-        if(A==0){
-            if(B>0){
-                shita=3.141592;
-                }else if(B<0){shita=-3.141592;
-                }else if(B==0){shita=0;
-                }
-                }
-            
+        }
+
+
+        driveMecani(power,shita);
         
+        if(A==0&&B==0) {
+            motor[omni].dir=BRAKE;
+            motor[omni].pwm=100;
+            motor[mekanamuR].dir=BRAKE;
+            motor[mekanamuR].pwm=100;
+            motor[mekanamuL].dir=BRAKE;
+            motor[mekanamuL].pwm=100;
+            int turn=controller->AnalogR.X;
+            turn=map(turn,0,14,-255,255);
+            turnMecani(turn,1);
+        } else if(A==0&&B>0) {
+            motor[omni].dir=BRAKE;
+            motor[omni].pwm=100;
+            motor[mekanamuR].dir=FOR;
+            motor[mekanamuR].pwm=power;
+            motor[mekanamuL].dir=BACK;
+            motor[mekanamuL].pwm=power;
+        } else if(A==0&&B<0) {
+            motor[omni].dir=BRAKE;
+            motor[omni].pwm=100;
+            motor[mekanamuR].dir=BACK;
+            motor[mekanamuR].pwm=power;
+            motor[mekanamuL].dir=FOR;
+            motor[mekanamuL].pwm=power;
+        }
+        pc.printf("right=%lf,left=%lf,omni=%lf",motor[mekanamuR].pwm,motor[mekanamuL].pwm,motor[omni].pwm);
+
+
+
         /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
-        
+
         MOTOR::Motor::Update(motor);
     }
 }