Diff: main.cpp

Revision:

0:8f324e7a7491

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 07 07:50:15 2019 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "controller.h"
+Controller pad(PC_10, PC_11, 208);
+Serial pc(USBTX, USBRX, 115200);
+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)
+};
+
+int main()
+{
+    serial.baud(115200);
+    ikarashi[0].braking = true;
+    int button[11];
+    float stickRadian[2],stickNorn[2];
+    double acc[4];
+    while(1){
+    //コントローラー通信
+        if(pad.receiveState()){
+            for(int i = 0; i < 13; i++){
+                button[i] = pad.getButton1(i);
+                pc.printf("%d ", button[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);
+            }
+        //左右旋回
+            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;
+            }
+            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]);
+        }else{
+            pc.printf("error\n\r");
+                    ikarashi[0].setSpeed(acc[0]);
+        ikarashi[1].setSpeed(0);
+        ikarashi[2].setSpeed(0);
+        ikarashi[3].setSpeed(0);
+        }
+    }
+}