Diff: SLIP_ACCELERATION.cpp

Revision:

0:4d4bddd95444

Child:

1:436de13b3320

diff -r 000000000000 -r 4d4bddd95444 SLIP_ACCELERATION.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SLIP_ACCELERATION.cpp	Thu Jan 05 11:15:13 2017 +0000
@@ -0,0 +1,100 @@
+#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
+    }
+}