estimator/estimator.cpp

Go to the documentation of this file.

#include "estimator.h"
#include "../truckd/hw_interface.h"

void EncEstimator::update(double fr, double fl, double br, double bl) {

        // If the above formuals are multiplied through by a small
        // timestep then, instead of working with velocities we use
        // diffrential for position and angle 
        
        // Convert BL,BR values from encoder ticks into meters
        fr *= hwInterface::metersPerTick;
        fl *= hwInterface::metersPerTick;
        br *= hwInterface::metersPerTick;
        bl *= hwInterface::metersPerTick;       

        // Caclulate new angle and forward movement
        double forward = (fr + fl + br + bl) * 0.5;
        double newAngle = angle + (fr + br - fl - bl) * (0.5f / hwInterface::axleLength);

        // Calculate new x,y positions
        // instead of making an arc assume half the forward movement
        // was traveled at the old distance and half the forward 
        // movement was traveled at the new angle
        // Formula :
        // xpos = xpos + cos(angle) * forward * 0.5
        //             + cos(newangle) * forward * 0.5
        double newCosValue = cos(newAngle);
        double newSinValue = sin(newAngle);             
        xpos += 0.5 * forward * (cosValue + newCosValue);
        ypos += 0.5 * forward * (sinValue + newSinValue);
   
        // sin/cos is a slow operation so remember result
        cosValue = newCosValue;
        sinValue = newSinValue;

        angle = newAngle;
}

---