23 lines
611 B
C++
23 lines
611 B
C++
#include "ControlledMotor.h"
|
|
|
|
ControlledMotor::ControlledMotor() {
|
|
this->isControlledMotor = true;
|
|
}
|
|
|
|
ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) {
|
|
this->motor = motor;
|
|
this->encoder = encoder;
|
|
}
|
|
|
|
void ControlledMotor::SetTargetSpeed(float velocity) {
|
|
this->targetVelocity = velocity;
|
|
}
|
|
|
|
void ControlledMotor::Update(float timeStep) {
|
|
float velocity = GetActualSpeed();
|
|
float error = targetVelocity - velocity;
|
|
|
|
float acceleration =
|
|
error * timeStep * pidP; // Just P is used at this moment
|
|
motor->SetSpeed(targetVelocity + acceleration); // or something like that
|
|
} |