File content as of revision 0:b63eda8e8952:

#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 = 50; //each loop takes 50ms
    //////////width PI controller
    int w_setpoint = 100;
    float w_Kp = 15;
    float w_Ki = 5; 
    int w_e = 0;
    int w_e2 = 0;
    
        //////////x PI controller
    int x_setpoint = 160;
    float x_Kp = 1;
    float x_Ki = 0.1; 
    int x_e = 0;
    int x_e2 = 0;
    
        
    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 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;
        }
        
        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;
        }
        
        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;
        }
        
        
        left_speed = Speed - Coef;
        //left_speed = aux_Speed - aux_Coef;
        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;        
        //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);
        }
        
        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();
    }
}