diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h index a9b2f1c65c380bdc9197ae2e12ff3f50ccd45e94..03f499d8b9042082b757d6f17a510dc97b6a388d 100644 --- a/src/boards/RIGv2/Configs/SensorsConfig.h +++ b/src/boards/RIGv2/Configs/SensorsConfig.h @@ -75,7 +75,7 @@ static constexpr bool ENABLED = true; namespace MAX31856 { static constexpr uint32_t SAMPLE_PERIOD = 100; -static constexpr bool ENABLED = true; +static constexpr bool ENABLED = true; } // namespace MAX31856 namespace Trafag @@ -104,20 +104,25 @@ static constexpr unsigned int CALIBRATE_SAMPLE_COUNT = 10; static constexpr unsigned int CALIBRATE_SAMPLE_PERIOD = 40; // LC Tank sensor calibration data -// - 1.866kg V: 0.000941 -// - 5.050kg V: 0.002550 -// - 6.916kg V: 0.003559 -static constexpr float TANK_SCALE = 1968.8771f; +static constexpr float TANK_P0_VOLTAGE = 0.000941; +static constexpr float TANK_P0_MASS = 1.866; +static constexpr float TANK_P1_VOLTAGE = 0.003559; +static constexpr float TANK_P1_MASS = 6.916; + // LC Vessel sensor calibration data // - 1.866kg V: 0.00027 // - 5.050kg V: 0.00073 // - 6.916kg V: 0.00100 -static constexpr float VESSEL_SCALE = 6914.9731f; -} +static constexpr float VESSEL_P0_VOLTAGE = 0.00027; +static constexpr float VESSEL_P0_MASS = 1.866; +static constexpr float VESSEL_P1_VOLTAGE = 0.0010; +static constexpr float VESSEL_P1_MASS = 6.916; +} // namespace LoadCell namespace InternalADC { -static constexpr Boardcore::InternalADC::Channel BATTERY_VOLTAGE_CHANNEL = Boardcore::InternalADC::CH14; +static constexpr Boardcore::InternalADC::Channel BATTERY_VOLTAGE_CHANNEL = + Boardcore::InternalADC::CH14; static constexpr float BATTERY_VOLTAGE_SCALE = 4.7917; static constexpr uint32_t SAMPLE_PERIOD = 100; diff --git a/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h b/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h index 62654d48f8455851031d77b1fa8bc50fd8408bd2..b1eb038a97600642fab74dd7e8beb84189c3427d 100644 --- a/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h +++ b/src/boards/RIGv2/Sensors/AnalogLoadCellSensor.h @@ -34,8 +34,10 @@ class AnalogLoadCellSensor : public Boardcore::Sensor<Boardcore::LoadCellData> { public: AnalogLoadCellSensor(std::function<Boardcore::ADCData()> getVoltage, - float scale) - : getVoltage{getVoltage}, scale{scale} + float p0Voltage, float p0Mass, float p1Voltage, + float p1Mass) + : getVoltage{getVoltage}, p0Voltage{p0Voltage}, p0Mass{p0Mass}, + p1Voltage{p1Voltage}, p1Mass{p1Mass} { } @@ -47,12 +49,24 @@ private: Boardcore::LoadCellData sampleImpl() override { auto voltage = getVoltage(); - return {voltage.voltageTimestamp, -voltage.voltage * scale}; + return {voltage.voltageTimestamp, -voltageToMass(voltage.voltage)}; + } + + float voltageToMass(float voltage) + { + // Two point calibration + // m = dmass/dvoltage + float scale = (p1Mass - p0Mass) / (p1Voltage - p0Voltage); + float offset = p0Mass - scale * p0Voltage; // Calculate offset + return scale * voltage + offset; } std::function<Boardcore::ADCData()> getVoltage; - float scale; + float p0Voltage; + float p0Mass; + float p1Voltage; + float p1Mass; }; } // namespace RIGv2 \ No newline at end of file