Use Angle16 for servo angles
This commit is contained in:
parent
4b362fffa8
commit
f3e2656502
@ -9,7 +9,7 @@ class AbsoluteEncoder {
|
||||
public:
|
||||
AbsoluteEncoder() {}
|
||||
|
||||
virtual Angle GetActualAngle() = 0;
|
||||
virtual Angle16 GetActualAngle() = 0;
|
||||
virtual float GetActualVelocity() = 0;
|
||||
};
|
||||
|
||||
|
@ -10,7 +10,7 @@ ServoMotor::ServoMotor()
|
||||
this->hasTargetAngle = false;
|
||||
}
|
||||
|
||||
void ServoMotor::SetTargetAngle(Angle angle) {
|
||||
void ServoMotor::SetTargetAngle(Angle16 angle) {
|
||||
angle = Float::Clamp(angle, minAngle, maxAngle);
|
||||
|
||||
if (maxVelocity == 0.0F || hasTargetAngle == false) {
|
||||
@ -23,7 +23,7 @@ void ServoMotor::SetTargetAngle(Angle angle) {
|
||||
this->hasTargetAngle = true;
|
||||
}
|
||||
|
||||
Angle ServoMotor::GetTargetAngle() {
|
||||
Angle16 ServoMotor::GetTargetAngle() {
|
||||
return this->targetAngle;
|
||||
}
|
||||
|
||||
|
11
ServoMotor.h
11
ServoMotor.h
@ -1,6 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include "ControlledMotor.h"
|
||||
#include "LinearAlgebra/Angle16.h"
|
||||
|
||||
namespace Passer {
|
||||
namespace RoboidContol {
|
||||
@ -16,8 +17,8 @@ class ServoMotor : public Thing {
|
||||
enum ControlMode { Position, Velocity };
|
||||
ControlMode controlMode = ControlMode::Position;
|
||||
|
||||
virtual void SetTargetAngle(Angle angle);
|
||||
virtual Angle GetTargetAngle();
|
||||
virtual void SetTargetAngle(Angle16 angle);
|
||||
virtual Angle16 GetTargetAngle();
|
||||
|
||||
virtual void SetMaximumVelocity(float maxVelocity);
|
||||
|
||||
@ -28,16 +29,16 @@ class ServoMotor : public Thing {
|
||||
|
||||
protected:
|
||||
bool hasTargetAngle = false;
|
||||
Angle targetAngle = 0.0F;
|
||||
Angle16 targetAngle = 0.0F;
|
||||
|
||||
float maxVelocity = 0.0F;
|
||||
|
||||
float targetVelocity = 0.0F;
|
||||
Angle limitedTargetAngle = 0.0F;
|
||||
Angle16 limitedTargetAngle = 0.0F;
|
||||
|
||||
float lastUpdateTimeMs = 0.0F;
|
||||
|
||||
virtual void SetAngle(Angle angle) = 0;
|
||||
virtual void SetAngle(Angle16 angle) = 0;
|
||||
};
|
||||
|
||||
} // namespace RoboidContol
|
||||
|
Loading…
x
Reference in New Issue
Block a user