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@6:690db8b5030b, 2016-02-04 (annotated)

Committer:
akashvibhute
Date:
Thu Feb 04 02:11:36 2016 +0000
Revision:
6:690db8b5030b
Parent:
5:099cb2e76c7d 
PID tuned, general code cleanup

Who changed what in which revision?

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