odometer.cpp

00001 #include "odometer.h"
00002 
00003 odometer::odometer():encLeft(enc_LA, enc_LB, NC, encoder_resolution, QEI::X4_ENCODING), encRight(enc_RB, enc_RA, NC, encoder_resolution, QEI::X4_ENCODING)
00004 {
00005     
00006 }
00007 
00008 void odometer::init()
00009 {
00010     uint16_t movWindow_size = encoder_MWindowSize;
00011     
00012     
00013     movWindow_index=0;
00014     if(movWindow_size <= movWindow_lenMax) movWindow_len = movWindow_size;
00015     else movWindow_len=movWindow_lenMax;
00016 
00017     for(int i=0; i<movWindow_len; i++) {
00018         rpmWindow_left[i]=0;
00019         rpmWindow_right[i]=0;
00020     }
00021     
00022     
00023     encRes = 4.0 * (float)encoder_resolution;
00024     enc_timer.start();
00025 }
00026 
00027 void odometer::update()
00028 {
00029     timer_s = enc_timer.read();
00030     enc_timer.reset();
00031     
00032     pulse_counter[0][1] = encLeft.getPulses();
00033     pulse_counter[1][1] = encRight.getPulses();
00034     
00035     filtered_reading[0][0] = pulse_counter[0][1]/encRes;
00036     filtered_reading[1][0] = pulse_counter[1][1]/encRes;
00037     
00038     rpmWindow_left[movWindow_index] = (pulse_counter[0][1] - pulse_counter[0][0])/(encRes * timer_s / 60);
00039     rpmWindow_right[movWindow_index] = (pulse_counter[1][1] - pulse_counter[1][0])/(encRes * timer_s / 60);
00040     
00041     filtered_reading[0][1] = generalFunctions::moving_window(rpmWindow_left, movWindow_len);
00042     filtered_reading[1][1] = generalFunctions::moving_window(rpmWindow_right, movWindow_len);
00043     
00044     pulse_counter[0][0] = pulse_counter[0][1];
00045     pulse_counter[1][0] = pulse_counter[1][1];
00046     
00047     movWindow_index++;
00048     if(movWindow_index >= movWindow_len) movWindow_index=0;
00049     
00050     rpm[0] = filtered_reading[0][1];
00051     rpm[1] = filtered_reading[1][1];
00052     
00053     revolutions[0] = filtered_reading[0][0];
00054     revolutions[1] = filtered_reading[1][0];
00055 }
00056 
00057 /*
00058 void odometer::getRPM(float *rpm[2])
00059 {
00060     *rpm[0] = filtered_reading[0][1];
00061     *rpm[1] = filtered_reading[1][1];
00062 }
00063 
00064 void odometer::getRevolutions(float *revolutions[2])
00065 {
00066     *revolutions[0] = filtered_reading[0][0];
00067     *revolutions[1] = filtered_reading[1][0];
00068 }
00069 */