Revision 8:8e49e21d80a2, committed 2014-06-22

Comitter:

backman

Date:

Sun Jun 22 15:29:20 2014 +0000

Parent:

7:fd976e1ced33

Child:

9:33b99cb45e99

Commit message:

same work

Changed in this revision

--- a/camera_api.cpp	Sun Jun 22 13:58:01 2014 +0000
+++ b/camera_api.cpp	Sun Jun 22 15:29:20 2014 +0000
@@ -125,8 +125,8 @@
     b_w=b_start-b_end;
     b2_w=b2_start-b2_end;
     
-    de_v=b_start;
-    de_v2=b_end;    
+    de_v=b2_start;
+    de_v2=b2_end;    
 
     switch(find_type) {
 
@@ -135,7 +135,7 @@
                 b2_center=(b2_end+b2_start)/2;
                 
                 
-                if(w_thr_up>b_w&&(b_center-64)<(64-b2_center))
+                if(w_thr_up>b_w&&(b_center!=0)&&(b_center-64)<(64-b2_center))
                       center=b_center;                   
                 else
                       center=b2_center;
@@ -169,7 +169,6 @@
             break;
     }
 
-    de_v=center;
     return center;
    
 }

--- a/controller.cpp	Sun Jun 22 13:58:01 2014 +0000
+++ b/controller.cpp	Sun Jun 22 15:29:20 2014 +0000
@@ -10,7 +10,7 @@
     //Default the limits to the full range of I/O.
     //Make sure to set these to more appropriate limits for your application.
     setInputLimits(10.0, 118.0);
-    setOutputLimits(-1.0,1.0);
+    setOutputLimits(0.0,1.0);
  
     tSample_ = interval;
  
@@ -208,7 +208,7 @@
     }
  
     float error = scaledSP - scaledPV;
- 
+
     //Check and see if the output is pegged at a limit and only
     //integrate if it is not. This is to prevent reset-windup.
     if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
@@ -230,8 +230,8 @@
     //controllerOutput_ = Kc_ * error + tauR_ * accError_ + tauD_ * dMeas;
  
     //Make sure the computed output is within output constraints.
-    if (controllerOutput_ < 0.0) {
-        controllerOutput_ = 0.0;
+    if (controllerOutput_ < -1.0) {
+        controllerOutput_ = -1.0;
     }
     else if (controllerOutput_ > 1.0) {
         controllerOutput_ = 1.0;
@@ -241,9 +241,10 @@
     prevControllerOutput_ = controllerOutput_;
     //Remember the input for the derivative calculation next time.
     prevProcessVariable_ = scaledPV;
- 
+    
+     
     //Scale the output from percent span back out to a real world number.
-    return ((controllerOutput_ * outSpan_) + outMin_);
+    return (controllerOutput_ * 90);
  
 }
  

--- a/controller.h	Sun Jun 22 13:58:01 2014 +0000
+++ b/controller.h	Sun Jun 22 15:29:20 2014 +0000
@@ -6,6 +6,9 @@
 {
 public:
  
+   float  de_v;
+ 
+ 
     /*
     * Constructeur
     * Sets default limits, calculates tuning parameters, and sets manual mode with no bias.

--- 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