#include "ControlledMotor.h" #include ControlledMotor::ControlledMotor() { this->type = Type::ControlledMotor; } 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; //(int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs); } bool ControlledMotor::Drive(float distance) { if (!driving) { targetDistance = distance; startDistance = encoder->GetDistance(); driving = true; } // else // targetDistance = encoder->GetDistance(); // encoder->RestartCountingRevolutions(); float totalDistance = encoder->GetDistance() - startDistance; Serial.printf("total distance = %f\n", totalDistance); bool completed = totalDistance > targetDistance; return completed; }