Diff: main.cpp

Revision:

1:f4dbd6b9cc27

Parent:

0:f6828b914b1c

Child:

2:c5996dd62e9c

--- a/main.cpp	Wed Mar 21 06:51:59 2018 +0000
+++ b/main.cpp	Thu Mar 22 06:16:03 2018 +0000
@@ -2,12 +2,16 @@
 #include "config.h"
 #include "moter.h"
 #include "PID.h"
+#include "encoder.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);
+Encoder Enc_Carry_X(PA_15,PA_14,PC_1);
+Encoder Enc_Carry_Y(PF_1,PA_1,PA_0);
+int linecheck(char *buff);
 
 PwmOut pwm_pins[] = {
     PwmOut( PC_8 ),
@@ -41,8 +45,84 @@
 int linedata = 0;
 int prelinedata = 0;
 double output = 0.0;
-bool rpower = false;
-bool lpower = false;
+char buff1[2];
+char buff2[2];
+char buff3[2];
+float x_dist = 0.0;
+float y_dist = 0.0;
+
+    
+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_pins_1, dir_pins_2);
+            }else if(kbtread == 3){
+                Right(power, power, pwm_pins, dir_pins_1, dir_pins_2);
+            }else if(kbtread == 4){
+                Left(power, power, pwm_pins, dir_pins_1, dir_pins_2);
+            }else{
+                Stop(pwm_pins, dir_pins_1, dir_pins_2);
+            }
+//--------------自動---------------------------------
+        }else if(mode == false){
+            led2 = 0.0;
+            if(stopflag == true){
+                Stop(pwm_pins, dir_pins_1, dir_pins_2);
+            }else{
+                if(line == 255){
+                    stopflag = true;
+                    Stop(pwm_pins, dir_pins_1, dir_pins_2);
+                }else{
+                    output = S_pid.compute((double)line);
+                    if(output >= 0){
+                        Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
+                    }else {
+                        Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, 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);
+    }
+}
+
 
 int linecheck(char *buff){
     if(buff[0] == 24){
@@ -78,86 +158,11 @@
     }else if(buff[0] == 15 && buff[1] == 15){
         linedata = 255;
     }else if(buff[0] == 0 && buff[1] == 0){
-        linedata = -255;
+        linedata = prelinedata;
     }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);
-    }
-}
+}
\ No newline at end of file