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@30:44676e1b38f8, 2018-03-20 (annotated)

Committer:
ahmed_lv
Date:
Tue Mar 20 05:56:22 2018 +0000
Revision:
30:44676e1b38f8
Parent:
16:9a227791998a 
Editted Input Variables to PID

Who changed what in which revision?

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