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

01_DriveTrain/odometer.cpp@5:099cb2e76c7d, 2016-01-25 (annotated)

Committer:
akashvibhute
Date:
Mon Jan 25 07:28:40 2016 +0000
Revision:
5:099cb2e76c7d
Parent:
4:315716ef8178
Child:
6:690db8b5030b
all threads populated

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 5:099cb2e76c7d 42
akashvibhute 5:099cb2e76c7d 43 rpm[0] = filtered_reading[0][1];
akashvibhute 5:099cb2e76c7d 44 rpm[1] = filtered_reading[1][1];
akashvibhute 5:099cb2e76c7d 45
akashvibhute 5:099cb2e76c7d 46 revolutions[0] = filtered_reading[0][0];
akashvibhute 5:099cb2e76c7d 47 revolutions[1] = filtered_reading[1][0];
akashvibhute 2:761e3c932ce0 48 }
akashvibhute 2:761e3c932ce0 49
akashvibhute 2:761e3c932ce0 50 void odometer::getRPM(float *rpm[2])
akashvibhute 2:761e3c932ce0 51 {
akashvibhute 2:761e3c932ce0 52 *rpm[0] = filtered_reading[0][1];
akashvibhute 2:761e3c932ce0 53 *rpm[1] = filtered_reading[1][1];
akashvibhute 2:761e3c932ce0 54 }
akashvibhute 2:761e3c932ce0 55
akashvibhute 2:761e3c932ce0 56 void odometer::getRevolutions(float *revolutions[2])
akashvibhute 2:761e3c932ce0 57 {
akashvibhute 2:761e3c932ce0 58 *revolutions[0] = filtered_reading[0][0];
akashvibhute 2:761e3c932ce0 59 *revolutions[1] = filtered_reading[1][0];
akashvibhute 2:761e3c932ce0 60 }