File content as of revision 2:0dd04ad0ce1b:

#include "mbed.h"
#include "robot.h" // Inicializa o robô. Este include deverá ser usado em todos os main.cpp!
#include "Pixy.h"

Pixy pixy(Pixy::I2C,PTC11,PTC10);
Timer t;

int main() {
    initRobot();
    pixy.setAddress(0x21); //Camera Address 0x10
    
    
    int Speed = 0;
    int aux_Speed = 0;
    int Coef = 0;
    int aux_Coef = 0;
    int left_speed = 0;
    int left_Dir = 1;
    int right_speed = 0;
    int right_Dir = 1;
    
    int period = 20; //each loop takes 20ms
    //////////width PI controller
    int w_setpoint = 150;
    float w_Kp = 15;
    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 = 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
    

    uint16_t Block_width = 0, Block_x = 0, Block_y = 0;
    uint16_t blocks;

    t.start();
    while(1) {        
        if(q_led_red_fro == 1){
            q_led_red_fro = 0;
        }else{
            q_led_red_fro = 1;
        }
                
        blocks = 0;
        blocks = (uint16_t) pixy.getBlocks();
        
        if (blocks > 0){
            Block_width = pixy.blocks[0].width;
            Block_x = pixy.blocks[0].x;
            Block_y = pixy.blocks[0].y;
        
            //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);

        
        }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 = max_speed;
            right_Dir =1;
            right_speed = max_speed;
        }
        
        leftMotor(left_Dir,left_speed);
        rightMotor(right_Dir,right_speed);  
        
        while (t.read_ms () < period )
        { 
            //waste time
        }
        //pc.printf("Speed=%d , width=%d , w_e2=%d, w_e=%d \n",Speed , Block_width , w_e2 , w_e  );
        //pc.printf("Coef=%d , x=%d, x_e2=%d, x_e=%d  \n",Coef, Block_x , x_e2 , x_e );
        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);

    
        t.reset();
    }
}