68 lines
2.2 KiB
C++
68 lines
2.2 KiB
C++
#include "ControlledMotor.h"
|
|
#include "LinearAlgebra/FloatSingle.h"
|
|
|
|
namespace RoboidControl {
|
|
|
|
ControlledMotor::ControlledMotor(Motor* motor,
|
|
RelativeEncoder* encoder,
|
|
Thing* parent)
|
|
: Motor(parent), motor(motor), encoder(encoder) {
|
|
this->type = Type::ControlledMotor;
|
|
//encoder->SetParent(null);
|
|
// Thing parent = motor.GetParent();
|
|
// this->SetParent(parent);
|
|
this->integral = 0;
|
|
}
|
|
|
|
void ControlledMotor::SetTargetVelocity(float velocity) {
|
|
this->targetVelocity = velocity;
|
|
this->rotationDirection =
|
|
(targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
|
|
}
|
|
|
|
void ControlledMotor::Update(bool recurse) {
|
|
unsigned long currentTimeMs = GetTimeMs();
|
|
float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
|
|
this->lastUpdateTime = currentTimeMs;
|
|
if (timeStep <= 0)
|
|
return;
|
|
|
|
// encoder->Update(false);
|
|
|
|
this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
|
|
|
|
float error = this->targetVelocity - this->actualVelocity;
|
|
float p_term = error * pidP;
|
|
this->integral += error * timeStep;
|
|
float i_term = pidI * this->integral;
|
|
float d_term = pidD * (error - this->lastError) / timeStep;
|
|
this->lastError = error;
|
|
|
|
float output = p_term + i_term + d_term;
|
|
std::cout << "target " << this->targetVelocity << " actual "
|
|
<< this->actualVelocity << " output = " << output << std::endl;
|
|
// float acceleration =
|
|
// error * timeStep * pidP; // Just P is used at this moment
|
|
// std::cout << "motor acc. " << acceleration << std::endl;
|
|
|
|
// float newTargetVelocity = motor->targetVelocity + acceleration;
|
|
output = LinearAlgebra::Float::Clamp(output, -1, 1);
|
|
motor->SetTargetVelocity(output); // or something like that
|
|
//motor->Update(false);
|
|
}
|
|
|
|
// 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
|