File content as of revision 0:4d4bddd95444:

#include "SLIP_ACCELERATION.h"

SLIP_ACCELERATION_2WHEEL::SLIP_ACCELERATION_2WHEEL(size_t onBoardDelay_ms_in, float samplingTime):
        Ts(samplingTime),
        onBoardDelay_ms(onBoardDelay_ms_in),
        yawAcce_cal(samplingTime),
        lpf_dVs(2,samplingTime,200), // 200 Hz
        hpf_dVs(2,samplingTime,0.015), // 0.015 Hz
        OnboardSignal_FIFO() // Empty
{
    //
    n = 2; // This class is made specifically for differential-drive vehicle
    //
    zeros_2.assign(2,0.0);

    // Parameters
    btotal = 0.194; // 0.2; // m
    br = 0.103; // btotal/2.0; // m
    bl = btotal - br; // m
    r = 0.139/2.0; // m

    // Inputs
    acce_vehicleCenter = 0.0;
    // wheelRotationalAcce = zeros_2;
    acce_wheelSpeed = zeros_2;
    yawRate = 0.0;

    // States
    yawAcce = 0.0;
    acce_wheelCenter = zeros_2;
    acce_wheelCenter_delay = zeros_2;

    // Results
    dVs = zeros_2;


    // Initialize the queue
    for (size_t i = 0; i < onBoardDelay_ms; ++i){
        OnboardSignal_FIFO.push(zeros_2);
    }
}
vector<float> SLIP_ACCELERATION_2WHEEL::iterateOnce(void){ // Calculate dVs


    // Claculate the latest signal
    acce_wheelCenter_cal();

    // Pushing queue (On-board signals need to be delayed)
    queueOp(acce_wheelCenter_delay,acce_wheelCenter);

    // Calculate the dVs
    for (size_t i = 0; i < n; ++i){
        // acce_wheelSpeed[i] = r*wheelRotationalAcce[i];
        dVs[i] = acce_wheelSpeed[i] - acce_wheelCenter_delay[i];
    }

    // Filtering the dVs
    // dVs_filtered = dVs; // Nothing to do
    // dVs_filtered = lpf_dVs.filter(dVs); // Low-pass: 0.0 Hz ~ 200 Hz
    dVs_filtered = hpf_dVs.filter(lpf_dVs.filter(dVs)); // Band-pass: 0.015 Hz ~ 200 Hz

    //
    // return dVs;
    return dVs_filtered;
}
//
void SLIP_ACCELERATION_2WHEEL::acce_wheelCenter_cal(void){ // Calculate the linear acceleration at each wheel center
    // Calculate the yawAcce
    yawAcce = yawAcce_cal.filter(yawRate);

    // Calculate the linear acceleration at each wheel center
    // acce_wheelCenter[0] = acce_vehicleCenter; // Right
    // acce_wheelCenter[1] = acce_vehicleCenter; // Left
    //
    acce_wheelCenter[0] = acce_vehicleCenter + yawAcce*br; // Right
    acce_wheelCenter[1] = acce_vehicleCenter - yawAcce*br; // Left
}
//
void SLIP_ACCELERATION_2WHEEL::queueOp(vector<float> &signal_delayed, const vector<float> &signal_in){ // The operation of the queue

    // Push into queue first to prevent the "empty" error when onBoardDelay_ms is zero
    OnboardSignal_FIFO.push(signal_in);

    //----------------------------------------------------------//

    // Get the delayed signal: signal_delayed
    signal_delayed = OnboardSignal_FIFO.front();

    // Control the buffer size
    // Currently, the size of the queue is expected to be (onBoardDelay_ms + 1)
    if (OnboardSignal_FIFO.size() <= onBoardDelay_ms){
        // nothing to do
    }else{
        //
        while(OnboardSignal_FIFO.size() > onBoardDelay_ms){
            OnboardSignal_FIFO.pop();
        }
        // Now: OnboardSignal_FIFO.size() = onBoardDelay_ms
    }
}