main.cpp

00001 #include "mbed.h"
00002 #include "robot.h" // Inicializa o robô. Este include deverá ser usado em todos os main.cpp!
00003 #include "Pixy.h"
00004 
00005 Pixy pixy(Pixy::I2C,PTC11,PTC10);
00006 Timer t;
00007 
00008 int main() {
00009     initRobot();
00010     pixy.setAddress(0x21); //Camera Address 0x10
00011     
00012     
00013     int Speed = 0;
00014     int aux_Speed = 0;
00015     int Coef = 0;
00016     int aux_Coef = 0;
00017     int left_speed = 0;
00018     int left_Dir = 1;
00019     int right_speed = 0;
00020     int right_Dir = 1;
00021     
00022     int period = 20; //each loop takes 20ms
00023     //////////width PI controller
00024     int w_setpoint = 150;
00025     float w_Kp = 15;
00026     float w_Ki = 0.05; 
00027     int w_e = 0;
00028     int w_e2 = 0;
00029     int w_e_threshold = 25;
00030     int max_speed = 1000;
00031     
00032         //////////x PI controller
00033     int x_setpoint = 160;
00034     float x_Kp = 3;
00035     float x_Ki = 0.02; 
00036     int x_e = 0;
00037     int x_e2 = 0;
00038     int x_e_threshold = 30;
00039     int max_speed_rotation = 2000;
00040 
00041         //////////servo PI controller
00042     int y_control=0;
00043     int y_setpoint = 100;
00044     float y_Kp = 2;
00045     float y_Ki = 0.02; 
00046     int y_e = 0;
00047     int y_e2 = 0;
00048     int y_e_threshold = 10;
00049     int y_max = 2000;
00050         
00051         
00052     q_led_red_fro = 1;  //Led Red Front
00053     
00054 
00055     uint16_t Block_width = 0, Block_x = 0, Block_y = 0;
00056     uint16_t blocks;
00057 
00058     t.start();
00059     while(1) {        
00060         if(q_led_red_fro == 1){
00061             q_led_red_fro = 0;
00062         }else{
00063             q_led_red_fro = 1;
00064         }
00065                 
00066         blocks = 0;
00067         blocks = (uint16_t) pixy.getBlocks();
00068         
00069         if (blocks > 0){
00070             Block_width = pixy.blocks[0].width;
00071             Block_x = pixy.blocks[0].x;
00072             Block_y = pixy.blocks[0].y;
00073         
00074             //Distance/Speed Control
00075             w_e2 = (w_setpoint - Block_width) - w_e;
00076             w_e = w_setpoint - Block_width;
00077             Speed = Speed+ int(w_Kp*w_e2) + int ((w_Ki*w_e*period));
00078             aux_Speed = Speed;
00079             if (abs(Speed) >max_speed)
00080             { 
00081                 Speed = (int) max_speed *Speed/abs(Speed) ;
00082             }else if (abs(w_e) <w_e_threshold)
00083             {
00084                 Speed = 0;
00085             }
00086             
00087             //x Control
00088             x_e2 = (x_setpoint - Block_x) - x_e;
00089             x_e = x_setpoint - Block_x;
00090             Coef = Coef+ int(x_Kp*x_e2) + int ((x_Ki*x_e*period));
00091             aux_Coef = Coef;
00092             if (abs(Coef) >max_speed_rotation)
00093             { 
00094                 Coef =(int) max_speed_rotation*Coef/abs(Coef);
00095             }
00096             else if (abs(x_e) < x_e_threshold )
00097             {
00098                 Coef = 0;
00099             }
00100             
00101             left_speed = Speed - Coef;
00102             if (abs(left_speed) >max_speed)
00103             { 
00104                 left_speed =(int) max_speed*left_speed/abs(left_speed);
00105             }
00106             if (left_speed >0)
00107             {
00108                 left_Dir=1;
00109                 left_speed = abs(left_speed);
00110             }else{
00111                 left_Dir=0;
00112                 left_speed = abs(left_speed);
00113             }
00114             
00115             right_speed = Speed + Coef;        
00116                     if (abs(right_speed) >max_speed)
00117             { 
00118                 right_speed =(int) max_speed*right_speed/abs(right_speed);
00119             }
00120             if (right_speed >0)
00121             {
00122                 right_Dir=1;
00123                 right_speed = abs(right_speed);
00124             }else{
00125                 right_Dir=0;
00126                 right_speed = abs(right_speed);
00127             }
00128             
00129             
00130             //y Control
00131             y_e2 = (y_setpoint - Block_y) - y_e;
00132             y_e = y_setpoint - Block_y;
00133             y_control = y_control+ int(y_Kp*y_e2) + int ((y_Ki*y_e*period));
00134             if (abs(y_control) >y_max)
00135             { 
00136                 y_control =(int) y_max*y_control/abs(y_control);
00137             }
00138             
00139             pixy.setServos(0,500+y_control);
00140 
00141         
00142         }else{ //If no block received
00143             //Reset all linear speed variables
00144             w_e2 = 0;
00145             w_e = 0;
00146             Speed = 0;
00147             aux_Speed = 0;
00148             //Reset all angular speed variables
00149             x_e2 = 0;
00150             x_e = 0;
00151             Coef = 0;
00152             aux_Coef = 0;
00153             //Reset all servo speed variables
00154             y_e2 = 0;
00155             y_e = 0;
00156 
00157             
00158             
00159             left_Dir=0;
00160             left_speed = max_speed;
00161             right_Dir =1;
00162             right_speed = max_speed;
00163         }
00164         
00165         leftMotor(left_Dir,left_speed);
00166         rightMotor(right_Dir,right_speed);  
00167         
00168         while (t.read_ms () < period )
00169         { 
00170             //waste time
00171         }
00172         //pc.printf("Speed=%d , width=%d , w_e2=%d, w_e=%d \n",Speed , Block_width , w_e2 , w_e  );
00173         //pc.printf("Coef=%d , x=%d, x_e2=%d, x_e=%d  \n",Coef, Block_x , x_e2 , x_e );
00174         pc.printf("Coef=%d , x=%d, x_e2=%d, x_e=%d :: Speed=%d ,leftMotor=%d , rightMotor=%d \n",Coef, Block_x , x_e2 , x_e , Speed , left_speed , right_speed);
00175 
00176     
00177         t.reset();
00178     }
00179 }