Chun Feng Huang / SLIP_ACCELERATION

SLIP_ACCELERATION.cpp@1:436de13b3320, 2017-01-05 (annotated)

Committer:
benson516
Date:
Thu Jan 05 15:01:46 2017 +0000
Revision:
1:436de13b3320
Parent:
0:4d4bddd95444
Child:
2:e275f3919042
Change the cut-off frequency of the low-pass filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:4d4bddd95444 1 #include "SLIP_ACCELERATION.h"
benson516 0:4d4bddd95444 2
benson516 0:4d4bddd95444 3 SLIP_ACCELERATION_2WHEEL::SLIP_ACCELERATION_2WHEEL(size_t onBoardDelay_ms_in, float samplingTime):
benson516 0:4d4bddd95444 4 Ts(samplingTime),
benson516 0:4d4bddd95444 5 onBoardDelay_ms(onBoardDelay_ms_in),
benson516 0:4d4bddd95444 6 yawAcce_cal(samplingTime),
benson516 1:436de13b3320 7 lpf_dVs(2,samplingTime,70), // 200 Hz
benson516 0:4d4bddd95444 8 hpf_dVs(2,samplingTime,0.015), // 0.015 Hz
benson516 0:4d4bddd95444 9 OnboardSignal_FIFO() // Empty
benson516 0:4d4bddd95444 10 {
benson516 0:4d4bddd95444 11 //
benson516 0:4d4bddd95444 12 n = 2; // This class is made specifically for differential-drive vehicle
benson516 0:4d4bddd95444 13 //
benson516 0:4d4bddd95444 14 zeros_2.assign(2,0.0);
benson516 0:4d4bddd95444 15
benson516 0:4d4bddd95444 16 // Parameters
benson516 0:4d4bddd95444 17 btotal = 0.194; // 0.2; // m
benson516 0:4d4bddd95444 18 br = 0.103; // btotal/2.0; // m
benson516 0:4d4bddd95444 19 bl = btotal - br; // m
benson516 0:4d4bddd95444 20 r = 0.139/2.0; // m
benson516 0:4d4bddd95444 21
benson516 0:4d4bddd95444 22 // Inputs
benson516 0:4d4bddd95444 23 acce_vehicleCenter = 0.0;
benson516 0:4d4bddd95444 24 // wheelRotationalAcce = zeros_2;
benson516 0:4d4bddd95444 25 acce_wheelSpeed = zeros_2;
benson516 0:4d4bddd95444 26 yawRate = 0.0;
benson516 0:4d4bddd95444 27
benson516 0:4d4bddd95444 28 // States
benson516 0:4d4bddd95444 29 yawAcce = 0.0;
benson516 0:4d4bddd95444 30 acce_wheelCenter = zeros_2;
benson516 0:4d4bddd95444 31 acce_wheelCenter_delay = zeros_2;
benson516 0:4d4bddd95444 32
benson516 0:4d4bddd95444 33 // Results
benson516 0:4d4bddd95444 34 dVs = zeros_2;
benson516 0:4d4bddd95444 35
benson516 0:4d4bddd95444 36
benson516 0:4d4bddd95444 37 // Initialize the queue
benson516 0:4d4bddd95444 38 for (size_t i = 0; i < onBoardDelay_ms; ++i){
benson516 0:4d4bddd95444 39 OnboardSignal_FIFO.push(zeros_2);
benson516 0:4d4bddd95444 40 }
benson516 0:4d4bddd95444 41 }
benson516 0:4d4bddd95444 42 vector<float> SLIP_ACCELERATION_2WHEEL::iterateOnce(void){ // Calculate dVs
benson516 0:4d4bddd95444 43
benson516 0:4d4bddd95444 44
benson516 0:4d4bddd95444 45 // Claculate the latest signal
benson516 0:4d4bddd95444 46 acce_wheelCenter_cal();
benson516 0:4d4bddd95444 47
benson516 0:4d4bddd95444 48 // Pushing queue (On-board signals need to be delayed)
benson516 0:4d4bddd95444 49 queueOp(acce_wheelCenter_delay,acce_wheelCenter);
benson516 0:4d4bddd95444 50
benson516 0:4d4bddd95444 51 // Calculate the dVs
benson516 0:4d4bddd95444 52 for (size_t i = 0; i < n; ++i){
benson516 0:4d4bddd95444 53 // acce_wheelSpeed[i] = r*wheelRotationalAcce[i];
benson516 0:4d4bddd95444 54 dVs[i] = acce_wheelSpeed[i] - acce_wheelCenter_delay[i];
benson516 0:4d4bddd95444 55 }
benson516 0:4d4bddd95444 56
benson516 0:4d4bddd95444 57 // Filtering the dVs
benson516 0:4d4bddd95444 58 // dVs_filtered = dVs; // Nothing to do
benson516 0:4d4bddd95444 59 // dVs_filtered = lpf_dVs.filter(dVs); // Low-pass: 0.0 Hz ~ 200 Hz
benson516 0:4d4bddd95444 60 dVs_filtered = hpf_dVs.filter(lpf_dVs.filter(dVs)); // Band-pass: 0.015 Hz ~ 200 Hz
benson516 0:4d4bddd95444 61
benson516 0:4d4bddd95444 62 //
benson516 0:4d4bddd95444 63 // return dVs;
benson516 0:4d4bddd95444 64 return dVs_filtered;
benson516 0:4d4bddd95444 65 }
benson516 0:4d4bddd95444 66 //
benson516 0:4d4bddd95444 67 void SLIP_ACCELERATION_2WHEEL::acce_wheelCenter_cal(void){ // Calculate the linear acceleration at each wheel center
benson516 0:4d4bddd95444 68 // Calculate the yawAcce
benson516 0:4d4bddd95444 69 yawAcce = yawAcce_cal.filter(yawRate);
benson516 0:4d4bddd95444 70
benson516 0:4d4bddd95444 71 // Calculate the linear acceleration at each wheel center
benson516 0:4d4bddd95444 72 // acce_wheelCenter[0] = acce_vehicleCenter; // Right
benson516 0:4d4bddd95444 73 // acce_wheelCenter[1] = acce_vehicleCenter; // Left
benson516 0:4d4bddd95444 74 //
benson516 0:4d4bddd95444 75 acce_wheelCenter[0] = acce_vehicleCenter + yawAcce*br; // Right
benson516 0:4d4bddd95444 76 acce_wheelCenter[1] = acce_vehicleCenter - yawAcce*br; // Left
benson516 0:4d4bddd95444 77 }
benson516 0:4d4bddd95444 78 //
benson516 0:4d4bddd95444 79 void SLIP_ACCELERATION_2WHEEL::queueOp(vector<float> &signal_delayed, const vector<float> &signal_in){ // The operation of the queue
benson516 0:4d4bddd95444 80
benson516 0:4d4bddd95444 81 // Push into queue first to prevent the "empty" error when onBoardDelay_ms is zero
benson516 0:4d4bddd95444 82 OnboardSignal_FIFO.push(signal_in);
benson516 0:4d4bddd95444 83
benson516 0:4d4bddd95444 84 //----------------------------------------------------------//
benson516 0:4d4bddd95444 85
benson516 0:4d4bddd95444 86 // Get the delayed signal: signal_delayed
benson516 0:4d4bddd95444 87 signal_delayed = OnboardSignal_FIFO.front();
benson516 0:4d4bddd95444 88
benson516 0:4d4bddd95444 89 // Control the buffer size
benson516 0:4d4bddd95444 90 // Currently, the size of the queue is expected to be (onBoardDelay_ms + 1)
benson516 0:4d4bddd95444 91 if (OnboardSignal_FIFO.size() <= onBoardDelay_ms){
benson516 0:4d4bddd95444 92 // nothing to do
benson516 0:4d4bddd95444 93 }else{
benson516 0:4d4bddd95444 94 //
benson516 0:4d4bddd95444 95 while(OnboardSignal_FIFO.size() > onBoardDelay_ms){
benson516 0:4d4bddd95444 96 OnboardSignal_FIFO.pop();
benson516 0:4d4bddd95444 97 }
benson516 0:4d4bddd95444 98 // Now: OnboardSignal_FIFO.size() = onBoardDelay_ms
benson516 0:4d4bddd95444 99 }
benson516 0:4d4bddd95444 100 }