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

Committer:
ahmed_lv
Date:
Tue Mar 20 05:56:22 2018 +0000
Revision:
30:44676e1b38f8
Parent:
11:49344285c82a 
Editted Input Variables to PID

Who changed what in which revision?

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