public double Get(double input)
{
if (!m_timer.IsRunning)
{
m_timer.Restart();
}
double velocity = m_kv*m_profile.GetVelocity(m_timer.ElapsedTicks/Stopwatch.Frequency);
double acceleration = m_ka*m_profile.GetAcceleration(m_timer.ElapsedTicks/Stopwatch.Frequency);
double correction = m_correction?.Get(m_profile.GetPosition(m_timer.ElapsedTicks/Stopwatch.Frequency) - input) ?? 0;
bool reverse = m_profile.GetPosition(m_timer.ElapsedMilliseconds/Stopwatch.Frequency) > SetPoint;
return (velocity + acceleration + correction) * (reverse ? -1 : 1);
}