Rauno U
/
Miisu
Six crescent shaped legs
PIDController.cpp
- Committer:
- sim642
- Date:
- 2016-06-21
- Revision:
- 47:4f418a4b0051
- Parent:
- 13:dc5f5f19e4e8
File content as of revision 47:4f418a4b0051:
#include "PIDController.hpp" PIDController::PIDController(PIDData nData) : data(nData) { reset(); } PIDController::~PIDController() { } void PIDController::reset() { timer.reset(); timer.start(); prev_error = 0; integral = 0; } float PIDController::step(float error) { float dt = timer.read(); timer.reset(); // timer keeps running integral += error * dt; float derivative = (error - prev_error) / dt; float out = data.Kp * error + data.Ki * integral + data.Kd * derivative; prev_error = error; return out; } void PIDController::set(PIDData nData) { data = nData; }