From 3dd0bb9163380de64d0df4f0b5c16dd86979714e Mon Sep 17 00:00:00 2001 From: Jacob Palecki Date: Thu, 20 Aug 2020 00:32:11 -0700 Subject: Sigmoid gain --- common/accel-sigmoidgain.hpp | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 common/accel-sigmoidgain.hpp (limited to 'common/accel-sigmoidgain.hpp') diff --git a/common/accel-sigmoidgain.hpp b/common/accel-sigmoidgain.hpp new file mode 100644 index 0000000..0a2e54d --- /dev/null +++ b/common/accel-sigmoidgain.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include + +#include "accel-base.hpp" + +namespace rawaccel { + + /// Struct to hold sigmoid (s-shaped) gain implementation. + struct accel_sigmoidgain : accel_base { + double limit = 1; + double midpoint = 0; + double additive_constant = 0; + double integration_constant = 0; + + accel_sigmoidgain(const accel_args& args) : accel_base(args) { + verify(args); + + limit = args.limit - 1; + midpoint = args.midpoint; + additive_constant = exp(speed_coeff*midpoint); + integration_constant = log(1 + additive_constant); + } + + inline double accelerate(double speed) const { + //f(x) = k/(1+e^(-m(c-x))) + double scaled_speed = speed_coeff * speed; + return limit * ((log(additive_constant+exp(scaled_speed)) - integration_constant)/scaled_speed); + } + + void verify(const accel_args& args) const { + if (args.limit <= 1) bad_arg("exponent must be greater than 1"); + if (args.midpoint < 0) bad_arg("midpoint must not be negative"); + } + }; + +} -- cgit v1.2.3