Diff: main.cpp

Revision:

8:8e49e21d80a2

Parent:

7:fd976e1ced33

Child:

9:33b99cb45e99

--- a/main.cpp	Sun Jun 22 13:58:01 2014 +0000
+++ b/main.cpp	Sun Jun 22 15:29:20 2014 +0000
@@ -10,7 +10,7 @@
 #define R_eye
 #define motor_on 
 #define Pcontroller
-//#define task_ma_time
+#define task_ma_time
 
 Serial pc(USBTX, USBRX);
 
@@ -25,9 +25,9 @@
 BX_pot pot1('1');
 
                 // 90/30=3
-//PID cam_to_M_ctrlr(3.0,0.0,0.0,10);
+PID cam_to_M_ctrlr(3.0,0.0,0.0,10);
 
-//DigitalOut task_pin(PTD1);
+DigitalOut task_pin(PTD1);
 
 int main() {
     
@@ -37,8 +37,8 @@
      
      
       double motor;
-     int b_r_c;
-     
+     double b_r_c;
+     double PID_v;
      while(1){
          
         #ifdef task_ma_time
@@ -110,7 +110,6 @@
      
      
      
-       b_r_c=cam.black_centerR();
      
      
        
@@ -130,11 +129,15 @@
        
        
        
+               b_r_c=(double)cam.black_centerR();
+
+         PID_v=cam_to_M_ctrlr.compute(b_r_c,64.0);
+         pc.printf("%f %d %d speed :%f  bk_center %f PID:%f \r\n",cam_to_M_ctrlr.de_v,cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
+         
+         
+        servo.set_angle(PID_v);
         
-     
-         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