File content as of revision 0:a7324f51348d:

#include "Robot.h"
#include "mbed.h"

Serial PC(SERIAL_TX, SERIAL_RX);
I2C i2c(I2C_SDA, I2C_SCL );
const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).

int16_t countsLeft = 0;
int16_t countsRight = 0;
float De=0, Dd=0, pi=3.141529, Ldis=14.2, r=6.95/2, theta=0, teta=0, x=50, y=10, D=0, Wee=0, Wdd=0, v=0, w=0;


////////////////////////////////////////
// Movimento para uma pose arbitrária //
////////////////////////////////////////
void to_pos(float x2,float y2,float phi2)
{
    while(1){
    float alfa,beta,a=33,b=30;
    float Kp=0.7,Ka=5,Kb=-2;
    while(abs(x)<abs(x2)-1 || abs(y)<abs(y2)-1 || teta<=(phi2-0.05) || teta>=(phi2+0.05)) {// Condição de paragem
        // Controlo linear
        v=Kp*(sqrt(((x2-x)*(x2-x)+(y2-y)*(y2-y))));
        
        alfa=-teta+atan2(y2-y,x2-x);
        
        if(abs(v)<1){ 
            alfa=0;
            a=0;
            b=0;
        }
        // Verifica se o alpha pertence ao limite entre [-pi/2; pi/2], se não adiciona/remove pi e inverte a velocidade
        if(alfa<-pi/2){
            alfa=alfa+pi;
            v=-v;
        }
        else if(alfa>pi/2){
            alfa=alfa-pi;
            v=-v;
        }
        
        beta=-teta-alfa+phi2;
        w=Ka*alfa+Kb*beta;
        
        if(alfa==0){
             w=w+abs(w)/w*4;
             Kb=3;
        }

        motion();
        setSpeeds(Wee+a*abs(v)/v,Wdd+b*abs(v)/v);

        //PC.printf("v=%f w=%f alfa=%f beta=%f teta=%f phi2=%f x=%f y=%f\n", v,w,alfa,beta,teta,phi2,x,y);
        PC.printf("%f %f\n", x,y);
        }
        setSpeeds(0,0); // A condição de paragem verifica-se e para-se o robo
    }
}


/////////////////////////////////
// Movimento segundo uma linha //
/////////////////////////////////
void to_line(float a,float b,float c)
{
    float d=0,alfad=0,phi1=0,alfah=0;
    float Kd=0.45,Kh=15;
    while(1){
        while((abs(x) < 200)&&(abs(y)<200)) {
            v=100;
            d=(a*x+b*y+c)/sqrt(a*a+b*b);

            //Controlo proporcional que roda a plataforma na direção da linha
            alfad=(-Kd)*d;
            if (abs(alfad)<0.3) alfad=alfad*3;
            phi1=atan2(-a,b);
            
            //Controlo proporcional que ajusta o ângulo de direcionamento
            alfah=Kh*(phi1-teta);
            
            w=alfad+alfah;
            
            if(w > 12) w = 12;
            else if(w<-12) w = -12;

            motion();
            setSpeeds(Wee,Wdd);
            //PC.printf("v=%f w=%f alfad=%f alfah=%f teta=%f x=%f y=%f\n", v,w,alfad,alfah,teta,x,y);
            PC.printf("%f %f\n", x,y);
        }
        setSpeeds(0,0);
    }
}


/////////////////////////////
// Movimento para um ponto //
/////////////////////////////
void to_point(float x1,float y1)
{
    while(1) {
        float phi1=0,aux=0;
        float Kv=1.5,Ks=30;
        while((abs(x)<abs(x1)-1)||(abs(y)<abs(y1)-1)||(abs(x)>abs(x1)+1)||(abs(y)>abs(y1)+1)) {
            v=Kv*(sqrt(((x1-x)*(x1-x)+((y1-y)*(y1-y))))); //Controlo de velocidade -- Quanto mais próximo do ponto objetivo, menor vai ser a velocidade
            
            phi1=atan2((y1-y),(x1-x)); // Ângulo entre a posição final e a posição atual
            
            aux=phi1-teta;
            //Verifica se a diferença dos angulos pertence ao limite entre [-pi;pi]
            if(aux<-pi){
                aux=aux+2*pi;
            }
            else if(aux>pi){
                aux=aux-2*pi;
            }
            
            //Controlo proporcional
            w=Ks*(aux);
            
            if(w > 12) w = 12;
            else if(w<-12) w = -12;

            motion();
            setSpeeds(Wee+28,Wdd+25);
            //PC.printf("v=%f w=%f phi1=%f x=%f y=%f teta=%f\n", v,w,phi1,x,y,teta);
            PC.printf("%f %f %f %f\n", x,y,teta, w);
        }
        setSpeeds(0,0);
    }

}

///////////////
// Odometria //
///////////////
void motion()
{
    getCountsAndReset();
    De=((2*pi*r)*countsLeft)/1440;
    Dd=((2*pi*r)*countsRight)/1440;
    theta=(Dd-De)/Ldis;
    
    D=(De+Dd)/2;
    
    if(theta==0) {
        x=x+D*cos(teta);
        y=y+D*sin(teta);
        teta=teta;
    } else {
        x = x + D*((sin(theta/2)/(theta/2))*cos(teta+theta/2));
        y = y + D*((sin(theta/2)/(theta/2))*sin(teta+theta/2));
        teta=teta+theta;
    }
    
    //Limite entre [-2pi; 2pi]
    if (teta>=(2*pi)) {
        teta=teta-2*pi;
    }
    if (teta<=(-2*pi)) {
        teta=teta+2*pi;
    }

    Wee=(v-(Ldis/2)*w);
    Wdd=(v+(Ldis/2)*w);
    //PC.printf("phi1=%f D=%f x=%f y=%f teta=%f\n", w,theta,x,y,teta);
}

void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
{
    char buffer[5];

    buffer[0] = 0xA1;
    memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
    memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));

    i2c.write(addr8bit, buffer, 5); // 5 bytes
}

void setLeftSpeed(int16_t speed)
{
    char buffer[3];

    buffer[0] = 0xA2;
    memcpy(&buffer[1], &speed, sizeof(speed));

    i2c.write(addr8bit, buffer, 3); // 3 bytes
}

void setRightSpeed(int16_t speed)
{
    char buffer[3];

    buffer[0] = 0xA3;
    memcpy(&buffer[1], &speed, sizeof(speed));

    i2c.write(addr8bit, buffer, 3); // 3 bytes
}

void getCounts()
{
    char write_buffer[2];
    char read_buffer[4];

    write_buffer[0] = 0xA0;
    i2c.write(addr8bit, write_buffer, 1);
    wait_us(100);
    i2c.read( addr8bit, read_buffer, 4);
    countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
    countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
}

void getCountsAndReset()
{
    char write_buffer[2];
    char read_buffer[4];

    write_buffer[0] = 0xA4;
    i2c.write(addr8bit, write_buffer, 1);
    wait_us(100);
    i2c.read( addr8bit, read_buffer, 4);
    countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
    countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
}