68 lines
2.4 KiB
C++
68 lines
2.4 KiB
C++
#include <eqpalg/utility/entCentExit.h>
|
|
#include <functional>
|
|
|
|
using std::map;
|
|
extern const int TRUEMAXTIME;
|
|
std::function<bool(double, double, double)> Speed_is_stable =
|
|
[](double a, double b, double c) { return std::fabs(a - b) < c; };
|
|
void stableDetector(double v1, double v2, double th, int& times,
|
|
bool& isStable) {
|
|
if (!isStable) {
|
|
if (Speed_is_stable(v1, v2, th)) {
|
|
if (times < TRUEMAXTIME) {
|
|
times++;
|
|
} else {
|
|
isStable = true;
|
|
}
|
|
} else {
|
|
times = 0;
|
|
}
|
|
} else {
|
|
if (!Speed_is_stable(v1, v2, th)) {
|
|
times = 0;
|
|
isStable = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
void entCentExit::statusDetection() {
|
|
double centSpeedSet =
|
|
SingletonTemplate<GlobaltemSharedMemory>::GetInstance()[prefix +
|
|
cent_speed_set];
|
|
double centSpeedAct =
|
|
SingletonTemplate<GlobaltemSharedMemory>::GetInstance()[prefix +
|
|
cent_speed_act];
|
|
double entSpeed =
|
|
SingletonTemplate<GlobaltemSharedMemory>::GetInstance()[prefix +
|
|
ent_speed_act];
|
|
double exitSpeed =
|
|
SingletonTemplate<GlobaltemSharedMemory>::GetInstance()[prefix +
|
|
exit_speed_act];
|
|
//中央速度平稳性
|
|
stableDetector(centSpeedSet, centSpeedAct, 1, cent_times_OK_,
|
|
cent_speed_stable_);
|
|
//入口速度平稳性
|
|
stableDetector(entSpeed, centSpeedAct, 2, ent_times_OK_,
|
|
ent_speed_stable_);
|
|
//出口速度平稳性
|
|
stableDetector(exitSpeed, centSpeedAct, 2, exit_times_OK_,
|
|
exit_speed_stable_);
|
|
|
|
if (ECEqueue_.size() < TRUEMAXTIME) {
|
|
ECEqueue_.push(ECELabel(cent_speed_stable_, ent_speed_stable_, exit_speed_stable_));
|
|
} else {
|
|
auto ihd_ece = ECEqueue_.front();
|
|
cent_speed_stable_last_ = ihd_ece.cent_speed_stable;
|
|
ent_speed_stable_last_ = ihd_ece.ent_speed_stable;
|
|
exit_speed_stable_last_ = ihd_ece.exit_speed_stable;
|
|
ECEqueue_.pop();
|
|
}
|
|
|
|
// auto nowTime = std::chrono::system_clock::now();
|
|
// if (nowTime - before_five_seconds_ > 5s) {
|
|
// before_five_seconds_ = nowTime;
|
|
// bool cent_speed_stable_last_ = cent_speed_stable_;
|
|
// bool ent_speed_stable_last_ = ent_speed_stable_;
|
|
// bool exit_speed_stable_last_ = exit_speed_stable_;
|
|
// }
|
|
} |