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
localization.cpp
00001 #include "localization.h" 00002 00003 Localization::Localization() 00004 { 00005 wheelDia = wheel_dia; 00006 trackWidth = track_width; 00007 00008 for(int i=0; i<2; i++) { 00009 prevRevolutions[i]=0; 00010 position[i]=0; 00011 } 00012 } 00013 00014 void Localization::updatePosition(float heading, float wheel_revolutions[2]) 00015 { 00016 incrementalDistance = M_PI * wheelDia * ( (wheel_revolutions[0] - prevRevolutions[0]) + (wheel_revolutions[1] - prevRevolutions[1]) ) / 2.0; 00017 00018 position[0] += cos(heading + M_PI/2) * incrementalDistance; 00019 position[1] += sin(heading + M_PI/2) * incrementalDistance; 00020 00021 prevRevolutions[0] = wheel_revolutions[0]; 00022 prevRevolutions[1] = wheel_revolutions[1]; 00023 } 00024 00025 void Localization::setPosition(float position_in[2]) //[x,y] position 00026 { 00027 position[0] = position_in[0]; 00028 position[1] = position_in[1]; 00029 }
Generated on Tue Jul 12 2022 20:53:07 by
1.7.2