99 lines
3.1 KiB
C++
99 lines
3.1 KiB
C++
#ifndef CONTROLLER_HPP
|
|
#define CONTROLLER_HPP
|
|
|
|
#include <stdint.h>
|
|
#include <avr/pgmspace.h>
|
|
#include "IndSensorMap.hpp"
|
|
|
|
// ── Pin Mapping ──────────────────────────────────────────────
|
|
#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 CAP 250 // Max PWM magnitude (0-255 Arduino range)
|
|
|
|
// ── PID Gains (single set per loop — matches simulation) ────
|
|
typedef struct PIDGains {
|
|
float kp;
|
|
float ki;
|
|
float kd;
|
|
} PIDGains;
|
|
|
|
// ── PID Error State ──────────────────────────────────────────
|
|
typedef struct PIDState {
|
|
float e; // Current error
|
|
float eDiff; // Derivative of error (e[k] - e[k-1])
|
|
float eInt; // Integral of error (accumulated)
|
|
} PIDState;
|
|
|
|
// ── Feedforward LUT (PROGMEM) ────────────────────────────────
|
|
// Generated by gen_ff_lut.py from MaglevPredictor model
|
|
// Pod mass: 9.4 kg, Coil R: 1.1Ω, V_supply: 12V
|
|
// Positive = repelling, Negative = attracting
|
|
// Beyond ~16mm: clamped to -CAP (no equilibrium exists)
|
|
#define FF_LUT_SIZE 64
|
|
#define FF_GAP_MIN 3.0f
|
|
#define FF_GAP_MAX 20.0f
|
|
#define FF_GAP_STEP 0.269841f
|
|
|
|
// ── Geometry (mm, matching simulation) ───────────────────────
|
|
// Sensor-to-sensor distances for angle computation
|
|
#define Y_DISTANCE_MM 101.6f // Left↔Right sensor spacing (mm)
|
|
#define X_DISTANCE_MM 251.8f // Front↔Back sensor spacing (mm)
|
|
|
|
// ── Controller Class ─────────────────────────────────────────
|
|
class FullController {
|
|
public:
|
|
bool oor; // Any sensor out-of-range
|
|
bool outputOn; // Enable/disable output
|
|
|
|
FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
|
|
PIDGains heightGains, PIDGains rollGains, PIDGains pitchGains,
|
|
float avgRef, bool useFeedforward);
|
|
|
|
void update();
|
|
void zeroPWMs();
|
|
void sendOutputs();
|
|
void report();
|
|
|
|
// Runtime tuning
|
|
void updateHeightPID(PIDGains gains);
|
|
void updateRollPID(PIDGains gains);
|
|
void updatePitchPID(PIDGains gains);
|
|
void updateReference(float avgReference);
|
|
void setFeedforward(bool enabled);
|
|
|
|
private:
|
|
int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
|
|
int16_t feedforward(float gapMM);
|
|
|
|
IndSensor& Left;
|
|
IndSensor& Right;
|
|
IndSensor& Front;
|
|
IndSensor& Back;
|
|
|
|
PIDGains heightGains;
|
|
PIDGains rollGains;
|
|
PIDGains pitchGains;
|
|
|
|
PIDState heightErr;
|
|
PIDState rollErr;
|
|
PIDState pitchErr;
|
|
|
|
float AvgRef; // Target gap height (mm)
|
|
float avg; // Current average gap (mm)
|
|
bool ffEnabled; // Feedforward on/off
|
|
|
|
int16_t FLPWM;
|
|
int16_t BLPWM;
|
|
int16_t FRPWM;
|
|
int16_t BRPWM;
|
|
};
|
|
|
|
#endif // CONTROLLER_HPP
|