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
02_Localization/localization.cpp
- Committer:
- ahmed_lv
- Date:
- 2018-03-20
- Revision:
- 30:44676e1b38f8
- Parent:
- 16:9a227791998a
File content as of revision 30:44676e1b38f8:
#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]; }