File content as of revision 4:1defb279922a:

#include <math.h>
#include "Robot.h"
#include "mbed.h"
#include "control.h"
#include "I2C.h"

I2C i2c(PB_9, PB_8);
const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).

const int16_t COUNTS_PER_ROTATION = 1440;   // ticks dos encoders por volta do motor
const int16_t WHEEL_DIAMETER = 7;           // diâmetro das rodas, em centímetros
const int16_t WHEEL_DISTANCE = 14;
const float MAX_LIN_VEL = 20;
const float MIN_LIN_VEL = -20;
const float MIN_SET_SPEEDS = -100;
const float MAX_SET_SPEEDS = 100;

int16_t countsLeft = 0;
int16_t countsRight = 0;

float poseX = 0;
float poseY = 50;
float poseTheta = 0;

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

float mapSpeeds(float value){
    value = (value - MIN_LIN_VEL) * (MAX_SET_SPEEDS - MIN_SET_SPEEDS)/(MAX_LIN_VEL - MIN_LIN_VEL) + MIN_SET_SPEEDS;

    if(value < 35 && value > 0)
        value = 35;
    if(value > -35 && value < 0)
        value = -35;
    if(value > 150)
        value = 150;
    if(value < -150)
        value = -150;

    return value;
}

void updatePose(){
    //printf("início do updatePose; ");
    getCountsAndReset();

    float Dl = (2 * 3.14 * countsLeft) * (WHEEL_DIAMETER / 2) / COUNTS_PER_ROTATION;

    float Dr = (2 * 3.14 * countsRight) * (WHEEL_DIAMETER / 2) / COUNTS_PER_ROTATION;

    float D = (Dl + Dr) / 2;

    float deltaTheta = (Dr - Dl) / WHEEL_DISTANCE;

    poseX = poseX + D * cos(poseTheta + deltaTheta / 2);
    poseY = poseY + D * sin(poseTheta + deltaTheta / 2);
    poseTheta = poseTheta + deltaTheta;
    poseTheta = atan2(sin(poseTheta), cos(poseTheta));
    //printf("fim do updatePose; ");
}

void robotVel2wheelVel(float v, float w, float *wheelSpeeds){
    wheelSpeeds[0] = (v - (WHEEL_DISTANCE / 2) * w) / (WHEEL_DIAMETER / 2);
    wheelSpeeds[1] = (v + (WHEEL_DISTANCE / 2) * w) / (WHEEL_DIAMETER / 2);
}