Diff: main.cpp

Revision:

1:a88edf25d893

Parent:

0:b63eda8e8952

--- a/main.cpp	Wed Mar 29 12:15:58 2017 +0000
+++ b/main.cpp	Wed Jul 05 09:17:14 2017 +0000
@@ -19,27 +19,40 @@
     int right_speed = 0;
     int right_Dir = 1;
     
-    int period = 50; //each loop takes 50ms
+    int period = 20; //each loop takes 20ms
     //////////width PI controller
-    int w_setpoint = 100;
+    int w_setpoint = 150;
     float w_Kp = 15;
-    float w_Ki = 5; 
+    float w_Ki = 0.05; 
     int w_e = 0;
     int w_e2 = 0;
+    int w_e_threshold = 25;
+    int max_speed = 1000;
     
         //////////x PI controller
     int x_setpoint = 160;
-    float x_Kp = 1;
-    float x_Ki = 0.1; 
+    float x_Kp = 3;
+    float x_Ki = 0.02; 
     int x_e = 0;
     int x_e2 = 0;
-    
+    int x_e_threshold = 30;
+    int max_speed_rotation = 2000;
+
+        //////////servo PI controller
+    int y_control=0;
+    int y_setpoint = 100;
+    float y_Kp = 2;
+    float y_Ki = 0.02; 
+    int y_e = 0;
+    int y_e2 = 0;
+    int y_e_threshold = 10;
+    int y_max = 2000;
+        
         
     q_led_red_fro = 1;  //Led Red Front
-    pwm_led_whi.pulsewidth_us(1000); //Led White Front
     
 
-    uint16_t Block_width = 0,Block_x = 0;
+    uint16_t Block_width = 0, Block_x = 0, Block_y = 0;
     uint16_t blocks;
 
     t.start();
@@ -56,64 +69,102 @@
         if (blocks > 0){
             Block_width = pixy.blocks[0].width;
             Block_x = pixy.blocks[0].x;
-        }
-        
-        w_e2 = (w_setpoint - Block_width) - w_e;
-        w_e = w_setpoint - Block_width;
-        Speed = Speed+ int(w_Kp*w_e2) + int ((w_Ki*w_e*period)/1000);
-        aux_Speed = Speed;
-        if (abs(Speed) >666)
-        { 
-            Speed = (int) 666 *Speed/abs(Speed) ;
-            //aux_Speed = (short int) 666 *Speed/abs(Speed) ;
-
-        }//else if (abs(Speed) <150)
-        else if (abs(w_e) <25)
-        {
-            Speed = 0;
-            //aux_Speed = 0;
-        }
+            Block_y = pixy.blocks[0].y;
         
-        x_e2 = (x_setpoint - Block_x) - x_e;
-        x_e = x_setpoint - Block_x;
-        Coef = Coef+ int(x_Kp*x_e2) + int ((x_Ki*x_e*period)/1000);
-        aux_Coef = Coef;
-        if (abs(Coef) >334)
-        { 
-            Coef =(int) 334*Coef/abs(Coef);
-            //aux_Coef =(short int) 334*Coef/abs(Coef);
-        }//else if (abs(Coef) < 20 )
-        else if (abs(x_e) < 30 )
-        {
-            Coef = 0;
-            //aux_Coef = 0;
-        }
-        
+            //Distance/Speed Control
+            w_e2 = (w_setpoint - Block_width) - w_e;
+            w_e = w_setpoint - Block_width;
+            Speed = Speed+ int(w_Kp*w_e2) + int ((w_Ki*w_e*period));
+            aux_Speed = Speed;
+            if (abs(Speed) >max_speed)
+            { 
+                Speed = (int) max_speed *Speed/abs(Speed) ;
+            }else if (abs(w_e) <w_e_threshold)
+            {
+                Speed = 0;
+            }
+            
+            //x Control
+            x_e2 = (x_setpoint - Block_x) - x_e;
+            x_e = x_setpoint - Block_x;
+            Coef = Coef+ int(x_Kp*x_e2) + int ((x_Ki*x_e*period));
+            aux_Coef = Coef;
+            if (abs(Coef) >max_speed_rotation)
+            { 
+                Coef =(int) max_speed_rotation*Coef/abs(Coef);
+            }
+            else if (abs(x_e) < x_e_threshold )
+            {
+                Coef = 0;
+            }
+            
+            left_speed = Speed - Coef;
+            if (abs(left_speed) >max_speed)
+            { 
+                left_speed =(int) max_speed*left_speed/abs(left_speed);
+            }
+            if (left_speed >0)
+            {
+                left_Dir=1;
+                left_speed = abs(left_speed);
+            }else{
+                left_Dir=0;
+                left_speed = abs(left_speed);
+            }
+            
+            right_speed = Speed + Coef;        
+                    if (abs(right_speed) >max_speed)
+            { 
+                right_speed =(int) max_speed*right_speed/abs(right_speed);
+            }
+            if (right_speed >0)
+            {
+                right_Dir=1;
+                right_speed = abs(right_speed);
+            }else{
+                right_Dir=0;
+                right_speed = abs(right_speed);
+            }
+            
+            
+            //y Control
+            y_e2 = (y_setpoint - Block_y) - y_e;
+            y_e = y_setpoint - Block_y;
+            y_control = y_control+ int(y_Kp*y_e2) + int ((y_Ki*y_e*period));
+            if (abs(y_control) >y_max)
+            { 
+                y_control =(int) y_max*y_control/abs(y_control);
+            }
+            
+            pixy.setServos(0,500+y_control);
+
         
-        left_speed = Speed - Coef;
-        //left_speed = aux_Speed - aux_Coef;
-        if (left_speed >0)
-        {
-            left_Dir=1;
-            left_speed = abs(left_speed);
-        }else{
+        }else{ //If no block received
+            //Reset all linear speed variables
+            w_e2 = 0;
+            w_e = 0;
+            Speed = 0;
+            aux_Speed = 0;
+            //Reset all angular speed variables
+            x_e2 = 0;
+            x_e = 0;
+            Coef = 0;
+            aux_Coef = 0;
+            //Reset all servo speed variables
+            y_e2 = 0;
+            y_e = 0;
+
+            
+            
             left_Dir=0;
-            left_speed = abs(left_speed);
-        }
-        
-        right_speed = Speed + Coef;        
-        //right_speed = aux_Speed + aux_Coef;
-        if (right_speed >0)
-        {
-            right_Dir=1;
-            right_speed = abs(right_speed);
-        }else{
-            right_Dir=0;
-            right_speed = abs(right_speed);
+            left_speed = max_speed;
+            right_Dir =1;
+            right_speed = max_speed;
         }
         
         leftMotor(left_Dir,left_speed);
-        rightMotor(right_Dir,right_speed);
+        rightMotor(right_Dir,right_speed);  
+        
         while (t.read_ms () < period )
         { 
             //waste time