Diff: main.cpp

Revision:

0:d14e21c64226

Child:

1:8a67adccebd9

diff -r 000000000000 -r d14e21c64226 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 13 11:18:21 2019 +0000
@@ -0,0 +1,229 @@
+#include "mbed.h"
+#include "main.h"
+#include "moterdrive.h"
+#include "DualShockMod.h"
+
+#define PERIOD 100
+
+DigitalOut  my_led(LED1);
+PwmOut      md1_pwm1(PB_2);
+DigitalOut  md1_cw1(PB_12);
+DigitalOut  md1_ccw1(PB_1);
+DigitalOut  md1_dis1(PA_11);
+
+PwmOut      md1_pwm2(PB_15);
+DigitalOut  md1_cw2(PA_12);
+DigitalOut  md1_ccw2(PB_14);
+DigitalOut  md1_dis2(PB_13);
+
+PwmOut      md2_pwm1(PC_7);
+DigitalOut  md2_cw1(PA_8);
+DigitalOut  md2_ccw1(PA_3);
+DigitalOut  md2_dis1(PB_10);
+
+PwmOut      md2_pwm2(PB_3);
+DigitalOut  md2_cw2(PB_4);
+DigitalOut  md2_ccw2(PA_10);
+DigitalOut  md2_dis2(PB_5);
+
+DigitalIn   Limit1(PC_9);
+DigitalIn   Limit2(PC_8);
+DigitalIn   Limit3(PC_6);
+DigitalIn   Limit4(PC_5);
+
+DigitalIn   photo1(PA_6);
+DigitalIn   photo2(PA_7);
+DigitalIn   photo3(PB_6); 
+
+//serial通信
+Serial pc(SERIAL_TX, SERIAL_RX);
+Serial tsuushin(PC_10,PC_11);  
+
+
+/*プロトタイプ宣言*/
+void moter(int num, char dir, float power);
+
+
+Timer timer;
+
+int main(){
+    double posX = 0;
+    double posY = 0;
+    
+    typedef enum{
+        WAIT,
+        HARI1,
+        HARI2,
+        HARI3,
+        SYM1,
+        END   
+    }SEQENCE;
+    SEQENCE seq = WAIT;
+
+    typedef enum{
+        STOP,
+        FFAST,
+        FSLOW,
+        RIGHT,
+        LEFT,
+        RROLL,
+        LROLL,
+        BACK 
+    }MOVEDIR;
+    MOVEDIR move = STOP;
+
+    //エンコーダーの値
+    pc.printf("posX=%d,posY=%d\n\r",posX,posY);
+    uint8_t InitDS(Serial* f_serial);
+    void getPOSdata(void);
+    tsuushin.baud(115200);    
+    InitDS(&tsuushin);
+    tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始           
+
+    
+    timer.start();
+    while(1){
+//代入部
+//        posX = ;
+//        posY = ;     
+
+        switch(seq){
+            case WAIT:
+            if(timer.read_ms() > 3000){
+                seq = HARI1;
+                timer.reset();
+                }    
+            break;
+            case HARI1:
+                move = FFAST;
+                
+            break;
+            
+            }
+            
+            
+        switch(move){
+            case STOP:
+                moter(0,STOP,0);
+                moter(1,STOP,0);
+                moter(2,STOP,0);
+            break;
+            case FFAST:
+                moter(0,STOP,0);
+                moter(1,cw,1);
+                moter(2,ccw,1);
+            break;
+            case FSLOW:
+                moter(0,STOP,0);
+                moter(1,cw,0.5);
+                moter(2,ccw,0.5);
+            break;
+            case BACK:
+                moter(0,STOP,0);
+                moter(1,ccw,1);
+                moter(2,cw,1);
+            break;
+            case RROLL:
+                moter(0,ccw,0.3);
+                moter(1,ccw,0.3);
+                moter(2,ccw,0.3);
+            break;
+            case LROLL:
+                moter(0,cw,0.3);
+                moter(1,cw,0.3);
+                moter(2,cw,0.3);
+            break;
+            case RIGHT:
+                moter(0,cw,1);
+                moter(1,ccw,0.5);
+                moter(2,ccw,0.5);
+            break;
+            case LEFT:
+                moter(0,ccw,1);
+                moter(1,cw,0.5);
+                moter(2,cw,0.5);
+            break;
+            
+        }
+
+
+
+
+         }    
+}
+
+
+
+void moter(int num, char dir, float power){
+    
+    int output;
+    if(num == 0){
+        if(dir == cw){
+            md1_cw1 = 1;
+            md1_ccw1 = 0;
+            }
+            else if(dir == ccw){
+            md1_cw1 = 0;
+            md1_ccw1 = 1;
+            }
+            else if(dir == STOP){
+            md1_cw1 = 0;
+            md1_ccw1 = 0;
+            }
+        output = PERIOD * power ;
+        md1_pwm1.pulsewidth_ms(output);
+        } 
+        if(num == 1){
+            if(dir == cw){
+            md1_cw2 = 1;
+            md1_ccw2 = 0;
+            }
+            else if(dir == ccw){
+            md1_cw2 = 0;
+            md1_ccw2 = 1;
+            }
+            else if(dir == STOP){
+            md1_cw2 = 0;
+            md1_ccw2 = 0;
+            }
+        output = PERIOD * power ;
+        md1_pwm2.pulsewidth_ms(output);
+        } 
+        if(num == 2){
+            if(dir == cw){
+            md2_cw1 = 1;
+            md2_ccw1 = 0;
+            }
+            else if(dir == ccw){
+            md2_cw1 = 0;
+            md2_ccw1 = 1;
+            }
+            else if(dir == STOP){
+            md2_cw1 = 0;
+            md2_ccw1 = 0;
+            }
+        output = PERIOD * power ;
+        md2_pwm1.pulsewidth_ms(output);
+        }
+        if(num == 3){
+            if(dir == cw){
+            md2_cw2 = 1;
+            md2_ccw2 = 0;
+            }
+            else if(dir == ccw){
+            md2_cw2 = 0;
+            md2_ccw2 = 1;
+            }
+            else if(dir == STOP){
+            md2_cw2 = 0;
+            md2_ccw2 = 0;
+            }
+        output = PERIOD * power ;
+        md2_pwm2.pulsewidth_ms(output);
+        }
+     
+    
+    
+    
+    }
+ 
\ No newline at end of file