Diff: main.cpp

Revision:

1:6078ee91e5d7

Parent:

0:b2c626a3336d

--- a/main.cpp	Thu Feb 28 05:33:24 2019 +0000
+++ b/main.cpp	Tue Mar 05 13:00:32 2019 +0000
@@ -5,7 +5,7 @@
 #define PI 3.14159265
 Serial serial(PC_6,PC_7);
 DigitalOut serialcontrol(D2);
-DigitalOut stop(PB_1,PB_15);
+DigitalOut stop(PB_12,PB_2);
 
 ikarashiMDC ikarashi[] {
     ikarashiMDC(&serialcontrol,2,0,SM,&serial),
@@ -14,7 +14,7 @@
     ikarashiMDC(&serialcontrol,2,3,SM,&serial)
 };
 
-PS3 ps3(PC_12, PD_2);
+PS3 ps3(PC_10, PC_11);
 Serial pc(USBTX,USBRX,115200);
 
 int main()
@@ -27,106 +27,102 @@
     do{
         for(int i = 0; i < 4; i++) {
             neutral[i] =  ps3.getStick(i);
-            pc.printf("%4d",neutral[i]);
         }
         count++;
-    } while(count <= 50);
+    } while(count <= 1152000);
     for(int i = 0; i < 4; i++) {
         neutral[i] =  ps3.getStick(i);
-        pc.printf("%4d",neutral[i]);
     }
     while(1){
         stop = 1;
     /*ボタン0=上　1=下 2=左　 3=右 4=L1 5=R1 6=△ 7=× 8=□ 9=〇 10=L3 11=R3*/
         for(int i = 0; i < 12; i++) {
             b[i] = ps3.getButton(i);
-            pc.printf("%2d",b[i]);
         }
     /*ジョイスティック*/
         for(int i = 0; i < 4; i++) {
             stick[i] = ps3.getStick(i);
-            pc.printf("%4d",stick[i] - neutral[i]);
         }
     /*トリガースイッチ*/
         for(int i = 0; i < 2; i++) {
             trigger[i] = ps3.getTrigger(i);
-            pc.printf("%4d",trigger[i]);
         }
         X = stick[0] - neutral[0];
         Y = neutral[1] - stick[1];
+        if(abs(X) <= 20){
+            X = 0;
+        }
+        if(abs(Y) <= 20) {
+            Y = 0;
+        }
     /*微調整用コマンド*/
         power = 1;
-        if(b[0] == 1 && b[1] == 0 && b[2] == 0 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
+        if(b[0] == 1 && b[1] == 0 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) {
             X = 0;
-            Y = 1;
-            power = 0.2;
+            Y = 128;
         }
-        if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
+        if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) {
             X = 0;
-            Y = -1;
-            power = 0.2;
+            Y = -128;
         }
-        if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
-            X = -1;
+        if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && X == 0 && Y == 0) {
+            X = -128;
             Y = 0;
-            power = 0.2;
         }
-        if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
-            X = 1;
+        if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && X == 0 && Y == 0) {
+            X = 128;
             Y = 0;
-            power = 0.2;
         }
     /*オムニ制御*/
-        if((stick[0] == 0 && stick[1] == 0 && stick[2] == 0 && stick [3] == 0) || ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
+        if((stick[0] == 0 && stick[1] == 0 && stick[2] == 0 && stick [3] == 0) || (X == 0 && Y == 0)) {
             acc[0] = 0;
             acc[1] = 0;
             acc[2] = 0;
         } else { 
-            acc[0] = omuni_speed(X,Y,PI * 1 / 3) * power;
-            acc[1] = omuni_speed(X,Y,PI * 3 / 3) * power;
-            acc[2] = omuni_speed(X,Y,PI * 5 / 3) * power;
+            acc[0] = omuni_speed(X,Y,PI * 1 / 6) * power;
+            acc[1] = omuni_speed(X,Y,PI * 9 / 6) * power;
+            acc[2] = omuni_speed(X,Y,PI * 5 / 6) * power;
        }
-       if((trigger[0] <= 200 && trigger[1] >= 200) && ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
+       if((trigger[0] <= 200 && trigger[1] >= 200) && (X == 0 && Y == 0)) {
             acc[0] = -0.5;
             acc[1] = -0.5;
             acc[2] = -0.5;
         }
-        if((trigger[1] <= 200 && trigger[0] >= 200) && ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
+        if((trigger[1] <= 200 && trigger[0] >= 200) && (X == 0 && Y == 0)) {
             acc[0] = 0.5;
             acc[1] = 0.5;
             acc[2] = 0.5;
         }
     /*機構制御*/
         if(b[4] == 1 && b[5] == 0){
-            acc[3] = -0.2;
+            acc[3] = 0.4;
         }
         if(b[4] == 0 && b[5] == 1){
-            acc[3] = 0.2; 
+            acc[3] = -0.4; 
         }
         if(b[4] == 0 && b[5] == 0){
             acc[3] = 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;
+        if (b[7] == 1) {
+            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;
+                }
             }
         }
-   /*タイムアウト制御 & 非常停止*/
+   /*タイムアウト制御*/
         if((ps3.status == false) || (b[10] == 1 && b[11] == 1)) {
-            stop = 0;
+        acc[0] = 0;
+        acc[1] = 0;
+        acc[2] = 0;
         }
-
         ikarashi[0].setSpeed(acc[0]);
         ikarashi[1].setSpeed(acc[1]);
         ikarashi[2].setSpeed(acc[2]);
         ikarashi[3].setSpeed(acc[3]);
-        for(int i = 0;i < 5;i++) {
-            pc.printf(" %f",acc[i]);
-        }
-        pc.printf("\r\n");
     }
 }
\ No newline at end of file