45 lines
1.5 KiB
C++
45 lines
1.5 KiB
C++
#include "ControlledMotor.h"
|
|
#include <Arduino.h>
|
|
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;
|
|
} |