20 lines
578 B
C++
20 lines
578 B
C++
#include <ControlledMotor.h>
|
|
|
|
ControlledMotor::ControlledMotor() {}
|
|
|
|
ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder) {
|
|
this->motor = motor;
|
|
this->encoder = encoder;
|
|
}
|
|
|
|
void ControlledMotor::SetTargetVelocity(float velocity) {
|
|
this->targetVelocity = velocity;
|
|
}
|
|
|
|
void ControlledMotor::Update(float timeStep) {
|
|
float velocity = GetActualVelocity();
|
|
float error = targetVelocity - velocity;
|
|
|
|
float acceleration = error * timeStep * pidP; // Just P is used at this moment
|
|
motor->SetSpeed(targetVelocity + acceleration); // or something like that
|
|
} |