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@2:761e3c932ce0, 2016-01-18 (annotated)

Committer:
akashvibhute
Date:
Mon Jan 18 06:00:43 2016 +0000
Revision:
2:761e3c932ce0
Child:
5:099cb2e76c7d
guess this is the initial commit :P

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 2:761e3c932ce0 1 #include "localization.h"
akashvibhute 2:761e3c932ce0 2
akashvibhute 2:761e3c932ce0 3 localization::localization(float wheel_dia, float track_width)
akashvibhute 2:761e3c932ce0 4 {
akashvibhute 2:761e3c932ce0 5 wheelDia = wheel_dia;
akashvibhute 2:761e3c932ce0 6 trackWidth = track_width;
akashvibhute 2:761e3c932ce0 7
akashvibhute 2:761e3c932ce0 8 for(int i=0; i<2; i++) {
akashvibhute 2:761e3c932ce0 9 prevRevolutions[i]=0;
akashvibhute 2:761e3c932ce0 10 position[i]=0;
akashvibhute 2:761e3c932ce0 11 }
akashvibhute 2:761e3c932ce0 12 }
akashvibhute 2:761e3c932ce0 13
akashvibhute 2:761e3c932ce0 14 void localization::getPosition(float *position_out[2], float heading, float revolutions[2])
akashvibhute 2:761e3c932ce0 15 {
akashvibhute 2:761e3c932ce0 16 incrementalDistance = M_PI * wheelDia * ( (revolutions[0] - prevRevolutions[0]) + (revolutions[1] - prevRevolutions[1]) ) / 2.0;
akashvibhute 2:761e3c932ce0 17
akashvibhute 2:761e3c932ce0 18 position[0] += cos(heading + M_PI/2) * incrementalDistance;
akashvibhute 2:761e3c932ce0 19 position[1] += sin(heading + M_PI/2) * incrementalDistance;
akashvibhute 2:761e3c932ce0 20
akashvibhute 2:761e3c932ce0 21 *position_out[0] = position[0];
akashvibhute 2:761e3c932ce0 22 *position_out[1] = position[1];
akashvibhute 2:761e3c932ce0 23 }
akashvibhute 2:761e3c932ce0 24
akashvibhute 2:761e3c932ce0 25 void localization::setPosition(float position_in[2]) //[x,y] position
akashvibhute 2:761e3c932ce0 26 {
akashvibhute 2:761e3c932ce0 27 position[0] = position_in[0];
akashvibhute 2:761e3c932ce0 28 position[1] = position_in[1];
akashvibhute 2:761e3c932ce0 29 }