File content as of revision 9:d0ef39e209b7:

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


//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;

//struct Robot{
    //Robot properties
//    float worldTrobot[3][3]; 
    //Wheels properties
//    float nWheels;
//    float robotTwheels;
//    float wheels2Twheels;
//    float wheelsPoints;
//    float hWheels;
    //Platform properties
//    float platformRadius;
//    float hPlatform;
//}; 

void getPose(struct *Robot,float *pose){
    pose[0] = Robot.worldTrobot[0][2];
    pose[1] = Robot.worldTrobot[1][2];
    pose[2] = atan2(Robot.worldTrobot[1][0],Robot.worldTrobot[0][0]);
}

void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float *w){
    w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius;
    w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius;
}

void nextPose(float T,float *w,float wRobot,float *poseRobot,float wheelsRadius,float wheelsDistance,float *nextPose){
    nextPose[0] = poseRobot[0]+(T*wheelsRadius*((w[1]+w[0]/2))*cos(poseRobot[2]+(wRobot*T/2)));
    nextPose[1] = poseRobot[1]+(T*wheelsRadius*((w[1]+w[0]/2))*sin(poseRobot[2]+(wRobot*T/2)));
    nextPose[2] = poseRobot[2]+(T*wheelsRadius*(w[1]-w[0])/wheelsDistance);
}

void robot_init(struct Robot *r, float rPos_a[],float rTheta,float nW){
    r->worldTrobot[0][0] = cos(rTheta);
    r->worldTrobot[0][1] = -sin(rTheta);
    r->worldTrobot[0][2] = rPos_a[0];
    r->worldTrobot[1][0] = sin(rTheta);
    r->worldTrobot[1][1] = cos(rTheta);
    r->worldTrobot[1][2] = rPos_a[1];
    r->worldTrobot[2][0] = 0;
    r->worldTrobot[2][1] = 0;
    r->worldTrobot[2][2] = 1;        

    r->nWheels=nW;
}

int inverse_range_sensor_model(float d,float theta,float atual_x,float atual_y,float cell_x,float cell_y){
    //Variaveis
    //d têm de ser em cm
    float z_max=200; //2m
    float beta=2*(3.14159265358979323846/180.0);
    float alpha=5; //5cm
    float l_occ=0.65;
    float l_free=-0.65;
    float phi=0;
    float r = 0;
    float aux_min=0;
    
    r = sqrt((double)((pow(cell_x*5-atual_x,2) + pow(cell_y*5-atual_y,2))));
    phi=atan2(cell_y*5-atual_y,cell_x*5-atual_x)-theta;
    
    if(z_max<(d+alpha/2)){
        aux_min=z_max;
    }
    else{
        aux_min=d+alpha/2;
    }

    float odds=0;

    if (d<z_max && r>d){
        odds=l_occ;
    }
    else if (r<=d){
        odds=l_free;
    }
    else if ((r>aux_min) || (abs(phi - theta)>beta/2)){
        odds=0;
    }
    return odds;
}

void occupancy_grid_mapping(float *log_mapa[40][40], float d,float theta,float actual_x,float actual_y,float angle,int i,int j){
    angle = (angle+90) * (3.14159265358979323846 / 180.0);
    float angle_total = angle + theta;

    log_mapa[i][j] = log_mapa[i][j] + inverse_range_sensor_model(d,angle_total,actual_x,actual_y,i,j);
}

void ComputeLidarPoint(float *pose, float *PointDetected, float distance, float angle){
    float angle_tot, angle_tot_deg;
    //Angle Retification
    angle = angle-90;
    if (angle < 0){
        angle = 360+angle;
    }

    angle_tot = angle * (3.14159265358979323846/180.0);

    angle_tot = angle_tot+pose[2];
    angle_tot_deg = angle_tot * (180.0/3.14159265358979323846);

    if ((angle_tot_deg >= 0) && (angle_tot_deg < 89.99)){
        PointDetected[0] = ceil(pose[0]-cos(angle_tot))/5;
        PointDetected[1] = ceil(pose[1]+sin(angle_tot))/5;
    }
    else if((angle_tot_deg >= 90) && (angle_tot_deg < 179.99)){
        PointDetected[0] = ceil(pose[0]-cos(angle_tot))/5;
        PointDetected[1] = ceil(pose[1]-sin(angle_tot))/5;
    }
    else if((angle_tot_deg >= 180) && (angle_tot_deg < 269.99)){
        PointDetected[0] = ceil(pose[0]+cos(angle_tot))/5;
        PointDetected[1] = ceil(pose[1]-sin(angle_tot))/5;
    }
    else if((angle_tot_deg >= 270) && (angle_tot_deg < 359.99)){
        PointDetected[0] = ceil(pose[0]+cos(angle_tot))/5;
        PointDetected[1] = ceil(pose[1]+sin(angle_tot))/5;
    }
}

void setPose(Robot *r, float *pose){
    r->worldTrobot[0][0] = cos(pose[3]);
    r->worldTrobot[0][1] = -sin(pose[3]);
    r->worldTrobot[0][2] = pose[1];

    r->worldTrobot[1][0] = sin(pose[3]);
    r->worldTrobot[1][1] = cos(pose[3]);
    r->worldTrobot[1][2] = pose[2];

    r->worldTrobot[2][0] = 0;
    r->worldTrobot[2][1] = 0;
    r->worldTrobot[2][2] = 1;
}       

void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
{
    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
    char buffer[5];
    I2C i2c(I2C_SDA, I2C_SCL);
    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)
{
    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
    char buffer[3];
    I2C i2c(I2C_SDA, I2C_SCL);
    buffer[0] = 0xA2;
    memcpy(&buffer[1], &speed, sizeof(speed));

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

void setRightSpeed(int16_t speed)
{
    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
    char buffer[3];
    I2C i2c(I2C_SDA, I2C_SCL);
    buffer[0] = 0xA3;
    memcpy(&buffer[1], &speed, sizeof(speed));

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

void getCounts()
{
    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
    int16_t countsLeft = 0;
    int16_t countsRight = 0;
    char write_buffer[2];
    char read_buffer[4];
    I2C i2c(I2C_SDA, I2C_SCL);
    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()
{
    const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
    int16_t countsLeft = 0;
    int16_t countsRight = 0;
    char write_buffer[2];
    char read_buffer[4];
    I2C i2c(I2C_SDA, I2C_SCL);
    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]));
}