Diff: main.cpp

Revision:

7:fd976e1ced33

Parent:

6:5a39bde2e016

Child:

8:8e49e21d80a2

--- a/main.cpp	Wed Jun 11 14:41:52 2014 +0000
+++ b/main.cpp	Sun Jun 22 13:58:01 2014 +0000
@@ -1,13 +1,17 @@
 #include "mbed.h"
+#include "controller.h"
 #include "servo_api.h"
 #include "camera_api.h"
 #include "motor_api.h"
+#include "pot.h"
+
 
 #define Debug_cam_uart
 #define R_eye
-//#define motor_on 
+#define motor_on 
 #define Pcontroller
-//#define servo_center
+//#define task_ma_time
+
 Serial pc(USBTX, USBRX);
 
 
@@ -18,104 +22,130 @@
 BX_motor MotorA('A');
 BX_motor MotorB('B');
 
+BX_pot pot1('1');
+
+                // 90/30=3
+//PID cam_to_M_ctrlr(3.0,0.0,0.0,10);
+
+//DigitalOut task_pin(PTD1);
 
 int main() {
     
-  /*  
-    int black_va;
-    int white_va;
-    */
-      
-    #ifdef servo_center
- 
-    
-       while(1)
-    servo.set_angle(0);
-    #endif
-
-      pc.baud(115200);
-     
-  #ifdef Pcontroller  
      
      
-     int car_center=30;
+     pc.baud(115200);
      
-     double Kp= 90.0/64.0;
      
-     #ifdef motor_on 
-     double motor=0.3;
-    
-    
-  
-     MotorA.rotate(motor);
-     MotorB.rotate(motor);
-    #endif
-     int error=0;
-     
+      double motor;
+     int b_r_c;
      
      while(1){
          
+        #ifdef task_ma_time
+             task_pin=1;
+        #endif 
+         
+         
+         
         cam.read();  
      
-    // #ifdef Debug_cam_uart
-     
+#ifdef Debug_cam_uart
+      
       #ifdef L_eye  
         for(int i=0;i<128;i++){
+
+              
+              
+              
              if(i==64)
                pc.printf("X");
              else          
                pc.printf("%c", cam.sign_line_imageL[i]);
          }
          pc.printf("           ||             ");
-         #endif
-      //   #ifdef R_eye
-         for(int i=0;i<128;i++){
+      #endif
+      #ifdef R_eye
+        for(int i=128;i>=0;i--){
              if(i==64)
                pc.printf("X");
+             
+             else if(i<10)
+               pc.printf("-");
+               
+             else if(i>117)
+
+
+              pc.printf("-");
+             
              else          
                pc.printf("%c", cam.sign_line_imageR[i]);
+         
+           
+            
+            
+               
+         
          }
          pc.printf("\r\n");
-     //    #endif
          
          
        
      
+     #endif
+         
+         
      
+            
      
      
   
      
    
 
-      //   #endif   
-     
-     
+#endif   
      
-     
-     
-     
+      
      
      
      
      
      
-       
-        
-        error=car_center-cam.black_centerR();
+       b_r_c=cam.black_centerR();
      
      
-         
+       
+         #ifdef motor_on 
+           
+     
+             motor=pot1.read();
      
-        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);
+  
+             MotorA.rotate(motor);
+             MotorB.rotate(motor);
+        #endif
+       
+       
+       
+       
+       
+        
+     
+         pc.printf("speed :%f  bk_center %d \r\n",motor,b_r_c);
+       
+        //servo.set_angle(cam_to_M_ctrlr.compute(b_r_c,30.0));
+       
+     
+      #ifdef task_ma_time
+        task_pin=0;
+        #endif 
+        
+     }
+   
      
      
-     }
-   #endif   
-     
-   
+