50 lines
1.5 KiB
C++
50 lines
1.5 KiB
C++
#include "ControlledMotor.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
ControlledMotor::ControlledMotor(Motor* motor, RelativeEncoder* encoder)
|
|
: Motor(), motor(motor), encoder(encoder) {
|
|
this->type = Type::ControlledMotor;
|
|
Thing* parent = motor->GetParent();
|
|
this->SetParent(parent);
|
|
}
|
|
|
|
void ControlledMotor::SetTargetVelocity(float velocity) {
|
|
this->targetVelocity = velocity;
|
|
this->rotationDirection =
|
|
(targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
|
|
}
|
|
|
|
void ControlledMotor::Update(bool recurse) {
|
|
encoder->Update(recurse);
|
|
|
|
this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
|
|
|
|
unsigned long currentTimeMs = GetTimeMs();
|
|
float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
|
|
this->lastUpdateTime = currentTimeMs;
|
|
|
|
float error = this->targetVelocity - this->actualVelocity;
|
|
float acceleration =
|
|
error * timeStep * pidP; // Just P is used at this moment
|
|
motor->SetTargetVelocity(targetVelocity +
|
|
acceleration); // or something like that
|
|
|
|
motor->Update(recurse);
|
|
}
|
|
|
|
// float ControlledMotor::GetActualVelocity() {
|
|
// return (int)rotationDirection * encoder->rotationSpeed;
|
|
// }
|
|
|
|
// 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;
|
|
// }
|
|
} // namespace RoboidControl
|