134 lines
3.3 KiB
C++
Executable File
134 lines
3.3 KiB
C++
Executable File
#include "RangedMotor.hpp"
|
|
#include "StateMachine.hpp"
|
|
#include "defs.hpp"
|
|
|
|
RangedMotor::RangedMotor(
|
|
const float gearboxRatio,
|
|
const float scaleCalibration,
|
|
const float zeroOffset,
|
|
const float minSafeAngle,
|
|
const float maxSafeAngle
|
|
) noexcept
|
|
{
|
|
// Using setters for validation
|
|
setGearboxRatio(gearboxRatio);
|
|
setScaleCalibration(scaleCalibration);
|
|
setZeroOffset(zeroOffset);
|
|
setSafeMinAngle(minSafeAngle);
|
|
setSafeMaxAngle(maxSafeAngle);
|
|
}
|
|
|
|
// Getters
|
|
float RangedMotor::getGearboxRatio() const noexcept
|
|
{
|
|
return gearboxRatio;
|
|
}
|
|
|
|
float RangedMotor::getScaleCalibration() const noexcept
|
|
{
|
|
return scaleCalibration;
|
|
}
|
|
|
|
float RangedMotor::getZeroOffset() const noexcept
|
|
{
|
|
return zeroOffset;
|
|
}
|
|
|
|
float RangedMotor::getSafeMinAngle() const noexcept
|
|
{
|
|
return hardwareAngleToVirtualAngle(safeHardwareMinAngle);
|
|
}
|
|
|
|
float RangedMotor::getSafeMaxAngle() const noexcept
|
|
{
|
|
return hardwareAngleToVirtualAngle(safeHardwareMaxAngle);
|
|
}
|
|
|
|
float RangedMotor::getSafeHardwareMinAngle() const noexcept
|
|
{
|
|
return safeHardwareMinAngle;
|
|
}
|
|
|
|
float RangedMotor::getSafeHardwareMaxAngle() const noexcept
|
|
{
|
|
return safeHardwareMaxAngle;
|
|
}
|
|
|
|
// Setters
|
|
void RangedMotor::setGearboxRatio(float gearboxRatio) noexcept
|
|
{
|
|
if (abs(gearboxRatio) < COMP_EPSILON) {
|
|
StateMachine::getInstance().raiseError("Zero-gearbox-ratio was set on ranged motor!");
|
|
return;
|
|
}
|
|
this->gearboxRatio = gearboxRatio;
|
|
}
|
|
|
|
void RangedMotor::setScaleCalibration(float scaleCalibration) noexcept
|
|
{
|
|
if (abs(scaleCalibration) < COMP_EPSILON) {
|
|
StateMachine::getInstance().raiseError("Zero-scale-calibration was set on ranged motor!");
|
|
return;
|
|
}
|
|
this->scaleCalibration = scaleCalibration;
|
|
}
|
|
|
|
void RangedMotor::setZeroOffset(float zeroOffset) noexcept
|
|
{
|
|
this->zeroOffset = zeroOffset;
|
|
}
|
|
|
|
void RangedMotor::setSafeMinAngle(float safeMinAngle) noexcept
|
|
{
|
|
safeHardwareMinAngle = virtualAngleToHardwareAngle(safeMinAngle);
|
|
}
|
|
|
|
void RangedMotor::setSafeMaxAngle(float safeMaxAngle) noexcept
|
|
{
|
|
safeHardwareMaxAngle = virtualAngleToHardwareAngle(safeMaxAngle);
|
|
}
|
|
|
|
void RangedMotor::setSafeHardwareMinAngle(float safeHardwareMinAngle) noexcept
|
|
{
|
|
this->safeHardwareMinAngle = safeHardwareMinAngle;
|
|
}
|
|
|
|
void RangedMotor::setSafeHardwareMaxAngle(float safeHardwareMaxAngle) noexcept
|
|
{
|
|
this->safeHardwareMaxAngle = safeHardwareMaxAngle;
|
|
}
|
|
|
|
float RangedMotor::virtualAngleToHardwareAngle(float virtualAngle) const noexcept
|
|
{
|
|
// Apply scaling correction
|
|
virtualAngle *= scaleCalibration;
|
|
|
|
// Apply offset
|
|
virtualAngle += zeroOffset;
|
|
|
|
// Apply gearbox reduction
|
|
virtualAngle *= gearboxRatio;
|
|
|
|
// Return the now no-longer virtual angle
|
|
return virtualAngle;
|
|
}
|
|
|
|
float RangedMotor::hardwareAngleToVirtualAngle(float hardwareAngle) const noexcept
|
|
{
|
|
// Undo gearbox ration
|
|
hardwareAngle /= gearboxRatio;
|
|
|
|
// Undo offset
|
|
hardwareAngle -= zeroOffset;
|
|
|
|
// Undo scaling correction
|
|
hardwareAngle /= scaleCalibration;
|
|
|
|
return hardwareAngle;
|
|
}
|
|
|
|
float RangedMotor::calculateScaleCalibration(const float virtualRange, const float hardwareRange)
|
|
{
|
|
return hardwareRange / virtualRange;
|
|
}
|