Diff: main.cpp

Revision:

0:14ecdafa55eb

diff -r 000000000000 -r 14ecdafa55eb main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 28 05:10:23 2019 +0000
@@ -0,0 +1,144 @@
+#include "mbed.h"
+#include"PS3.h"
+#include "ikarashiMDC.h"
+
+Serial serial(PC_6, PC_7);
+DigitalOut serialcontrol(D2);
+DigitalOut led1(LED1);
+PS3 ps3(PC_12, PD_2);
+Serial pc(USBTX, USBRX,115200);
+
+ikarashiMDC ikarashi[] {
+    ikarashiMDC(&serialcontrol,2,3,SM,&serial),
+    ikarashiMDC(&serialcontrol,2,0,SM,&serial),
+    ikarashiMDC(&serialcontrol,2,1,SM,&serial),
+    ikarashiMDC(&serialcontrol,2,2,SM,&serial),
+    ikarashiMDC(&serialcontrol,3,0,SM,&serial),
+    ikarashiMDC(&serialcontrol,3,1,SM,&serial)
+};
+PwmOut pwm(LED1);
+
+int main()
+{
+    serial.baud(115200);
+    float i = -1;
+    ikarashi[0].braking = true;
+    float pwm = 0;
+    while(1) {
+
+        int b[12], stick[4],X,Y,M;
+        double SPe[4];
+      
+        /*ボタンスイッチ*/
+        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] );
+        }
+        
+        //ベルト機構
+        if(b[6] == 0 || b[7] == 0){ 
+            ikarashi[5].setSpeed(0);
+        }  
+        if(b[6] == 1){   
+            ikarashi[4].setSpeed(0.08);        
+            ikarashi[5].setSpeed(-0.45);
+        }
+        if(b[7] == 1){
+            ikarashi[4].setSpeed(-0.08); 
+            ikarashi[5].setSpeed(0.45);
+        }
+            
+        
+        //バケット機構
+        if(b[0] == 0 || b[1] == 0){ 
+            ikarashi[4].setSpeed(0);
+        }  
+        if(b[0] == 1){           
+            ikarashi[4].setSpeed(0.08);
+        }
+        if(b[1] == 1){
+            ikarashi[4].setSpeed(-0.08);
+        }
+        
+        
+        //足回り　　　
+        X = stick[0];
+        Y = stick[1];
+        M = stick[2];
+        
+        if(X > 100 && X < 155 && Y > 100 && Y < 155) { //停止
+            SPe[0] = 0;
+            SPe[1] = 0;
+            SPe[2] = 0;
+            SPe[3] = 0;
+            
+            pc.printf("  停止　　");
+        }
+        if(X > 100 && Y <= 100) { //右前方向
+            SPe[0] = -0.45;
+            SPe[1] = ((127.5 - Y) - (X - 127.5)) * 0.004;
+            SPe[2] = 0.45;
+            SPe[3] = ((X - 127.5) - (127.5 - Y)) * 0.004;
+            
+            pc.printf("  右前方向  ");
+        }
+        if(X <= 100 && Y < 155) { //左前方向
+            SPe[0] = ((127.5 - X) - (127.5 - Y)) * 0.004;
+            SPe[1] = 0.45;
+            SPe[2] = ((127.5 - X) - (127.5 - Y)) * -0.004;
+            SPe[3] = -0.45;
+            
+            pc.printf("  左前方向  ");
+        }
+        if(X >= 155 && Y > 100) { //右後ろ方向
+            SPe[0] = ((X - 127.5) - (Y - 127.5)) * -0.004;
+            SPe[1] = -0.45;
+            SPe[2] = ((X - 127.5) - (Y - 127.5)) * 0.004;
+            SPe[3] = 0.45;
+            
+            pc.printf("  右後ろ方向　　");
+        }
+        if(X < 155 && Y >= 155) { //左後ろ方向
+            SPe[0] = 0.45;
+            SPe[1] = ((X - 127.5) + (127.5 - Y)) * 0.004;
+            SPe[2] = -0.45;
+            SPe[3] = ((X - 127.5) - (127.5 - Y)) * 0.004;
+            
+            pc.printf("  左後ろ方向　　");
+        }   
+        if(M < 10) {  //左回転
+            SPe[0] = 0.4;
+            SPe[1] = 0.4;
+            SPe[2] = 0.4;
+            SPe[3] = 0.4;
+            
+            pc.printf("  左回転  ");
+        }
+        if(M > 245) {  //右回転
+            SPe[0] = -0.4;
+            SPe[1] = -0.4;
+            SPe[2] = -0.4;
+            SPe[3] = -0.4;
+            
+            pc.printf("  右回転  ");
+
+        }
+           
+
+            ikarashi[0].setSpeed(SPe[0]);
+            ikarashi[1].setSpeed(SPe[1]);
+            ikarashi[2].setSpeed(SPe[2]);
+            ikarashi[3].setSpeed(SPe[3]);
+            
+            pc.printf("%.2f  ",SPe[0]);
+            pc.printf("%.2f  ",SPe[1]);
+            pc.printf("%.2f  ",SPe[2]);
+            pc.printf("%.2f  ",SPe[3]);
+            
+             pc.printf("\r\n");
+    }