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
Diff: 02_Localization/localization.cpp
- Revision:
- 2:761e3c932ce0
- Child:
- 5:099cb2e76c7d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/02_Localization/localization.cpp Mon Jan 18 06:00:43 2016 +0000
@@ -0,0 +1,29 @@
+#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];
+}
\ No newline at end of file