#include "ControlledMotor.h" ControlledMotor::ControlledMotor() { this->type = Thing::ControlledMotorType; } ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder) : ControlledMotor() { this->motor = motor; this->encoder = encoder; } void ControlledMotor::SetTargetSpeed(float velocity) { this->targetVelocity = velocity; this->rotationDirection = (targetVelocity < 0) ? Direction::Reverse : Direction::Forward; } void ControlledMotor::Update(float currentTimeMs) { actualVelocity = (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs); float error = targetVelocity - velocity; float timeStep = currentTimeMs - lastUpdateTime; float acceleration = error * timeStep * pidP; // Just P is used at this moment motor->SetSpeed(targetVelocity + acceleration); // or something like that this->lastUpdateTime = currentTimeMs; } float ControlledMotor::GetActualSpeed() { return actualVelocity; } bool ControlledMotor::Drive(float distance) { if (!driving) { targetDistance = distance; startDistance = encoder->GetDistance(); driving = true; } float totalDistance = encoder->GetDistance() - startDistance; bool completed = totalDistance > targetDistance; return completed; }