85 lines
2.9 KiB
C#
85 lines
2.9 KiB
C#
namespace RoboidControl {
|
|
|
|
/// @brief A motor with speed control
|
|
/// It uses a feedback loop from an encoder to regulate the speed
|
|
/// The speed is measured in revolutions per second.
|
|
public class ControlledMotor : Motor {
|
|
public ControlledMotor(Thing parent, RelativeEncoder encoder) : base(parent) {
|
|
this.encoder = encoder;
|
|
}
|
|
// Upgrade an existing motor with an encoder
|
|
public ControlledMotor(Motor motor, RelativeEncoder encoder) : base(motor.parent) {
|
|
this.motor = motor;
|
|
this.encoder = encoder;
|
|
}
|
|
|
|
readonly RelativeEncoder encoder;
|
|
readonly Motor motor;
|
|
|
|
public override float targetSpeed {
|
|
get {
|
|
if (this.motor != null)
|
|
return this.motor.targetSpeed;
|
|
else
|
|
return base.targetSpeed;
|
|
}
|
|
set {
|
|
if (this.motor != null)
|
|
this.motor.targetSpeed = value;
|
|
else
|
|
base.targetSpeed = value;
|
|
}
|
|
}
|
|
|
|
private float _targetAngularSpeed; // In degrees per second
|
|
public virtual float targetAngularSpeed {
|
|
get => _targetAngularSpeed;
|
|
set {
|
|
if (value != this._targetAngularSpeed) {
|
|
this._targetAngularSpeed = value;
|
|
updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
|
|
owner.Send(new BinaryMsg(this));
|
|
}
|
|
}
|
|
}
|
|
/// @brief Get the actual speed from the encoder
|
|
/// @return The speed in angle per second
|
|
public virtual float actualAngularSpee {
|
|
get => encoder.angularSpeed;
|
|
}
|
|
|
|
float pidP = 0.1F;
|
|
float pidD = 0.0F;
|
|
//float pidI = 0.0F;
|
|
|
|
public override void Update(ulong currentTimeMs, bool recurse = false) {
|
|
float actualSpeed = this.encoder.angularSpeed;
|
|
// Simplified rotation direction, shouldbe improved
|
|
// This goes wrong when the target speed is inverted and the motor axcle is still turning in the old direction
|
|
if (this.targetAngularSpeed < 0)
|
|
actualSpeed = -actualSpeed;
|
|
|
|
float deltaTime = (currentTimeMs - this.lastUpdateTime) / 1000.0f;
|
|
|
|
float error = this.targetAngularSpeed - actualSpeed;
|
|
float errorChange = (error - lastError) * deltaTime;
|
|
|
|
float delta = error * pidP + errorChange * pidD;
|
|
|
|
// Update the motor speed
|
|
if (motor != null)
|
|
motor.targetSpeed += delta;
|
|
else
|
|
base.targetSpeed += delta;
|
|
this.lastUpdateTime = currentTimeMs;
|
|
}
|
|
|
|
ulong lastUpdateTime = 0;
|
|
float lastError = 0;
|
|
|
|
// enum Direction { Forward = 1, Reverse = -1 };
|
|
// Direction rotationDirection;
|
|
};
|
|
|
|
} // namespace RoboidControl
|