File content as of revision 3:892592b2c2ea:

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

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

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

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( Robor *r, float rPos,float rTheta,float nW,float wPos,float wTheta[2],float wRad[2],float wWid[2],float platRad){
    r.rworldTrobot[1][1] = cos(rTheta);
    r.rworldTrobot[1][2] = -sin(rTheta);
    r.rworldTrobot[1][3] = rPos(1);
    r.rworldTrobot[2][1] = sin(rTheta);
    r.rworldTrobot[2][2] = cos(rTheta);
    r.rworldTrobot[2][3] = rPos(2);
    r.rworldTrobot[3][1] = 0;
    r.rworldTrobot[3][2] = 0;
    r.rworldTrobot[3][3] = 1;        

    r.nWheels=nW;
}

void occupancy_grid_mapping(int *log_mapa, float *mapa, float d,float theta,float actual_x,float actual_y,float angle,int i,int j){
    angle = (angle+90) * (M_PI / 180.0);
    double 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);
        
    //Atualização do mapa
    if 1-(1/(1+exp(log_mapa[i][j]))) > 0.5{
        mapa[i][j] = 1;
    }
    else{
        mapa[i][j] = 0;
    }
}

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

    float angle_tot = angle * (M_PI/180.0);

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

    if (angle_tot_deg >= 0) && (angle_tot_deg < 89.99){
        PointDetected[0] = ceil(pose[0]-cos(angle_tot))/5;
        PointDetected[1] = ceil(pose[1]+sen(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]-sen(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]-sen(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]+sen(angle_tot))/5;
    }
}

void 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=deg2rad(2);
    float alpha=5; %5cm
    float l_occ=0.65;
    float l_free=-0.65;


    r=sqrt((cell_x*5-atual_x)^2 + (cell_y*5-atual_y)^2);
    phi=atan2(cell_y*5-atual_y,cell_x*5-atual_x)-theta;

    float aux_min=min(z_max,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;
    }

}

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)
{
    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]));
}