Diff: main.cpp

Revision:

0:193dcda90d39

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 07 07:40:22 2019 +0000
@@ -0,0 +1,109 @@
+#include"mbed.h"
+#include"ikarashiMDC.h"
+#include"PS3.h"
+#define PI 3.14159265
+Serial serial(PC_6,PC_7);
+DigitalOut serialcontrol(D2);
+
+    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,1,0,SM,&serial)
+};
+
+PS3 ps3(PC_10,PC_11);
+Serial pc(USBTX,USBRX,115200);
+
+double way(double x,double y,double direction)
+{
+    double atanval;
+    atanval = atan2(y,x);
+    return sin(atanval - direction);
+}
+
+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];
+    /*ジョイスティック　ニュートラル設定*/
+    do{
+        for(int i = 0;i < 4;i++) {
+            neutral[i] = ps3.getStick(i);
+            pc.printf("%4d",neutral[i]);
+        }
+        count++;
+    }while(count <= 115200);
+    for(int i = 0;i < 4;i++) {
+        neutral[i] = ps3.getStick(i);
+        pc.printf("%4d",neutral[i]);
+    }
+    while(1){
+          /*ボタン０＝上　１＝下　２＝左　３＝右　４＝L１　５＝R１　６＝△　７＝✖
+          ８＝🔲　９＝◯　１０＝L３　１１＝R３*/
+          for(int i=0;i < 12;i++) {
+              b[i] = ps3.getButton(i);
+              pc.printf("%d",b[i]);
+          }
+          /*ジョイスティック*/
+        for(int i = 0;i < 4;i++){
+            stick[i] = ps3.getStick(i);
+            pc.printf("%4d\r\n",stick[i]);
+            }
+        
+        for(int i = 0;i < 2;i++) {
+            trigger[i] = ps3.getTrigger(i);
+            pc.printf("%4d\r\n",trigger[i]);
+    }
+     X = stick[0] - neutral[0];
+     Y = neutral[1] - stick[1]; 
+     if( abs(X) <= 20 && abs(Y) <= 20){
+         acc[0] = 0;
+         acc[1] = 0;
+         acc[2] = 0;
+     }else { 
+             acc[0] = way(X,Y,PI/6);
+             acc[1] = way(X,Y,PI*3/2);
+             acc[2] = way(X,Y,PI/5/6);
+            }
+    if(b[4] == 1 && abs(X) <= 20 && abs(Y) <= 20){
+             acc[0] = 1;
+             acc[1] = 1;
+             acc[2] = 1;
+            }
+    if(b[5] == 1 && abs(X) <= 20 && abs(Y) <= 20){
+             acc[0] = -1;
+             acc[1] = -1;
+             acc[2] = -1;
+            }
+    if(b[7] == 1){
+        acc[0] = 0;
+        acc[1] = 0;
+        acc[2] = 0;
+        }
+    if(trigger[0] == 0 && trigger[1] == 0) {
+        acc[3] = 0;
+    }
+    if(trigger[0] >= 1 && trigger[1] == 0) {
+        acc[3] = 0.4;
+    }
+    if(trigger[0] == 0 && trigger[1] >= 1) {
+        acc[3] = -0.4;
+    }
+    if(trigger[0] >= 1 && trigger[1] >= 1) {
+        acc[3] = 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 < 4;i++) {
+        pc.printf("%f\r\n",acc[i]);
+        }
+    
+    }
+
+}