diff --git a/src/software/power/charger.cpp b/src/software/power/charger.cpp index f1cffc2c51..5ff6ad5836 100644 --- a/src/software/power/charger.cpp +++ b/src/software/power/charger.cpp @@ -2,21 +2,224 @@ Charger::Charger() { - pinMode(HV_SENSE, INPUT); + // Currently unused pinMode(FLYBACK_FAULT, INPUT); - pinMode(CHRG_DONE, INPUT); + + // DONE is active-low, also have hardware pullup on this pin. + pinMode(CHRG_DONE, INPUT_PULLUP); + pinMode(CHRG, OUTPUT); + digitalWrite(CHRG, LOW); + + // ADS7945 bit-banged SPI pins. + pinMode(ADC_CS, OUTPUT); + digitalWrite(ADC_CS, HIGH); + + pinMode(ADC_SCLK, OUTPUT); + digitalWrite(ADC_SCLK, LOW); + + pinMode(ADC_MISO, INPUT); + + // Hardware pulls CH_SEL low to select CH0. + // Can be changed later to include temperature sensing. } -void Charger::setCapacitorPin(bool pin_state) +void Charger::chargeCapacitors() { - // TODO: revert (setting low for now bc caps are sketchy) + // LT3750 charging is initiated by a CHARGE rising edge. + + if (charge_inhibited_) + { + return; + } + digitalWrite(CHRG, LOW); + delayMicroseconds(LT3750_CHARGE_LOW_TIME_US); + digitalWrite(CHRG, HIGH); + + charging_enabled_ = true; + last_charge_edge_ms_ = millis(); +} + +void Charger::stopCharging() +{ + digitalWrite(CHRG, LOW); + charging_enabled_ = false; +} + +void Charger::setChargeInhibited(const bool inhibited) +{ + charge_inhibited_ = inhibited; + + if (charge_inhibited_) + { + stopCharging(); + } +} + +void Charger::maintainCharge() +{ + // Do not enable charging until one valid capacitor-voltage measurement exists. + if (!capacitor_voltage_control_initialized_) + { + return; + } + + // During kick/chip guard and pulse time, do not charge. + if (charge_inhibited_) + { + stopCharging(); + return; + } + + const float voltage = capacitor_voltage_control_; + const uint32_t now = millis(); + + if (voltage >= CHARGE_STOP_VOLTAGE_V) + { + stopCharging(); + return; + } + + if (voltage <= CHARGE_RESTART_VOLTAGE_V) + { + const bool retry_due = (now - last_charge_edge_ms_) >= CHARGE_RETRY_INTERVAL_MS; + + if (!charging_enabled_ || retry_due) + { + chargeCapacitors(); + } + } +} + +bool Charger::getChargeDone() +{ + // LT3750 DONE is active low. + return digitalRead(CHRG_DONE) == LOW; +} + +uint16_t Charger::readAds7945WordBitBang() +{ + uint16_t raw = 0; + + digitalWrite(ADC_SCLK, LOW); + digitalWrite(ADC_CS, LOW); + delayMicroseconds(ADC_CS_DELAY_US); + + for (int i = 0; i < 16; i++) + { + // ADS7945 MSB is available after CS falling edge; subsequent bits update + // after SCLK falling edges. Sampling while SCLK is high is the intended + // first simple mode. + digitalWrite(ADC_SCLK, HIGH); + delayMicroseconds(ADC_CLOCK_DELAY_US); + + raw <<= 1; + raw |= digitalRead(ADC_MISO) ? 1 : 0; + + digitalWrite(ADC_SCLK, LOW); + delayMicroseconds(ADC_CLOCK_DELAY_US); + } + + digitalWrite(ADC_CS, HIGH); + delayMicroseconds(ADC_CS_DELAY_US); + + return raw; +} + +int16_t Charger::signExtend14(uint16_t raw14) +{ + raw14 &= 0x3FFF; + + if (raw14 & 0x2000) + { + return static_cast(raw14 | 0xC000); + } + + return static_cast(raw14); +} + +int16_t Charger::readAds7945SignedCode() +{ + // ADS7945 outputs the result of the previous conversion. + // Since CH_SEL is fixed to CH0 and capacitor voltage changes slowly, this is fine. + // Consider modifying if temperature sensing is added. + const uint16_t raw16 = readAds7945WordBitBang(); + const uint16_t raw14 = (raw16 >> ADC_RESULT_RIGHT_SHIFT) & 0x3FFF; + + return signExtend14(raw14); +} + +float Charger::adcCodeToDifferentialVoltage(float code) +{ + return code * ADC_VREF / static_cast(ADC_HALF_SCALE); +} + +float Charger::adcDifferentialVoltageToCapacitorVoltage(float v_adc_diff) +{ + // ADS7945 measures ISO224 differential output. + // ISO224: Vout_diff = Vin / 3, so Vin = Vout_diff / ISO224_GAIN. + const float v_iso_input = v_adc_diff / ISO224_GAIN; + + // ISO224 input is the divided capacitor voltage. + return v_iso_input / DIVIDER_RATIO; } float Charger::getCapacitorVoltage() { - return analogRead(HV_SENSE) / RESOLUTION * SCALE_VOLTAGE * VOLTAGE_DIVIDER; + static constexpr int NUM_SAMPLES = 8; + + // Calibration from scope measurements + // diagnostic 286.0 V corresponds to actual 196.44 V. + // This linear fit is applied after converting the raw ADC code to a voltage which + // already accounts for the hardware gain and divider + static constexpr float CAP_CAL_ANCHOR_DIAGNOSTIC_V = 286.0f; + static constexpr float CAP_CAL_ANCHOR_ACTUAL_V = 196.44f; + static constexpr float CAP_CAL_SLOPE = 0.7019289f; + + // ADS7945 returns the previous conversion, so discard one frame first. + (void)readAds7945SignedCode(); + + long code_sum = 0; + + for (int i = 0; i < NUM_SAMPLES; i++) + { + code_sum += readAds7945SignedCode(); + } + + const float avg_code = static_cast(code_sum) / static_cast(NUM_SAMPLES); + + const float v_adc_diff = adcCodeToDifferentialVoltage(avg_code); + + // Uncalibrated diagnostic voltage. + const float raw_capacitor_voltage = + adcDifferentialVoltageToCapacitorVoltage(v_adc_diff); + + // Convert diagnostic voltage to calibrated physical capacitor voltage. + const float calibrated_capacitor_voltage = + CAP_CAL_ANCHOR_ACTUAL_V + + CAP_CAL_SLOPE * (raw_capacitor_voltage - CAP_CAL_ANCHOR_DIAGNOSTIC_V); + + // Prevent a small negative reported voltage near 0 V. + const float physical_capacitor_voltage = + (calibrated_capacitor_voltage < 0.0f) ? 0.0f : calibrated_capacitor_voltage; + + // Non EMA value used in maintainCharge() logic for hysteresis. + capacitor_voltage_control_ = physical_capacitor_voltage; + capacitor_voltage_control_initialized_ = true; + + // Initialize from the first real reading so startup does not ramp from 0 V. + if (!capacitor_voltage_ema_initialized_) + { + capacitor_voltage_ema_ = physical_capacitor_voltage; + capacitor_voltage_ema_initialized_ = true; + } + else + { + capacitor_voltage_ema_ += + CAP_VOLTAGE_EMA_ALPHA * (physical_capacitor_voltage - capacitor_voltage_ema_); + } + return capacitor_voltage_ema_; } bool Charger::getFlybackFault() diff --git a/src/software/power/charger.h b/src/software/power/charger.h index 97e39c0518..c90ce3dc44 100644 --- a/src/software/power/charger.h +++ b/src/software/power/charger.h @@ -3,36 +3,115 @@ #include /* - * Represents the charger on the power board + * Represents the charger and capacitor voltage sensing on the power board. + * capacitor voltage -> divider -> ISO224 -> ADS7945 CH0 -> ESP32 GPIO5/18/21 */ class Charger { public: /** - * Creates a Charger setting up pins and attaching interrupts. + * Creates a Charger, setting up pins. */ Charger(); + /** - * Sets the state of the capacitors and whether we should charge them. - * @param pin_state HIGH to begin charging the capacitors. Otherwise, sets the charge - * pin to LOW. + * Starts/restarts an LT3750 charge cycle by creating a low -> high edge + * on CHRG. Does nothing while charging is inhibited. + * + * New edges may be issued periodically while voltage remains below target threshold. */ - static void setCapacitorPin(bool pin_state); + void chargeCapacitors(); + /** - * Returns the voltage of the capacitors - * - * @return voltage of capacitors + * Immediately disables the LT3750 by holding CHRG low. + */ + void stopCharging(); + + /** + * Forces the charger off while true. Used before and during kicker pulses. + */ + void setChargeInhibited(bool inhibited); + + /** + * Maintains capacitor voltage using hysteresis and the latest unfiltered + * calibrated voltage reading. + */ + void maintainCharge(); + + /** + * Returns the EMA-smoothed capacitor voltage for telemetry. + * Also updates the internal unfiltered control voltage used by maintainCharge(). */ float getCapacitorVoltage(); + /** - * Returns the status of the flyback fault - * - * @return whether the flyback fault is tripped or not + * Returns whether the flyback fault is tripped. + * Kept for compatibility with previous firmware, but unused. */ bool getFlybackFault(); + /** + * Returns true when LT3750 DONE is active. + * Not supported by all boards, thus unused. + */ + static bool getChargeDone(); + private: - static constexpr float VOLTAGE_DIVIDER = 1003.0 / 13.0; - static constexpr float RESOLUTION = 4096.0; - static constexpr float SCALE_VOLTAGE = 3.3; + static uint16_t readAds7945WordBitBang(); + static int16_t readAds7945SignedCode(); + static int16_t signExtend14(uint16_t raw14); + static float adcCodeToDifferentialVoltage(float code); + static float adcDifferentialVoltageToCapacitorVoltage(float v_adc_diff); + + // ADS7945 / analog constants. + // + // 5.0f for REF5050. + static constexpr float ADC_VREF = 5.0f; + + static constexpr int ADC_BITS = 14; + static constexpr int ADC_HALF_SCALE = 1 << (ADC_BITS - 1); // 8192 + + // ADS7945 14-bit result is expected to be left-aligned in the 16-clock frame: + // raw16 bits [15:2] = D13..D0. + static constexpr int ADC_RESULT_RIGHT_SHIFT = 2; + + // ISO224 nominal gain: VOUT_DIFF = VIN / 3. + static constexpr float ISO224_GAIN = 1.0f / 3.0f; + + // High-voltage divider. + static constexpr float DIVIDER_R_TOP_OHMS = 230000.0f; + static constexpr float DIVIDER_R_BOTTOM_OHMS = 8200.0f; + static constexpr float DIVIDER_RATIO = + DIVIDER_R_BOTTOM_OHMS / (DIVIDER_R_TOP_OHMS + DIVIDER_R_BOTTOM_OHMS); + + // Bit-banged SPI timing. Slow and conservative. + static constexpr uint32_t ADC_CLOCK_DELAY_US = 2; + static constexpr uint32_t ADC_CS_DELAY_US = 2; + + // LT3750 timing. + static constexpr uint32_t LT3750_CHARGE_LOW_TIME_US = 25; // datasheet min is 20 us + + // Capacitor voltage control for maintainCharge() hysteresis. + static constexpr float CHARGE_STOP_VOLTAGE_V = 198.0f; + static constexpr float CHARGE_RESTART_VOLTAGE_V = 190.0f; + + // EMA is used for telemetry smoothing, not used for maintainCharge() logic. + static constexpr float CAP_VOLTAGE_EMA_ALPHA = + 0.02f; // smaller is smoother but slower to respond + + float capacitor_voltage_control_ = 0.0f; + bool capacitor_voltage_control_initialized_ = false; + + float capacitor_voltage_ema_ = 0.0f; + bool capacitor_voltage_ema_initialized_ = false; + + bool charging_enabled_ = false; + bool charge_inhibited_ = false; + + // Time of most recent CHRG edge + uint32_t last_charge_edge_ms_ = 0; + + // While voltage is below CHARGE_RESTART_VOLTAGE_V, + // retry charge edges at interval + static constexpr uint32_t CHARGE_RETRY_INTERVAL_MS = 250; }; diff --git a/src/software/power/chicker.cpp b/src/software/power/chicker.cpp index 741c614c14..5781624875 100644 --- a/src/software/power/chicker.cpp +++ b/src/software/power/chicker.cpp @@ -3,6 +3,7 @@ hw_timer_t* Chicker::pulse_timer = nullptr; hw_timer_t* Chicker::cooldown_timer = nullptr; volatile bool Chicker::on_cooldown = false; +volatile bool Chicker::pulse_finished_ = false; std::shared_ptr Chicker::charger_ = nullptr; Chicker::Chicker(std::shared_ptr charger) @@ -12,6 +13,9 @@ Chicker::Chicker(std::shared_ptr charger) pinMode(KICKER_PIN, OUTPUT); pinMode(BREAK_BEAM_PIN, INPUT); + digitalWrite(CHIPPER_PIN, LOW); + digitalWrite(KICKER_PIN, LOW); + pulse_timer = timerBegin(CHICKER_PULSE_TIMER, 80, true); timerAttachInterrupt(pulse_timer, &stopPulse, true); @@ -21,22 +25,12 @@ Chicker::Chicker(std::shared_ptr charger) void Chicker::kick(uint32_t kick_pulse_width) { - oneShotPulse(kick_pulse_width, KICKER_PIN); - // Charging occurs on rising edge, so toggle the pin - delay(CHARGE_DELAY_MILLISECONDS); - charger_->setCapacitorPin(LOW); - delay(CHARGE_DELAY_MILLISECONDS); - charger_->setCapacitorPin(HIGH); + requestPulse(kick_pulse_width, KICKER_PIN); } void Chicker::chip(uint32_t chip_pulse_width) { - oneShotPulse(chip_pulse_width, CHIPPER_PIN); - // Charging occurs on rising edge, so toggle the pin - delay(CHARGE_DELAY_MILLISECONDS); - charger_->setCapacitorPin(LOW); - delay(CHARGE_DELAY_MILLISECONDS); - charger_->setCapacitorPin(HIGH); + requestPulse(chip_pulse_width, CHIPPER_PIN); } void Chicker::autokick(uint32_t kick_pulse_width) @@ -55,11 +49,28 @@ void Chicker::autochip(uint32_t chip_pulse_width) } } +void Chicker::requestPulse(int duration, int pin) +{ + if (on_cooldown) + { + return; + } + + // Disable the LT3750 before discharging the capacitor bank. + charger_->setChargeInhibited(true); + + // Give flyback switching time to stop before the solenoid pulse. + delayMicroseconds(CHARGE_DISABLE_SETTLE_US); + + oneShotPulse(duration, pin); +} + void IRAM_ATTR Chicker::oneShotPulse(int duration, int pin) { if (!on_cooldown) { - on_cooldown = true; + on_cooldown = true; + pulse_finished_ = false; timerWrite(pulse_timer, 0); timerAlarmWrite(pulse_timer, duration, false); @@ -73,10 +84,23 @@ void IRAM_ATTR Chicker::oneShotPulse(int duration, int pin) } } +void Chicker::update() +{ + // stopPulse() runs in interrupt context. Resume charge control in the + // normal main-loop context only after the pulse has ended. + if (pulse_finished_) + { + pulse_finished_ = false; + charger_->setChargeInhibited(false); + } +} + void IRAM_ATTR Chicker::stopPulse() { digitalWrite(CHIPPER_PIN, LOW); digitalWrite(KICKER_PIN, LOW); + + pulse_finished_ = true; } void IRAM_ATTR Chicker::offCooldown() diff --git a/src/software/power/chicker.h b/src/software/power/chicker.h index 79859eafcc..58c3c3594e 100644 --- a/src/software/power/chicker.h +++ b/src/software/power/chicker.h @@ -17,18 +17,27 @@ class Chicker * Creates a Chicker setting up relevant pins and attaching interrupts */ explicit Chicker(std::shared_ptr charger); + /** * Sets the action of the chicker. Arguments can not be passed to isr's so these * need to be set before calling kick/chip */ static void kick(uint32_t kick_pulse_width); static void chip(uint32_t chip_pulse_width); + /** * Attaches an interrupt on the BREAK_BEAM_PIN to kick/chip. * kick/chip will only be triggered once */ static void autokick(uint32_t kick_pulse_width); static void autochip(uint32_t chip_pulse_width); + + /** + * Called repeatedly from the main loop. + * Re-enables normal charger control after a kick/chip pulse completes. + */ + static void update(); + /** * Get the current status of whether the break beam was tripped or not * This is reset before every kick/chip @@ -38,12 +47,22 @@ class Chicker static bool getBreakBeamTripped(); private: + /** + * Disables charging, waits briefly for the flyback to settle, then starts + * a one-shot kicker or chipper pulse. + * + * @param duration pulse width duration in microseconds + * @param pin the pin to send the pulse to + */ + static void requestPulse(int duration, int pin); + /** * Along with stopPulse creates a square wave to drive the chicker * @param duration pulse width duration in microseconds * @param pin the pin the send the pulse to */ static void oneShotPulse(int duration, int pin); + /** * Called on a pulse_timer to bring the CHIPPER/KICKER pin low */ @@ -56,6 +75,10 @@ class Chicker static hw_timer_t* cooldown_timer; static volatile bool on_cooldown; - static constexpr int COOLDOWN_MICROSECONDS = 3 * MICROSECONDS_IN_SECOND; - static constexpr int CHARGE_DELAY_MILLISECONDS = 5; + static volatile bool pulse_finished_; + + // Charger is held off for this long before the solenoid pulse begins. + static constexpr uint32_t CHARGE_DISABLE_SETTLE_US = 1000; + + static constexpr int COOLDOWN_MICROSECONDS = 3 * MICROSECONDS_IN_SECOND; }; diff --git a/src/software/power/pins.h b/src/software/power/pins.h index 7e7e9bedb8..516d4ce8b4 100644 --- a/src/software/power/pins.h +++ b/src/software/power/pins.h @@ -6,11 +6,18 @@ const uint8_t KICKER_PIN = 33; const uint8_t CHIPPER_PIN = 32; -// Charger -const uint8_t HV_SENSE = 36; +// Charger / LT3750 const uint8_t FLYBACK_FAULT = 27; -const uint8_t CHRG_DONE = 25; -const uint8_t CHRG = 26; +const uint8_t CHRG_DONE = 25; // LT3750 DONE, active-low/open-drain +const uint8_t CHRG = 26; // LT3750 CHARGE + +// ADS7945 external ADC +const uint8_t ADC_MISO = 5; // ADS7945 SDO, moved away from GPIO16 +const uint8_t ADC_SCLK = 18; // ADS7945 SCLK +const uint8_t ADC_CS = 21; // ADS7945 CS + +// CH_SEL is intentionally pulled low in hardware so ADS7945 CH0 is selected. +// CH_SEL could be used for temperature sensing, not a priority for now. // Break Beam const uint8_t BREAK_BEAM_PIN = 37; diff --git a/src/software/power/powerloop_main.cc b/src/software/power/powerloop_main.cc index c890c877b2..5f5f2b05d9 100644 --- a/src/software/power/powerloop_main.cc +++ b/src/software/power/powerloop_main.cc @@ -31,6 +31,7 @@ * functionality. */ void setup() {} + void loop() {} #else @@ -58,7 +59,6 @@ void setup() geneva = std::make_shared(); executor = std::make_shared(charger, chicker, geneva); dribbler = std::make_shared(); - charger->setCapacitorPin(HIGH); } void loop() @@ -104,11 +104,19 @@ void loop() dribbler->update(); + // Release the charger inhibit after a kicker/chipper pulse completes. + chicker->update(); + + // Update the raw control reading and obtain the EMA-smoothed telemetry value. + const float capacitor_voltage = charger->getCapacitorVoltage(); + + // Hysteretic charging control uses the fresh non-EMA reading. + charger->maintainCharge(); + // Read sensor values. These are all instantaneous TbotsProto_PowerStatus status = createNanoPbPowerStatus( - monitor->getBatteryVoltage(), charger->getCapacitorVoltage(), - monitor->getCurrentDrawAmp(), geneva->getCurrentSlot(), sequence_num++, - chicker->getBreakBeamTripped()); + monitor->getBatteryVoltage(), capacitor_voltage, monitor->getCurrentDrawAmp(), + geneva->getCurrentSlot(), sequence_num++, chicker->getBreakBeamTripped()); // Write sensor values out to Serial TbotsProto_PowerFrame status_frame = createUartFrame(status);