Diff: main.cpp

Revision:

0:bb6a0c792b11

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 06 12:03:00 2019 +0000
@@ -0,0 +1,127 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "PS3.h"
+#include "wheel.h"
+Serial serial(PC_6,PC_7);
+DigitalOut serialcontrol(D2);
+DigitalOut stop(PB_12,PB_2);
+
+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)
+};
+
+PS3 ps3(PC_10, PC_11);
+Serial pc(USBTX,USBRX,115200);
+
+int main()
+{
+    serial.baud(115200);
+    ikarashi[0].braking = true;
+    int b[12], stick[4], trigger[2],neutral[4],count = 0;
+    double X,Y,acc[5],power;
+    /*ジョイスティック ニュートラル設定*/
+    do{
+        for(int i = 0; i < 4; i++) {
+            neutral[i] =  ps3.getStick(i);
+        }
+        count++;
+    } while(count <= 1152000);
+    for(int i = 0; i < 4; i++) {
+        neutral[i] =  ps3.getStick(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);
+        }
+    /*ジョイスティック*/
+        for(int i = 0; i < 4; i++) {
+            stick[i] = ps3.getStick(i);
+        }
+    /*トリガースイッチ*/
+        for(int i = 0; i < 2; i++) {
+            trigger[i] = ps3.getTrigger(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 && X == 0 && Y == 0) {
+            X = 0;
+            Y = 128;
+        }
+        if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) {
+            X = 0;
+            Y = -128;
+        }
+        if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && X == 0 && Y == 0) {
+            X = -128;
+            Y = 0;
+        }
+        if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && X == 0 && Y == 0) {
+            X = 128;
+            Y = 0;
+        }
+    /*オムニ制御*/
+        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 / 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) && (X == 0 && Y == 0)) {
+            acc[0] = -0.5;
+            acc[1] = -0.5;
+            acc[2] = -0.5;
+        }
+        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.4;
+        }
+        if(b[4] == 0 && b[5] == 1){
+            acc[3] = -0.4; 
+        }
+        if(b[4] == 0 && b[5] == 0){
+            acc[3] = 0;
+        }
+    /*速度制限*/
+        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)) {
+        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]);
+    }
+}
\ No newline at end of file