Team Virgo v3 / Orion_newPCB_test_LV

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

02_Localization/localization.cpp

Committer:
akashvibhute
Date:
2016-01-18
Revision:
2:761e3c932ce0
Child:
5:099cb2e76c7d

File content as of revision 2:761e3c932ce0:

#include "localization.h"

localization::localization(float wheel_dia, float track_width)
{
    wheelDia = wheel_dia;
    trackWidth = track_width;

    for(int i=0; i<2; i++) {
        prevRevolutions[i]=0;
        position[i]=0;
    }
}

void localization::getPosition(float *position_out[2], float heading, float revolutions[2])
{
    incrementalDistance = M_PI * wheelDia * ( (revolutions[0] - prevRevolutions[0]) + (revolutions[1] - prevRevolutions[1]) ) / 2.0;

    position[0] += cos(heading + M_PI/2) * incrementalDistance;
    position[1] += sin(heading + M_PI/2) * incrementalDistance;

    *position_out[0] = position[0];
    *position_out[1] = position[1];
}

void localization::setPosition(float position_in[2]) //[x,y] position
{
    position[0] = position_in[0];
    position[1] = position_in[1];
}