Diff: main.cpp

Revision:

0:f6828b914b1c

Child:

1:f4dbd6b9cc27

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 21 06:51:59 2018 +0000
@@ -0,0 +1,163 @@
+#include "mbed.h"
+#include "config.h"
+#include "moter.h"
+#include "PID.h"
+
+Serial pc(USBTX, USBRX);
+Serial kbtpc(PC_12 ,PD_2);//serial5
+I2C master(sda, scl);
+DigitalOut led2(LED2);
+PID S_pid(SP, SI, SD, 0.0);
+
+PwmOut pwm_pins[] = {
+    PwmOut( PC_8 ),
+    PwmOut( PC_6 ),
+    PwmOut( PB_14 ),
+    PwmOut( PB_13 )
+};
+
+DigitalOut dir_pins_1[] = {
+    DigitalOut( PC_5 ),
+    DigitalOut( PA_7 ),
+    DigitalOut( PC_7 ),
+    DigitalOut( PB_15)
+};
+
+DigitalOut dir_pins_2[] = {
+    DigitalOut( PB_12 ),
+    DigitalOut( PA_6 ),
+    DigitalOut( PB_6 ),
+    DigitalOut( PB_1)
+};
+
+bool mode = true;//trueでラジコン、falseでオート
+bool stopflag = true;//trueで機体停止
+int posit[2] = {0};//機体の現在位置
+int nextposit[2] = {0};//機体の位置指定
+double power = POWER;
+int kbtread = 0;
+int count = 0;
+int counter = 0;
+int linedata = 0;
+int prelinedata = 0;
+double output = 0.0;
+bool rpower = false;
+bool lpower = false;
+
+int linecheck(char *buff){
+    if(buff[0] == 24){
+        linedata = 0;
+    }else if(buff[0] == 56){
+        linedata = 1;
+    }else if(buff[0] == 48){
+        linedata = 2;
+    }else if(buff[0] == 112){
+        linedata = 3;
+    }else if(buff[0] == 96){
+        linedata = 4;
+    }else if(buff[0] == 224){
+        linedata = 5;
+    }else if(buff[0] == 192){
+        linedata = 6;
+    }else if(buff[0] == 128){
+        linedata = 7;
+    }else if(buff[0] == 28){
+        linedata = -1;
+    }else if(buff[0] == 12){
+        linedata = -2;
+    }else if(buff[0] == 14){
+        linedata = -3;
+    }else if(buff[0] == 6){
+        linedata = -4;
+    }else if(buff[0] == 7){
+        linedata = -5;
+    }else if(buff[0] == 3){
+        linedata = -6;
+    }else if(buff[0] == 1){
+        linedata = -7;
+    }else if(buff[0] == 15 && buff[1] == 15){
+        linedata = 255;
+    }else if(buff[0] == 0 && buff[1] == 0){
+        linedata = -255;
+    }else{
+        linedata = prelinedata;
+    }
+    prelinedata = linedata;
+
+    return linedata;
+}
+    
+int main() {
+    
+    S_pid.init();
+    pwm_pins[0].period(0.00005);
+    pwm_pins[1].period(0.00005);
+    pwm_pins[2].period(0.00005);
+    pwm_pins[3].period(0.00005);
+    kbtpc.baud(2400);
+    
+    char buff1[2];
+    
+    while (true) {
+        master.read(addr1,buff1,2);
+        int line = linecheck(buff1);
+        kbtpc.putc(1);
+        if(kbtpc.readable()){
+            kbtread = kbtpc.getc();
+        }
+        if(kbtread == 255){
+            mode = true;
+        }else if(kbtread == 150){
+            mode = false;
+        }else if(kbtread == 100){
+            stopflag = true;
+        }else if(kbtread == 200){
+            stopflag = false;
+        }
+//--------------手動--------------------------------------
+        if(mode == true){
+            led2 = 1.0;
+            if(kbtread == 1){
+                Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2);
+            }else if(kbtread == 2){
+                Back(power, power, pwm_pins, dir_pins1, dir_pins_2);
+            }else if(kbtread == 3){
+                Right(power, power, pwm_pins, dir_pins1, dir_pins_2);
+            }else if(kbtread == 4){
+                Left(power, power, pwm_pins, dir_pins1, dir_pins_2);
+            }else{
+                Stop(pwm_pins, dir_pins1, dir_pins_2);
+            }
+//--------------自動---------------------------------
+        }else if(mode == false){
+            led2 = 0.0;
+            if(stopflag == true){
+                Stop(pwm_pins, dir_pins1, dir_pins_2);
+            }else{
+                if(line == 255 || line == -255){
+                    stopflag = true;
+                    Stop(pwm_pins, dir_pins1, dir_pins_2);
+                    wait(1.0);
+                }else{
+                    output = S_pid.compute((double)line);
+                    if(output >= 0){
+                        rpower = false;
+                        lpower = true;
+                        Straight(power, power + output, pwm_pins, dir_pins1, dir_pins_2);
+                    }else {
+                        lpower = false;
+                        rpower = true;
+                        Straight(power + (-1 * output), power, pwm_pins, dir_pins1, dir_pins_2);
+                    }
+                }
+            }
+        }
+        //pc.printf("kbtread = %d" ,kbtread);     
+        ///pc.printf("buff[0] = %d" ,buff1[0]);
+        //pc.printf("buff[1] = %d" ,buff1[1]);
+        pc.printf("line    = %d" ,line);
+        pc.printf("rpower    = %d" ,rpower);
+        pc.printf("lpower    = %d" ,lpower);
+        pc.printf("output  = %lf \n" ,output);
+    }
+}