65 lines
2.0 KiB
C++
65 lines
2.0 KiB
C++
#include "ControlledMotor.h"
|
|
|
|
ControlledMotor::ControlledMotor() {
|
|
this->type = (unsigned char)Type::ControlledMotor;
|
|
}
|
|
|
|
ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder)
|
|
: ControlledMotor() {
|
|
this->motor = motor;
|
|
this->encoder = encoder;
|
|
// this->rotationDirection = Direction::Forward;
|
|
}
|
|
|
|
#include <Arduino.h>
|
|
|
|
void ControlledMotor::SetTargetSpeed(float speed) {
|
|
this->currentTargetSpeed = speed;
|
|
// this->rotationDirection =
|
|
// (targetSpeed < 0) ? Direction::Reverse : Direction::Forward;
|
|
// this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise
|
|
// : Motor::Direction::Clockwise;
|
|
}
|
|
|
|
void ControlledMotor::Update(unsigned long currentTimeMs) {
|
|
this->actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
|
|
if (this->currentTargetSpeed < 0)
|
|
this->actualSpeed = -this->actualSpeed;
|
|
|
|
float deltaTime = currentTimeMs - this->lastUpdateTime;
|
|
|
|
float error = this->currentTargetSpeed - this->actualSpeed;
|
|
float errorChange = (error - lastError) * deltaTime;
|
|
|
|
float delta = error * pidP + errorChange * pidD;
|
|
|
|
Serial.print(" actual Speed ");
|
|
Serial.print(actualSpeed);
|
|
Serial.print(" target Speed ");
|
|
Serial.print(this->currentTargetSpeed);
|
|
Serial.print(" motor target speed ");
|
|
Serial.print(motor->currentTargetSpeed);
|
|
Serial.print(" + ");
|
|
Serial.print(error * pidP);
|
|
Serial.print(" + ");
|
|
Serial.print(errorChange * pidD);
|
|
Serial.print(" = ");
|
|
Serial.println(motor->currentTargetSpeed + delta);
|
|
|
|
motor->SetTargetSpeed(motor->currentTargetSpeed +
|
|
delta); // or something like that
|
|
this->lastUpdateTime = currentTimeMs;
|
|
}
|
|
|
|
float ControlledMotor::GetActualSpeed() { return actualSpeed; }
|
|
|
|
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;
|
|
} |