Revision 1:a1ad958c4836, committed 2019-04-26

Comitter:

piroro4560

Date:

Fri Apr 26 08:27:30 2019 +0000

Parent:

0:8f324e7a7491

Commit message:

three omuni

Changed in this revision

--- a/main.cpp	Thu Mar 07 07:50:15 2019 +0000
+++ b/main.cpp	Fri Apr 26 08:27:30 2019 +0000
@@ -1,7 +1,9 @@
-#include "mbed.h"
+#include "mbed.h"           //wheel test
 #include "ikarashiMDC.h"
 #include "controller.h"
-Controller pad(PC_10, PC_11, 208);
+#define PI 3.141593
+
+Controller pad(PC_12, PD_2, 208);
 Serial pc(USBTX, USBRX, 115200);
 Serial serial(PC_6,PC_7);
 DigitalOut serialcontrol(D2);
@@ -10,98 +12,73 @@
 ikarashiMDC ikarashi[] {
     ikarashiMDC(&serialcontrol,2,0,SM,&serial),
     ikarashiMDC(&serialcontrol,2,1,SM,&serial),
-    ikarashiMDC(&serialcontrol,2,2,SM,&serial),
-    ikarashiMDC(&serialcontrol,2,3,SM,&serial)
+    ikarashiMDC(&serialcontrol,2,2,SM,&serial)
 };
 
 int main()
 {
     serial.baud(115200);
     ikarashi[0].braking = true;
-    int button[11];
-    float stickRadian[2],stickNorn[2];
-    double acc[4];
+    int b[11];
+    float rad[2],dis[2];
+    double value[3];
     while(1){
-    //コントローラー通信
+
+/*
+      7       9
+      8       10
+    -------------
+    1    | |   3  
+         | | 2   4
+    0  --| |-- 5
+      |12| |11|  6
+                 
+
+*/
+
+    ////stick button////
         if(pad.receiveState()){
             for(int i = 0; i < 13; i++){
-                button[i] = pad.getButton1(i);
-                pc.printf("%d ", button[i]);
+                b[i] = pad.getButton1(i);
             }
             for(int i = 0; i < 2; i++){
-                stickRadian[i] = pad.getRadian(i);
-                stickNorn[i] = pad.getNorm(i);
-                pc.printf("%f %f\t", stickRadian[i], stickNorn[i]);
-            }
-            pc.printf("\n\r");
-        
-        //十字移動
-            if(button[2] == 1 && button[3] == 0 && button[4] == 1 && button[5] == 1){
-                stickNorn[0] = 1;
-                stickRadian[0] = 1.570796;
-            }else if(button[2] == 0 && button[3] == 1 && button[4] == 1 && button[5] == 1){
-                stickNorn[0] = 1;
-                stickRadian[0] = 0;
-            }else if(button[2] == 1 && button[3] == 1 && button[4] == 0 && button[5] == 1){
-                stickNorn[0] = 1;
-                stickRadian[0] = 3.141593;
-            }else if(button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 0){
-                stickNorn[0] = 1;
-                stickRadian[0] = -1.570796;
-            }else if(stickNorn[0] == 0 && button[7] == 1 && button[9] == 1) {
-                acc[0] = 0;
-                acc[1] = 0;
-                acc[2] = 0;
-            }
-        //全方位移動
-            if(stickNorn[0] != 0){
-                acc[0] = sin(stickRadian[0] - PI * 11 / 6);
-                acc[1] = sin(stickRadian[0] - PI * 1 / 2);
-                acc[2] = sin(stickRadian[0] - PI * 7 / 6);
+                rad[i] = pad.getRadian(i);
+                dis[i] = pad.getNorm(i);
+                ////change stick value////
+                rad[i] = PI - rad[i];
             }
-        //左右旋回
-            if(button[9] == 0 && button[7] == 1 && stickNorn[0] == 0) {
-                acc[0] = -0.5;
-                acc[1] = -0.5;
-                acc[2] = -0.5;
-            }
-            if(button[7] == 0 && button[9] == 1 && stickNorn[0] == 0) {
-                acc[0] = 0.5;
-                acc[1] = 0.5;
-                acc[2] = 0.5;
-            }
-        //機構制御
-            if(button[10] == 0 && button[8] == 1){
-                acc[3] = -0.4;
-            }
-            if(button[8] == 0 && button[10] == 1){
-                acc[3] = 0.4;
+
+            ////set speed////
+            
+            value[0] = sin(rad[1] - PI * 1 / 6) * dis[1];
+            value[1] = sin(rad[1] - PI * 5 / 6) * dis[1];
+            value[2] = sin(rad[1] - PI * 9 / 6) * dis[1];
+            
+            if(b[8] == 1 && b[10] == 0) {
+                value[0] -= 0.3;
+                value[1] -= 0.3;
+                value[2] -= 0.3;
+            } else if (b[10] == 1 && b[8] == 0) {
+                value[0] += 0.3;
+                value[1] += 0.3;
+                value[2] += 0.3;
             }
-            if(button[8] == button[10]) {
-                acc[3] = 0;
-            }
-        //減速
-            if(button[0] == 0) {
-                for(int i = 0;i < 4;i++) {
-                    if(acc[i] >= 0.5){
-                        acc[i] = 0.5;
-                    }
-                    if(acc[i] <= -0.5) {
-                        acc[i] = -0.5;
-                    }
-                }
-            }
-            pc.printf("%f %f %f %f",acc[0],acc[1],acc[2],acc[3]);
-            ikarashi[0].setSpeed(acc[0]);
-            ikarashi[1].setSpeed(acc[1]);
-            ikarashi[2].setSpeed(acc[2]);
-            ikarashi[3].setSpeed(acc[3]);
+            
+            pc.printf("%f", value[0]);
+            pc.printf("%f", value[1]);
+            pc.printf("%f", value[2]);
+
+            pc.printf("\n\r");
+            ikarashi[0].setSpeed(value[0]);
+            ikarashi[1].setSpeed(value[1]);
+            ikarashi[2].setSpeed(value[2]);
+
         }else{
             pc.printf("error\n\r");
-                    ikarashi[0].setSpeed(acc[0]);
-        ikarashi[1].setSpeed(0);
-        ikarashi[2].setSpeed(0);
-        ikarashi[3].setSpeed(0);
+            for (int i = 0; i < 3; i++)
+            {
+                ikarashi[i].setSpeed(0);
+            }
         }
     }
 }