Diff: main.cpp

Revision:

10:d2401a243e8d

Parent:

9:33b99cb45e99

Child:

11:ffd762ae141b

--- a/main.cpp	Tue Jun 24 10:06:54 2014 +0000
+++ b/main.cpp	Wed Jun 25 05:07:13 2014 +0000
@@ -27,6 +27,10 @@
 
                                  // 90/30=3
 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
+//PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval)
+//in_min in_out  camera array
+//out_min out_max servo range
+//
 
 DigitalOut task_pin(PTD1);
 TSISensor tsi;
@@ -34,7 +38,6 @@
     
     
        pc.baud(115200);
-    
      while(1){
          
        if(tsi.readPercentage()>0.00011)
@@ -139,11 +142,21 @@
        
                b_r_c=(double)cam.black_centerR();
 
-         PID_v=cam_to_M_ctrlr.compute(b_r_c,15.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);
+         //PID_v=cam_to_M_ctrlr.compute(b_r_c,15.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);
+            pc.printf("b_r_c       %lf\r\n",b_r_c);
+            int error = b_r_c-64;
+            if(error<8&&error>-8){
+                servo.set_angle(0.085);
+            }
+            else if(error<-8){
+                servo.set_angle(0.1);
+            }
+            else if(error>8){
+                 servo.set_angle(0.07);
+            }
          
-         
-        servo.set_angle(PID_v);
+        //servo.set_angle(PID_v);