#include "ControlledMotor.h" ControlledMotor::ControlledMotor() {} ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) { this->motor = motor; this->encoder = encoder; } void ControlledMotor::SetTargetVelocity(float velocity) { this->targetVelocity = velocity; } void ControlledMotor::Update(float timeStep) { float velocity = GetActualVelocity(); float error = targetVelocity - velocity; float acceleration = error * timeStep * pidP; // Just P is used at this moment motor->SetSpeed(targetVelocity + acceleration); // or something like that }