39 lines
1.2 KiB
C++
39 lines
1.2 KiB
C++
#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;
|
|
} |