Diff: main.cpp

Revision:

6:5a39bde2e016

Parent:

3:c5f2281b3ed2

Child:

7:fd976e1ced33

--- a/main.cpp	Thu Jun 05 10:32:55 2014 +0000
+++ b/main.cpp	Wed Jun 11 14:41:52 2014 +0000
@@ -1,16 +1,22 @@
 #include "mbed.h"
 #include "servo_api.h"
 #include "camera_api.h"
-
+#include "motor_api.h"
 
 #define Debug_cam_uart
+#define R_eye
+//#define motor_on 
+#define Pcontroller
+//#define servo_center
+Serial pc(USBTX, USBRX);
 
 
-Serial pc(USBTX, USBRX);
 BX_servo servo; 
- 
-    BX_camera cam;
+
+BX_camera cam;
 
+BX_motor MotorA('A');
+BX_motor MotorB('B');
 
 
 int main() {
@@ -19,15 +25,41 @@
     int black_va;
     int white_va;
     */
+      
+    #ifdef servo_center
+ 
     
-#ifdef Debug_cam_uart
-     pc.baud(115200);
+       while(1)
+    servo.set_angle(0);
+    #endif
+
+      pc.baud(115200);
+     
+  #ifdef Pcontroller  
      
      
-  while(1){   
+     int car_center=30;
+     
+     double Kp= 90.0/64.0;
      
-      cam.read();
+     #ifdef motor_on 
+     double motor=0.3;
+    
+    
+  
+     MotorA.rotate(motor);
+     MotorB.rotate(motor);
+    #endif
+     int error=0;
+     
+     
+     while(1){
          
+        cam.read();  
+     
+    // #ifdef Debug_cam_uart
+     
+      #ifdef L_eye  
         for(int i=0;i<128;i++){
              if(i==64)
                pc.printf("X");
@@ -35,7 +67,8 @@
                pc.printf("%c", cam.sign_line_imageL[i]);
          }
          pc.printf("           ||             ");
-         
+         #endif
+      //   #ifdef R_eye
          for(int i=0;i<128;i++){
              if(i==64)
                pc.printf("X");
@@ -43,34 +76,19 @@
                pc.printf("%c", cam.sign_line_imageR[i]);
          }
          pc.printf("\r\n");
-     
-
-
-        pc.printf("R center : %d  \r\nL center: %d\r\n",cam.black_centerR(),cam.black_centerL());
-
-
-       }
-
-     
-     
+     //    #endif
+         
+         
+       
      
      
      
      
+  
      
-        //   pc.printf("ang :%d\r\n ",( (64.0-center) /64.0  )*90);
-   //--------------------------------------------            
-
-
-         // servo.set_angle(( (64.0-center) /64.0  )*90 );
+   
 
-       
-         
-         
-     
-     
-  #endif   
-       
+      //   #endif   
      
      
      
@@ -82,7 +100,22 @@
      
      
      
+       
+        
+        error=car_center-cam.black_centerR();
+     
+     
+         
+     
+        servo.set_angle((int)Kp*error);
     
+        pc.printf("b1s: %d b1e:%d b_center %d  error :%d angle %d\r\n",cam.debugV,cam.debugV2,cam.black_centerR(),error,(int)Kp*error);
+     
+     
+     }
+   #endif   
+     
+