97 lines
3.1 KiB
C++
97 lines
3.1 KiB
C++
#ifndef HEAVE_CONTROLLER_HPP
|
|
#define HEAVE_CONTROLLER_HPP
|
|
|
|
#include <stdint.h>
|
|
#include "IndSensorLUT.hpp"
|
|
|
|
// ── Pin Mapping (mirrors Controller.hpp) ─────────────────────
|
|
#define dirBL 2
|
|
#define pwmBL 3
|
|
#define dirBR 4
|
|
#define pwmBR 10
|
|
#define pwmFL 11
|
|
#define dirFL 7
|
|
#define dirFR 8
|
|
#define pwmFR 9
|
|
|
|
// ── Output Cap ───────────────────────────────────────────────
|
|
#define HEAVE_CAP 250
|
|
|
|
// ── Feedforward LUT (gap mm → equilibrium PWM) ───────────────
|
|
// Source: FF_PWM_LUT in Controller.cpp (pod 9.4 kg, R 1.1 Ω, V 12 V).
|
|
// Positive = repel, negative = attract. Lives in PROGMEM.
|
|
#define HEAVE_FF_LUT_SIZE 64
|
|
#define HEAVE_FF_GAP_MIN 3.0f
|
|
#define HEAVE_FF_GAP_MAX 20.0f
|
|
#define HEAVE_FF_GAP_STEP 0.269841f
|
|
|
|
// ── Reference Ramp ───────────────────────────────────────────
|
|
// Per-update step the active ref moves toward the target. At 200Hz tick
|
|
// rate, 0.001f mm/tick = 0.2 mm/s. Override via constructor.
|
|
#define HEAVE_DEFAULT_RAMP_STEP 0.001f
|
|
|
|
// ── PID Gains / State ────────────────────────────────────────
|
|
typedef struct HeavePIDGains {
|
|
float kp;
|
|
float ki;
|
|
float kd;
|
|
} HeavePIDGains;
|
|
|
|
typedef struct HeavePIDState {
|
|
float e;
|
|
float eDiff;
|
|
float eInt;
|
|
} HeavePIDState;
|
|
|
|
// ── Heave-only Controller ────────────────────────────────────
|
|
// Single PID on the average gap across all four inductive sensors.
|
|
// Drives all four motor channels with the same PWM magnitude + direction.
|
|
class HeaveController {
|
|
public:
|
|
bool oor;
|
|
bool outputOn;
|
|
|
|
HeaveController(IndSensorL& f, IndSensorL& b,
|
|
HeavePIDGains gains, float avgRef,
|
|
float rampStep = HEAVE_DEFAULT_RAMP_STEP,
|
|
bool useFeedforward = false);
|
|
|
|
void update();
|
|
void zeroPWMs();
|
|
void sendOutputs();
|
|
void report();
|
|
|
|
void updatePID(HeavePIDGains gains);
|
|
void updateReference(float avgReference);
|
|
|
|
// Manual override: drive all channels at -HEAVE_CAP (full attract),
|
|
// bypassing the PID. OOR still zeroes the output via sendOutputs().
|
|
void setFullAttract(bool enabled);
|
|
bool isFullAttract() const { return fullAttract; }
|
|
|
|
void setFeedforward(bool enabled) { ffEnabled = enabled; }
|
|
|
|
private:
|
|
int16_t pidCompute();
|
|
int16_t feedforward(float gapMM);
|
|
|
|
IndSensorL& Front;
|
|
IndSensorL& Back;
|
|
|
|
HeavePIDGains gains;
|
|
HeavePIDState state;
|
|
|
|
float AvgRef; // active ref the PID tracks; ramps toward targetRef
|
|
float targetRef; // final desired ref (set by updateReference)
|
|
float rampStep; // per-update step size for AvgRef → targetRef
|
|
float avg;
|
|
|
|
int16_t PWM;
|
|
int16_t ffPWM; // last feedforward value (for debugging/reporting)
|
|
bool fullAttract;
|
|
bool ffEnabled;
|
|
bool prevPidActive; // transition detector for seeding AvgRef = avg
|
|
};
|
|
|
|
#endif // HEAVE_CONTROLLER_HPP
|