Diff: main.cpp

Revision:

2:7da4a8e74955

Parent:

1:692623e55ff7

Child:

3:bff63180d3e6

diff -r 692623e55ff7 -r 7da4a8e74955 main.cpp
--- a/main.cpp	Tue Jun 14 05:06:23 2022 +0000
+++ b/main.cpp	Wed Jun 15 09:33:38 2022 +0000
@@ -1,36 +1,24 @@
 /** 300_PS3com
  * PS3と通信して表示、ボタンに応じてモーターを動かす
- * モーターのピン確認、通信の確認用
- * ButtonEnumを実装したい
- * ライブラリの実装したい
  */
 
 //--------操作方法--------
-// ボタン１：モード変更
-// ボタン２：スタート、ストップ
-// ボタン３：動作１
-// ボタン４：動作２
 
 
 #include "main.h"
 #include "Master.h"
 #include "SerialArduino.h"
-
-#define PS3COM 1
-#define PS3UI  0
+//#include "UI.h"
 
 DigitalOut led(LED1);
 Master Master;// 出力等の基本クラス
-
-#if PS3COM
+//Thread DisplayThread;
+OmniWheel omni(4);
 SerialArduino mini(A0, A1, 115200);// Arduinoを用いてPS3コントローラと通信するクラス
-void PS3get(void);
-#endif
+Timer Time_Shot;
 
-#if PS3UI
-DigitalIn uiToggle[3] = {};
-DigitalIn uiButton();
-#endif
+void PS3get(bool flag_PS3print);
+//void Display(void);
 
 uint8_t h1,h2;
 bool    b[12]= {};
@@ -38,59 +26,92 @@
 uint8_t tr[2]= {};
 
 int main()
-{
+{    
     uint8_t mode=0;
-    bool EdgeToggle[3]= {};
-    bool pretoggle[3]= {};
+    bool EdgeToggle[3]={}, pretoggle[3]={};
+    bool flag_Dribble=false, flag_Shot=false;
+    Time_Shot.start();
 
-    double  pwm[4] = {};
+    double pwm[4]={}, DribblePower = 0.5;
+    double Vx=0, Vy=0, Vr=0;
+    double VxLimit=0.75, VyLimit=0.75, VrLimit=0.2;
 
     Master.SetPS3Address(b, tr, st);
+    
+//    DisplayThread.start(Display);
+#if RobotNumberIsOne
+    omni.wheel[0].setRadian(PI * 1.0 / 4.0);
+    omni.wheel[1].setRadian(PI * 7.0 / 4.0);
+    omni.wheel[2].setRadian(PI * 5.0 / 4.0);
+    omni.wheel[3].setRadian(PI * 3.0 / 4.0);
+#else
+    omni.wheel[0].setRadian(PI * 5.0 / 4.0);
+    omni.wheel[1].setRadian(PI * 7.0 / 4.0);
+    omni.wheel[2].setRadian(PI * 1.0 / 4.0);
+    omni.wheel[3].setRadian(PI * 3.0 / 4.0);
+#endif
 // 以下ループ
     while (true) {
+        
+        PS3get(true);
 
 // モーター出力値の計算
+        if (RobotNumberIsOne) {
+            Vx = ((st[0]-128.0)/128.0) * VxLimit;
+            Vy = (128.0-st[1])/128.0 * VyLimit;
+            Vr = -(st[2]-128.0)/128.0 * VrLimit;
+        } else {
+            Vx = ((st[0]-128.0)/128.0) * VxLimit;
+            Vy = (128.0-st[1])/128.0 * VyLimit;
+            Vr = (st[2]-128.0)/128.0 * VrLimit;
+        }
+        omni.computeXY(Vx, Vy, Vr);
+        
         for(int i=0; i < 4; i++) {
-            if (b[i+4]) {
-                pwm[i] = 0.5;
-            } else if (b[i+4+4]) {
-                pwm[i] = -0.5;
+            if (0) {
+                if (b[i+4]) {
+                    pwm[i] = 0.5;
+                } else if (b[i+4+4]) {
+                    pwm[i] = -0.5;
+                } else {
+                    pwm[i] = 0;
+                }
             } else {
-                pwm[i] = 0;
+                pwm[i] = ((double)(omni.wheel[i])*0.75);
             }
         }
+        if (b[2]) flag_Dribble = true;
+        else flag_Dribble = false;
+        
+        if (b[3] && (Time_Shot.read_ms() > 1000)) {
+            flag_Shot = true;
+            Time_Shot.reset();
+        } else {
+            flag_Shot = false;
+        }
+        
+        if (flag_Dribble) Master.Dribble(0.5);
+        else Master.Dribble(0.0);
+        if (flag_Shot) Master.Shot();
+        
+        for (int i=0; i<4; i++) {
+            if (fabs(pwm[i]) < 0.1) pwm[i] = 0.0;
+        }
 
-#if PS3COM
-        PS3get();
-#endif
-
-#if PS3UI
-        for (int i=0; i<3; i++) {
-            if (preToggle[i] && !((int)uiToggle[i])) EdgeToggle = 1;
-            preToggle[i] = (int)uiToggle[i];
-        }
-        if (uiButton) {
-            switch mode {
-            case 0:
-                Master.PS3Com();
-                    break;
-                case 1:
-                    Master.Shot();
-                    break;
-                case 2:
-                    Master.Dribble();
-                    break;
-            }
-        }
-#endif
+#if RobotNumberIsOne
+        Master.SetValueMotor(0, pwm[0]);
+        Master.SetValueMotor(1, pwm[1]);
+        Master.SetValueMotor(2, -pwm[2]);
+        Master.SetValueMotor(3, -pwm[3]);
+#else
         for (int i=0; i<4; i++) {
             Master.SetValueMotor(i, pwm[i]);
         }
+#endif
     }
 }
 
-#if PS3COM
-void PS3get(void)
+void PS3get(bool flag_PS3print)
 {
     h1 = mini.getHedder1();
     h2 = mini.getHedder2();
@@ -108,8 +129,35 @@
     for(int i=0; i<2; i++) printf("%3d ",tr[i]);
     printf("|");
     for(int i=0; i<4; i++) printf("%3d ",st[i]);
+//    printf("%3d %3d ", (int)(st[0]-128.0), (int)(128-st[1]));
     if(mini.getState()) printf("ok");
     else printf("bad");
     printf("\r\n");
 }
+
+#if 0
+void Display()
+{
+    char lcdname1[4] = {}, lcdname2[4] = {};
+    int  lcdvalue[2] = {};
+    double temp[2] = {};
+    while(1){
+        switch (ui.displayst) {
+        case 0:
+            strcpy(lcdname1, "mt0");
+            strcpy(lcdname2, "mt1");
+            lcdvalue[0] = (int)((double)(omni.wheel[0])*100);
+            lcdvalue[1] = (int)((double)(omni.wheel[1])*100);
+            break;
+        case 1:
+            strcpy(lcdname1, "mt2");
+            strcpy(lcdname2, "mt3");
+            lcdvalue[0] = (int)((double)(omni.wheel[2])*100);
+            lcdvalue[1] = (int)((double)(omni.wheel[3])*100);
+            break;
+        }
+        ui.change(lcdname1, lcdname2, lcdvalue);
+//        thread_sleep_for(3);
+    }
+}
 #endif
\ No newline at end of file