Diff: main.cpp

Revision:

7:f04bde0ca846

Parent:

6:b046d6ff3745

Child:

8:c2c3aee85c2d

--- a/main.cpp	Mon Jul 07 08:16:46 2014 +0000
+++ b/main.cpp	Mon Jul 07 15:23:50 2014 +0000
@@ -1,26 +1,34 @@
 #include "mbed.h"
+//#include "controller.h"
 #include "servo_api.h"
 #include "camera_api.h"
+#include "motor_api.h"
 
-#include "motor_api.h"
 #define Debug_cam_uart
 
-//middle 0.041
+//left   0.015
+//middle 0.045
+//right  0.072
 Serial pc(USBTX, USBRX);
 BX_servo servo; 
 BX_camera cam;
 BX_motor MotorA('A');
 BX_motor MotorB('B');
 
+//PID cam_to_M_ctrlr(10.0, 118.0, 0.06, 0.11, (0.104/30), 0.0, 0.0, 10);
 
 
 int main() {
-    
   /*  
     int black_va;
     int white_va;
     */
-    double Kp = 0.0;
+    double left = 0.016, middle = 0.045, right = 0.071;
+    double error, turn, last_turn = middle;
+    double Kp = 0.000527;
+    int black_centerR, black_centerL, center;
+    
+    char psudo_line[128];
     
 #ifdef Debug_cam_uart
      pc.baud(115200);
@@ -31,8 +39,74 @@
         cam.read();
         MotorA.rotate(0.0);
         MotorB.rotate(0.0);
-        servo.set_angle(0.051);
-        /*for(int i=0;i<128;i++){
+        
+        black_centerL = cam.black_centerL();
+        black_centerR = cam.black_centerR();
+        center = (black_centerL + black_centerR) / 2;
+        
+        MotorA.rotate(0.2);
+        MotorB.rotate(0.2);
+        
+        if(black_centerL == 128 && black_centerR == 0){//no line
+            
+            turn = middle;
+            
+        } else if (black_centerL == 128 && black_centerR != 0){//no left line
+        
+            turn = (middle + left) / 2;
+            
+        
+        } else if (black_centerL != 128 && black_centerR == 0){//no right line
+            
+            turn = (middle + right) / 2;
+            
+        } else {
+            if(60 < center && center < 68){
+                
+                turn = middle;
+                
+            } else{
+                error = 64 - center;
+                turn = Kp * error + middle;
+            }
+        }
+
+        /*if(turn == middle){
+            if (last_turn < middle){
+                
+                servo.set_angle(last_turn);
+                wait_ms(500);
+                
+            } else if (last_turn > middle){
+                
+                servo.set_angle(last_turn);
+                wait_ms(500);
+            }
+            servo.set_angle(turn);
+        } else {
+            servo.set_angle(turn);
+        }
+        last_turn = turn;*/
+        
+        servo.set_angle(turn);
+        
+        
+        
+        //output the psudo map
+        for(int i = 127; i >= 0; i--)   
+            psudo_line[i] = '0';
+        
+        for(int i = center-5; i < center+5; i++)
+            psudo_line[i] = ' ';
+        
+        for(int i = 117; i > 10; i--){
+            pc.printf("%c", psudo_line[i]);    
+        }
+        pc.printf("\r\n");
+        pc.printf("black centerL: %d   black_centerR: %d   psudo_line: %d turn: %lf\r\n", black_centerL, black_centerR, center, turn);
+        
+        
+        /*for(int i = 128; i > 64;i--){
              if(i==64)
                pc.printf("X");
              else          
@@ -40,16 +114,18 @@
          }
          pc.printf("           ||             ");
          
-         for(int i=0;i<128;i++){
+         for(int i = 64; i > 0; i--){
              if(i==64)
                pc.printf("X");
              else          
                pc.printf("%c", cam.sign_line_imageR[i]);
          }
-         pc.printf("\r\n");*/
+         pc.printf("\r\n");
+         pc.printf("black_centerL: %d   black_centerR: %d\r\n", black_centerL, black_centerR);
+       */
        }
 
-     
+