Team Virgo v3 / Orion_newPCB_test_LV

Implement new controller

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-05-16
Revision:
16:9a227791998a
Parent:
11:49344285c82a

File content as of revision 16:9a227791998a:

#include "localization.h"

Localization::Localization()
{
    wheelDia = wheel_dia;
    trackWidth = track_width;

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

void Localization::updatePosition(float heading, float wheel_revolutions[2])
{
    incrementalDistance = M_PI * wheelDia * ( (wheel_revolutions[0] - prevRevolutions[0]) + (wheel_revolutions[1] - prevRevolutions[1]) ) / 2.0;

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

    prevRevolutions[0] = wheel_revolutions[0];
    prevRevolutions[1] = wheel_revolutions[1];
}

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