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

01_DriveTrain/odometer.cpp@4:315716ef8178, 2016-01-20 (annotated)

Committer:
akashvibhute
Date:
Wed Jan 20 04:01:37 2016 +0000
Revision:
4:315716ef8178
Parent:
2:761e3c932ce0
Child:
5:099cb2e76c7d
added BNO055 wrapper to imuHandler function

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 2:761e3c932ce0 1 #include "odometer.h"
akashvibhute 2:761e3c932ce0 2
akashvibhute 4:315716ef8178 3 odometer::odometer():encLeft(enc_LA, enc_LB, NC, encoder_resolution, QEI::X4_ENCODING), encRight(enc_RA, enc_RB, NC, encoder_resolution, QEI::X4_ENCODING)
akashvibhute 2:761e3c932ce0 4 {
akashvibhute 4:315716ef8178 5 unsigned int movWindow_size = encoder_MWindowSize;
akashvibhute 4:315716ef8178 6
akashvibhute 2:761e3c932ce0 7 movWindow_index=0;
akashvibhute 2:761e3c932ce0 8 if(movWindow_size <= movWindow_lenMax) movWindow_len = movWindow_size;
akashvibhute 2:761e3c932ce0 9 else movWindow_len=movWindow_lenMax;
akashvibhute 2:761e3c932ce0 10
akashvibhute 2:761e3c932ce0 11 for(int i=0; i<movWindow_len; i++) {
akashvibhute 2:761e3c932ce0 12 rpmWindow_left[i]=0;
akashvibhute 2:761e3c932ce0 13 rpmWindow_right[i]=0;
akashvibhute 2:761e3c932ce0 14 }
akashvibhute 2:761e3c932ce0 15
akashvibhute 4:315716ef8178 16 encRes = 4.0 * (float)encoder_resolution;
akashvibhute 2:761e3c932ce0 17 enc_timer.start();
akashvibhute 2:761e3c932ce0 18 }
akashvibhute 2:761e3c932ce0 19
akashvibhute 2:761e3c932ce0 20 void odometer::update()
akashvibhute 2:761e3c932ce0 21 {
akashvibhute 2:761e3c932ce0 22 timer_s = enc_timer.read();
akashvibhute 2:761e3c932ce0 23 enc_timer.reset();
akashvibhute 2:761e3c932ce0 24
akashvibhute 2:761e3c932ce0 25 pulse_counter[0][1] = encLeft.getPulses();
akashvibhute 2:761e3c932ce0 26 pulse_counter[1][1] = encRight.getPulses();
akashvibhute 2:761e3c932ce0 27
akashvibhute 2:761e3c932ce0 28 filtered_reading[0][0] = pulse_counter[0][1]/encRes;
akashvibhute 2:761e3c932ce0 29 filtered_reading[1][0] = pulse_counter[1][1]/encRes;
akashvibhute 2:761e3c932ce0 30
akashvibhute 2:761e3c932ce0 31 rpmWindow_left[movWindow_index] = (pulse_counter[0][1] - pulse_counter[0][0])/(encRes * timer_s / 60);
akashvibhute 2:761e3c932ce0 32 rpmWindow_right[movWindow_index] = (pulse_counter[1][1] - pulse_counter[1][0])/(encRes * timer_s / 60);
akashvibhute 2:761e3c932ce0 33
akashvibhute 2:761e3c932ce0 34 filtered_reading[0][1] = generalFunctions::moving_window(rpmWindow_left, movWindow_len);
akashvibhute 2:761e3c932ce0 35 filtered_reading[1][1] = generalFunctions::moving_window(rpmWindow_right, movWindow_len);
akashvibhute 2:761e3c932ce0 36
akashvibhute 2:761e3c932ce0 37 pulse_counter[0][0] = pulse_counter[0][1];
akashvibhute 2:761e3c932ce0 38 pulse_counter[1][0] = pulse_counter[1][1];
akashvibhute 2:761e3c932ce0 39
akashvibhute 2:761e3c932ce0 40 movWindow_index++;
akashvibhute 2:761e3c932ce0 41 if(movWindow_index >= movWindow_len) movWindow_index=0;
akashvibhute 2:761e3c932ce0 42 }
akashvibhute 2:761e3c932ce0 43
akashvibhute 2:761e3c932ce0 44 void odometer::getRPM(float *rpm[2])
akashvibhute 2:761e3c932ce0 45 {
akashvibhute 2:761e3c932ce0 46 *rpm[0] = filtered_reading[0][1];
akashvibhute 2:761e3c932ce0 47 *rpm[1] = filtered_reading[1][1];
akashvibhute 2:761e3c932ce0 48 }
akashvibhute 2:761e3c932ce0 49
akashvibhute 2:761e3c932ce0 50 void odometer::getRevolutions(float *revolutions[2])
akashvibhute 2:761e3c932ce0 51 {
akashvibhute 2:761e3c932ce0 52 *revolutions[0] = filtered_reading[0][0];
akashvibhute 2:761e3c932ce0 53 *revolutions[1] = filtered_reading[1][0];
akashvibhute 2:761e3c932ce0 54 }