ARTUR Robot, Part 1

/*
  =====================================================================
  DIFFERENTIAL WRIST - CONTROL (sticks, homing, microstep, hi gesture)
  Arduino Nano (ATmega328P 16 MHz) + 2x DRV8825 + FlySky FS-iA6B (iBUS)
  Libraries: AltSoftSerial, AccelStepper
  =====================================================================
 
  PIN MAP
  -------
  D0, D1  USB Serial to computer (kept free)
  D2      Motor 2 DIR
  D3      Motor 2 STEP
  D4      M0   -> both drivers
  D5      EN   -> both drivers (+ 10k pull-up to 5V: drivers off during boot)
  D6      M1   -> both drivers
  D7      M2   -> both drivers
  D8      iBUS signal from FS-iA6B SERVO port (AltSoftSerial RX, fixed)
  D9      Reserved by AltSoftSerial (TX) - leave unconnected
  D11     Motor 1 STEP
  D12     Motor 1 DIR
  A0      Limit switch (COM -> GND, NO -> A0), 100 nF to GND
  SLP, RST (both drivers) -> 5V
  Common GND: Nano, drivers, receiver, motor PSU
 
  FAILSAFE
  --------
  No valid iBUS frame for FAILSAFE_MS (receiver unplugged, wire cut...)
  -> motors disabled at once, homing / hi gesture stopped. Motors can only be
  enabled again after CH7 has been seen OFF (also true at power-up).
  Motors run only while CH7 is clearly ON (>1750): if the receiver outputs
  neutral values (1500) on transmitter loss, motors are disabled and stay
  disabled until CH7 is switched OFF then ON again.
  If the receiver holds the last values instead, set the transmitter
  failsafe of CH7 to OFF (1000).
 
  CHANNELS (iBUS values 1000..2000, center 1500)
  --------
  CH1   phi rate (stick)
  CH2   psi rate (stick)
  CH5   max motor speed   (SPEED_MIN_RPM .. SPEED_MAX_RPM)
  CH6   motor acceleration (ACCEL_MIN_RPMS .. ACCEL_MAX_RPMS)
  CH7   drivers enable     (>1750 ON, anything else OFF)
  CH8   homing: hold (>1750) during the whole procedure, release = abort
  CH9   microstep, 3 positions: 1000 -> 1/8, 1500 -> 1/16, 2000 -> 1/32
  CH10  hi gesture: hold (>1750) during the whole gesture, release = stop
 
  KINEMATICS
  ----------
  R  = REDUCTION (motor -> input bevel), BR = BEVEL_OUT_TEETH / BEVEL_IN_TEETH
  psi = (theta1 + theta2) / (2 R)
  phi = (theta1 - theta2) / (2 R BR)        (side bevels turn opposite, central bevel rolls on them)
  theta1 = R (psi + BR phi)
  theta2 = R (psi - BR phi)
  "raw" psi/phi come from the step counters. Real angle = raw - offset.
  Offsets are 0 at power-up and are only changed by a completed homing.
  psi soft limits (PSI_MIN_DEG..PSI_MAX_DEG) are active once homed.
 
  MICROSTEP CHANGE
  ----------------
  Wrist ramps to a stop -> (if going coarser) small move to the nearest
  position valid in the new mode -> mode pins switched -> step counters
  rescaled so psi/phi are unchanged -> control resumes.
  Microstep changes are frozen during homing.
 
  HOMING (CH8 held, drivers enabled)
  ------
  Homing always runs at HOME_MS (1/8): the wrist stops and switches to it.
  The switch is pressed by the wrist body at psi = 90, and by the bump on
  the central bevel at psi ~ 89 when phi = PHI_BUMP_DEG (22.3 deg).
   0. If the switch is pressed: back psi off until released (+ margin)
   1. psi pass 1: fast approach -> back off -> slow approach -> P1
   2. lower psi to P1 - PSI_TURN_CLEAR_DEG (bump can't reach the switch),
      then rotate phi by PHI_CHECK_DEG in PHI_HOME_DIR (moves the bump away)
   3. psi pass 2: fast approach -> back off -> slow approach -> P2
   4. psi = 90 at max(P1, P2) (the bump triggers ~1 deg early)
   5. back off, approach psi = 89 from below (avoids switch hysteresis)
      if the switch is pressed there, it is the bump: rotate phi away
   6. phi: fast sweep (PHI_HOME_DIR) -> back off -> slow approach -> edge = PHI_BUMP_DEG
   7. move to psi = 0, phi = 0, then commit the new offsets
  Release CH8, drivers off or signal lost at any time -> stop, offsets
  unchanged. Any error -> stop, offsets unchanged, message on Serial.
 
  HI GESTURE (CH10 held, drivers enabled)
  ----------
  From the current position: phi shakes HI_PHI_SHAKES times +-HI_PHI_DEG,
  then psi nods HI_PSI_SHAKES times +-HI_PSI_DEG, then back to the start.
  Once homed, the psi targets are kept inside PSI_MIN_DEG..PSI_MAX_DEG.
  Not homed: no psi limit, start away from the switch.
  Release CH10, drivers off or signal lost -> the wrist ramps to a stop.
  CH10 must be released before a new gesture.
  =====================================================================
*/
 
#include <AltSoftSerial.h>
#include <AccelStepper.h>
 
AltSoftSerial ibusSerial;   // iBUS input: RX = D8, TX = D9 (fixed by the library)
 
// ---- Motors (theta1 = motor 1, theta2 = motor 2) ----
const uint8_t PIN_STEP1 = 11, PIN_DIR1 = 12;   // motor 1 STEP / DIR pins
const uint8_t PIN_STEP2 = 3,  PIN_DIR2 = 2;    // motor 2 STEP / DIR pins
const bool    MOTOR1_INVERT = true;    // reverse motor 1 direction (fixes psi/phi swapped or wrong sign)
const bool    MOTOR2_INVERT = false;   // reverse motor 2 direction
 
AccelStepper stepper1(AccelStepper::DRIVER, PIN_STEP1, PIN_DIR1);
AccelStepper stepper2(AccelStepper::DRIVER, PIN_STEP2, PIN_DIR2);
 
// ---- Limit switch ----
const uint8_t  PIN_LIMIT         = A0;     // switch input (internal pull-up)
const bool     LIMIT_NC          = false;  // true: COM->GND, NC->A0 | false: COM->GND, NO->A0
const uint16_t LIMIT_DEBOUNCE_US = 2000;   // switch must stay in the new state this long (us) to be accepted
 
// ---- DRV8825 shared pins (same pin drives both drivers) ----
const uint8_t PIN_MODE0 = 4;   // M0 microstep select
const uint8_t PIN_EN    = 5;   // EN, active LOW (LOW = motors powered)
const uint8_t PIN_MODE1 = 6;   // M1 microstep select
const uint8_t PIN_MODE2 = 7;   // M2 microstep select
 
// ---- Channels (index starts at 0: CH1 = 0) ----
const uint8_t CH_PHI    = 0;   // CH1  -> phi speed (stick)
const uint8_t CH_PSI    = 1;   // CH2  -> psi speed (stick)
const uint8_t CH_SPEED  = 4;   // CH5  -> max motor speed (knob)
const uint8_t CH_ACCEL  = 5;   // CH6  -> motor acceleration (knob)
const uint8_t CH_ENABLE = 6;   // CH7  -> drivers on (>1750) / off (<1250)
const uint8_t CH_HOME   = 7;   // CH8  -> homing, hold during the whole procedure
const uint8_t CH_MS     = 8;   // CH9  -> microstep, 3 positions
const uint8_t CH_HI     = 9;   // CH10 -> hi gesture (hold)
const bool    PSI_INVERT = false;   // reverse the psi stick only (does not affect homing)
const bool    PHI_INVERT = false;   // reverse the phi stick only (does not affect homing)
 
// ---- Microstepping ----
// CH9: 1000 -> 1/8 | 1500 -> 1/16 | 2000 -> 1/32
const uint8_t MS_TABLE[3] = { 8, 16, 32 };   // [CH9 position] -> microstep divisor
const uint8_t MS_START = 8;        // microstep used at power-up, before the radio is read
 
// ---- Mechanics ----
const float MOTOR_FULL_STEPS = 200;     // motor full steps per revolution (1.8 deg motor)
const float REDUCTION        = 3.0f;    // motor -> input bevel reduction (3:1). With tooth counts: e.g. 60.0f/20.0f
const float BEVEL_IN_TEETH   = 40.0f;   // teeth of each input (side) bevel gear
const float BEVEL_OUT_TEETH  = 64.0f;   // teeth of the central output bevel gear
const float BEVEL_RATIO      = BEVEL_OUT_TEETH / BEVEL_IN_TEETH;   // computed, do not edit
 
// ---- Soft limits (active once homed) ----
const float PSI_MIN_DEG = -88.0f;   // lowest psi allowed by the sticks
const float PSI_MAX_DEG =  88.0f;   // highest psi allowed by the sticks
 
// ---- Settings ----
const float    SPEED_MIN_RPM   = 5;      // motor speed with CH5 at minimum (RPM)
const float    SPEED_MAX_RPM   = 150;    // motor speed with CH5 at maximum (RPM)
const float    ACCEL_MIN_RPMS  = 5;      // motor acceleration with CH6 at minimum (RPM/s)
const float    ACCEL_MAX_RPMS  = 300;    // motor acceleration with CH6 at maximum (RPM/s)
const float    MAX_STEP_RATE   = 2000;   // hard cap on steps/s per motor (Nano limit) - caps ALL speeds, homing included
const float    ALIGN_STEP_RATE = 200;    // steps/s of the small alignment move before a coarser microstep
const uint16_t DEADBAND        = 50;     // stick dead zone around 1500 (no motion inside)
const uint16_t UPDATE_MS       = 10;     // control update period (ms): ramps, homing logic
const uint16_t FAILSAFE_MS     = 200;    // no valid iBUS frame for this long -> motors disabled (ms)
const uint16_t PRINT_MS        = 1000;   // status line period on the Serial Monitor (ms)
 
// ---- Homing reference angles (tune these to correct the zero positions) ----
const float PSI_REF_DEG      = 90.0f;   // real psi when the wrist body presses the switch (psi offset)
const float PHI_BUMP_DEG     = 22.3f;   // real phi at the bump edge (phi offset)
const float PSI_BUMP_DEG     = 89.0f;   // psi held during the phi search: must be high enough for the bump
                                        // to press the switch, but below the body trigger (PSI_REF_DEG)
 
// ---- Homing ----
const float HOME_ACCEL_RPMS  = 300;     // motor acceleration during homing (RPM/s)
const float PSI_FAST_DPS     = 10;      // psi search speed toward the switch (deg/s)
const float PSI_SLOW_DPS     = 2;       // psi final approach speed = measurement precision (deg/s)
const float PSI_BACK_DPS     = 5;       // psi back-off / small moves speed (deg/s)
const float PSI_MARGIN_DEG   = 3;       // extra psi back-off after the switch releases (deg)
const float PHI_FAST_DPS     = 30;      // phi search speed (deg/s), capped by MAX_STEP_RATE
const float PHI_SLOW_DPS     = 6;       // phi final approach speed = measurement precision (deg/s)
const float PHI_BACK_DPS     = 10;      // phi back-off speed (deg/s)
const float PHI_MARGIN_DEG   = 5;       // extra phi back-off after the switch releases (deg)
const float PHI_CHECK_DEG    = 45;      // phi rotation used to move the bump away from the switch (deg)
const float PSI_TURN_CLEAR_DEG = 10;    // psi lowered this much below pass 1 before rotating phi (deg)
const float PHI_HOME_DIR     = -1.0f;   // direction of all phi moves during homing (+1 / -1)
const uint8_t HOME_MS        = 8;       // microstep used during homing (1/8 detects the bump reliably)
const float GOTO_PSI_DPS     = 60;      // psi speed of the final move to (0,0) and phi-away moves (deg/s)
const float GOTO_PHI_DPS     = 30;      // phi speed of the final move to (0,0) and phi-away moves (deg/s)
const float PSI_SEEK_MAX     = 200;     // max psi travel of the fast search before "not found" error (deg)
const float PSI_SLOW_MAX     = 10;      // max psi travel of the slow approach before error (deg)
const float PSI_BACK_MAX     = 20;      // max psi travel to release the switch before error (deg)
const float PHI_SEEK_MAX     = 380;     // max phi travel of the bump search, a bit more than one turn (deg)
const float PHI_SLOW_MAX     = 40;      // max phi travel of the slow approach before error (deg)
const float PHI_BACK_MAX     = 120;     // max phi travel to release the switch before error (deg)
const float BUMP_TOL_DEG     = 0.4f;    // P1/P2 difference above this = one psi pass hit the bump (deg)
const float PASS_MAX_DIFF    = 8.0f;    // P1/P2 difference above this = inconsistent -> error (deg)
 
// ---- Hi gesture (CH10) ----
const uint8_t HI_PHI_SHAKES  =    3;    // phi back-and-forth count
const float   HI_PHI_DEG     = 30.0f;   // phi amplitude each side of the start (deg)
const uint8_t HI_PSI_SHAKES  =    2;    // psi up-and-down count (nod)
const float   HI_PSI_DEG     = 15.0f;   // psi amplitude each side of the start (deg)
const float   HI_PHI_DPS     =   60;    // phi speed (deg/s), capped by MAX_STEP_RATE
const float   HI_PSI_DPS     =   40;    // psi speed (deg/s), capped by MAX_STEP_RATE
 
#define IBUS_BUFFSIZE    32
#define IBUS_MAXCHANNELS 10
 
uint16_t channels[IBUS_MAXCHANNELS];
uint32_t goodFrames = 0;
uint32_t badFrames  = 0;
uint32_t lastFrame  = 0;
bool     haveFrame  = false;
bool     driversOn  = false;
bool     enableArmed = false;   // CH7 must be seen OFF before the motors can be enabled
 
// Microstep state
enum MsState { RUNNING, STOPPING, ALIGNING };
MsState msState   = RUNNING;
uint8_t curMS     = MS_START;
uint8_t desiredMS = MS_START;
uint8_t targetMS  = MS_START;
 
// Kinematic factors (depend on current microstep)
float stepsPerDeg, kPsi, kPhi;
 
float vmax = 0, acc = 0;          // steps/s, steps/s^2 (current mode)
float psiRate = 0, phiRate = 0;   // current (ramped) rates, deg/s
 
// Offsets: real angle = raw - offset
float psiOffset = 0, phiOffset = 0;
bool  homed = false;
 
// Limit switch (debounced, updated in the fast loop)
bool     limitState   = false;
bool     limitRawPrev = false;
uint32_t limitChangeUs = 0;
volatile bool limitEdge = false;     // set on press, cleared by the homing code
float    edgePsiRaw = 0, edgePhiRaw = 0;
 
// =====================================================================
// iBUS
// =====================================================================
 
bool readIBus() {
  static uint8_t buf[IBUS_BUFFSIZE];
  static uint8_t idx = 0;
  static unsigned long lastByte = 0;
 
  while (ibusSerial.available()) {
    uint8_t b = ibusSerial.read();
    unsigned long now = millis();
 
    if (idx > 0 && (now - lastByte) > 3) idx = 0;   // gap -> resync
    lastByte = now;
 
    if (idx == 0 && b != 0x20) continue;
    if (idx == 1 && b != 0x40) { idx = 0; continue; }
 
    buf[idx++] = b;
 
    if (idx == IBUS_BUFFSIZE) {
      idx = 0;
      uint16_t sum = 0xFFFF;
      for (uint8_t i = 0; i < 30; i++) sum -= buf[i];
      uint16_t rxsum = (uint16_t)buf[30] | ((uint16_t)buf[31] << 8);
      if (sum != rxsum) { badFrames++; return false; }
      for (uint8_t i = 0; i < IBUS_MAXCHANNELS; i++)
        channels[i] = (uint16_t)buf[2 + i * 2] | ((uint16_t)buf[3 + i * 2] << 8);
      goodFrames++;
      return true;
    }
  }
  return false;
}
 
// Channel value clamped to 1000..2000 (0 = no data yet -> center)
uint16_t safeChannel(uint8_t ch) {
  uint16_t v = channels[ch];
  if (v == 0)    return 1500;
  if (v < 1000)  return 1000;
  if (v > 2000)  return 2000;
  return v;
}
 
// =====================================================================
// Helpers
// =====================================================================
 
float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
  float r = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
  if (out_min < out_max) { if (r < out_min) return out_min; if (r > out_max) return out_max; }
  else                   { if (r > out_min) return out_min; if (r < out_max) return out_max; }
  return r;
}
 
float sgn(float x) { return x < 0 ? -1.0f : 1.0f; }
 
float normalizeStick(uint16_t v) {
  if (v > 1500 + DEADBAND) return fmap(v, 1500 + DEADBAND, 2000, 0.0f, 1.0f);
  if (v < 1500 - DEADBAND) return fmap(v, 1000, 1500 - DEADBAND, -1.0f, 0.0f);
  return 0.0f;
}
 
bool limitRaw() {
  bool high = digitalRead(PIN_LIMIT) == HIGH;
  return LIMIT_NC ? high : !high;
}
 
void setDriversEnabled(bool on) {
  if (on == driversOn) return;
  digitalWrite(PIN_EN, on ? LOW : HIGH);   // EN is active LOW
  driversOn = on;
}
 
// =====================================================================
// Microstepping
// =====================================================================
 
void setMicrostepPins(uint8_t ms) {
  uint8_t m0, m1, m2;              // DRV8825 table (M2 M1 M0)
  switch (ms) {
    case 1:  m2 = 0; m1 = 0; m0 = 0; break;
    case 2:  m2 = 0; m1 = 0; m0 = 1; break;
    case 4:  m2 = 0; m1 = 1; m0 = 0; break;
    case 8:  m2 = 0; m1 = 1; m0 = 1; break;
    case 16: m2 = 1; m1 = 0; m0 = 0; break;
    default: m2 = 1; m1 = 0; m0 = 1; break;   // 32
  }
  digitalWrite(PIN_MODE0, m0);
  digitalWrite(PIN_MODE1, m1);
  digitalWrite(PIN_MODE2, m2);
}
 
void updateKinematics() {
  stepsPerDeg = MOTOR_FULL_STEPS * curMS / 360.0f;       // motor steps per motor degree
  kPsi = REDUCTION * stepsPerDeg;                        // motor steps per deg of psi
  kPhi = REDUCTION * BEVEL_RATIO * stepsPerDeg;          // motor steps per deg of phi
}
 
float rpmToSteps(float rpm) { return rpm * MOTOR_FULL_STEPS * curMS / 60.0f; }
 
uint8_t readDesiredMS() {
  uint16_t a = safeChannel(CH_MS);                          // CH9, 3 positions
  return MS_TABLE[(a < 1250) ? 0 : (a > 1750) ? 2 : 1];
}
 
// Nearest position that exists in the coarser mode (multiple of r)
long alignedPos(long p, long r) {
  return (p >= 0 ? (p + r / 2) / r : (p - r / 2) / r) * r;
}
 
// =====================================================================
// Kinematics
// =====================================================================
 
float theta1() { return stepper1.currentPosition() / stepsPerDeg; }
float theta2() { return stepper2.currentPosition() / stepsPerDeg; }
float rawPsi() { return (theta1() + theta2()) / (2.0f * REDUCTION); }
float rawPhi() { return (theta1() - theta2()) / (2.0f * REDUCTION * BEVEL_RATIO); }
float getPsi() { return rawPsi() - psiOffset; }
float getPhi() { return rawPhi() - phiOffset; }
 
float motor1Speed(float dpsi, float dphi) { return kPsi * dpsi + kPhi * dphi; }
float motor2Speed(float dpsi, float dphi) { return kPsi * dpsi - kPhi * dphi; }
 
// Debounced limit switch, called every loop. Captures the exact position on press.
void updateLimit() {
  bool raw = limitRaw();
  uint32_t t = micros();
  if (raw != limitRawPrev) {
    limitRawPrev  = raw;
    limitChangeUs = t;
  } else if (raw != limitState && (t - limitChangeUs) >= LIMIT_DEBOUNCE_US) {
    limitState = raw;
    if (raw) {
      edgePsiRaw = rawPsi();
      edgePhiRaw = rawPhi();
      limitEdge  = true;
    }
  }
}
 
// =====================================================================
// Motion
// =====================================================================
 
void hardStop() {
  psiRate = 0;
  phiRate = 0;
  stepper1.setSpeed(0);
  stepper2.setSpeed(0);
}
 
bool isStopped() { return psiRate == 0 && phiRate == 0; }
 
// Desired psi/phi rates -> coordinated ramp -> motor speeds
void rampTo(float psiD, float phiD, float dt, float vmaxS, float accS) {
  // No motor faster than vmaxS (keeps direction)
  float m = max(fabs(motor1Speed(psiD, phiD)), fabs(motor2Speed(psiD, phiD)));
  if (m > vmaxS) { float s = vmaxS / m; psiD *= s; phiD *= s; }
 
  // Motor speed change limited to accS*dt, same ratio on both axes
  float dPsi = psiD - psiRate;
  float dPhi = phiD - phiRate;
  float mm   = max(fabs(motor1Speed(dPsi, dPhi)), fabs(motor2Speed(dPsi, dPhi)));
  float lim  = accS * dt;
  if (mm > lim) { float s = lim / mm; dPsi *= s; dPhi *= s; }
  psiRate += dPsi;
  phiRate += dPhi;
 
  stepper1.setSpeed(motor1Speed(psiRate, phiRate));
  stepper2.setSpeed(motor2Speed(psiRate, phiRate));
}
 
// Stick control
void updateMotion(float dt, bool sticksAllowed) {
  vmax = min(rpmToSteps(fmap(safeChannel(CH_SPEED), 1000, 2000, SPEED_MIN_RPM, SPEED_MAX_RPM)), MAX_STEP_RATE);
  acc  = rpmToSteps(fmap(safeChannel(CH_ACCEL), 1000, 2000, ACCEL_MIN_RPMS, ACCEL_MAX_RPMS));
 
  float n1 = 0, n2 = 0;
  if (sticksAllowed) {
    n1 = normalizeStick(safeChannel(CH_PSI)); if (PSI_INVERT) n1 = -n1;
    n2 = normalizeStick(safeChannel(CH_PHI)); if (PHI_INVERT) n2 = -n2;
  }
 
  // Full stick on one axis alone = motors at vmax
  float psiD = n1 * vmax / kPsi;   // deg/s
  float phiD = n2 * vmax / kPhi;   // deg/s
 
  // psi soft limits: slow down so the wrist can stop before the limit
  if (homed) {
    float a   = 0.5f * acc / kPsi;          // conservative psi deceleration (deg/s^2)
    float psi = getPsi();
    if (psiD > 0) psiD = min(psiD, sqrt(max(0.0f, 2.0f * a * (PSI_MAX_DEG - psi))));
    if (psiD < 0) psiD = max(psiD, -sqrt(max(0.0f, 2.0f * a * (psi - PSI_MIN_DEG))));
  }
 
  rampTo(psiD, phiD, dt, vmax, acc);
}
 
// =====================================================================
// Microstep change
// =====================================================================
 
// Small move to the nearest valid position of the coarser mode
void startAlign() {
  long r = curMS / targetMS;
  stepper1.moveTo(alignedPos(stepper1.currentPosition(), r));
  stepper2.moveTo(alignedPos(stepper2.currentPosition(), r));
  stepper1.setSpeed(ALIGN_STEP_RATE);   // runSpeedToPosition() sets the direction
  stepper2.setSpeed(ALIGN_STEP_RATE);
  msState = ALIGNING;
}
 
// Switch mode pins and rescale step counters so psi/phi stay the same
void applyMicrostep(uint8_t ms) {
  long p1 = stepper1.currentPosition();
  long p2 = stepper2.currentPosition();
  if (ms > curMS)      { long r = ms / curMS; p1 *= r; p2 *= r; }
  else if (ms < curMS) { long r = curMS / ms; p1 /= r; p2 /= r; }   // exact after alignment
  setMicrostepPins(ms);
  curMS = ms;
  stepper1.setCurrentPosition(p1);
  stepper2.setCurrentPosition(p2);
  updateKinematics();
  hardStop();
}
 
// =====================================================================
// Homing - motion primitives (all in raw coordinates)
// =====================================================================
 
enum Prim { PR_SEEK, PR_DECEL, PR_BACKOFF, PR_MOVE, PR_WAIT_ALIGN };
Prim  prim;
uint8_t primAxis;            // 0 = psi, 1 = phi
float primDir;               // +1 / -1 : search direction on that axis
float primSpeed, primMax, primStart, primMargin, primEdge;
bool  primHardStop;
float mvPsi, mvPhi, mvVPsi, mvVPhi;
const __FlashStringHelper* primErr;
 
float homeAcc() { return rpmToSteps(HOME_ACCEL_RPMS); }
float rawAxis(uint8_t a) { return a ? rawPhi() : rawPsi(); }
 
void axisVel(uint8_t a, float v, float dt) {
  rampTo(a ? 0 : v, a ? v : 0, dt, MAX_STEP_RATE, homeAcc());
}
 
// Move to a raw target with a sqrt (deceleration) profile. Returns true when arrived and stopped.
bool driveTo(float psiT, float phiT, float vPsi, float vPhi, float dt) {
  float accS = homeAcc();
  float aPsi = 0.5f * accS / kPsi, aPhi = 0.5f * accS / kPhi;
  float tolPsi = 1.0f / kPsi,      tolPhi = 1.0f / kPhi;
  float ePsi = psiT - rawPsi(),    ePhi = phiT - rawPhi();
  float psiD = 0, phiD = 0;
  if (fabs(ePsi) > tolPsi) psiD = sgn(ePsi) * min(vPsi, sqrt(2.0f * aPsi * fabs(ePsi)));
  if (fabs(ePhi) > tolPhi) phiD = sgn(ePhi) * min(vPhi, sqrt(2.0f * aPhi * fabs(ePhi)));
  rampTo(psiD, phiD, dt, MAX_STEP_RATE, accS);
  return psiD == 0 && phiD == 0 && isStopped();
}
 
// Move in direction dir on one axis until the switch is pressed
void primSeek(uint8_t axis, float dir, float speed, float maxTravel, bool hardStopOnEdge,
              const __FlashStringHelper* err) {
  prim = PR_SEEK; primAxis = axis; primDir = dir; primSpeed = speed; primMax = maxTravel;
  primHardStop = hardStopOnEdge; primErr = err;
  primStart = rawAxis(axis);
  limitEdge = false;
  if (limitState) {            // already pressed: edge is here
    edgePsiRaw = rawPsi(); edgePhiRaw = rawPhi(); limitEdge = true;
  }
}
 
// Move opposite to dir on one axis until the switch is released, then a margin more
void primBackoff(uint8_t axis, float dir, float speed, float margin, float maxTravel,
                 const __FlashStringHelper* err) {
  prim = PR_BACKOFF; primAxis = axis; primDir = dir; primSpeed = speed; primMargin = margin;
  primMax = maxTravel; primErr = err;
  primStart = rawAxis(axis);
}
 
void primMove(float psiT, float phiT, float vPsi, float vPhi) {
  prim = PR_MOVE; mvPsi = psiT; mvPhi = phiT; mvVPsi = vPsi; mvVPhi = vPhi;
  primErr = F("move");
}
 
// Returns 0 running, 1 done, -1 error
int8_t runPrim(float dt) {
  switch (prim) {
    case PR_SEEK:
      if (limitEdge) {
        limitEdge = false;
        primEdge = primAxis ? edgePhiRaw : edgePsiRaw;
        if (primHardStop) { hardStop(); return 1; }   // slow approach: stop right here
        prim = PR_DECEL;                              // fast approach: decelerate
        return 0;
      }
      if (primDir * (rawAxis(primAxis) - primStart) > primMax) return -1;
      axisVel(primAxis, primDir * primSpeed, dt);
      return 0;
 
    case PR_DECEL:
      rampTo(0, 0, dt, MAX_STEP_RATE, homeAcc());
      return isStopped() ? 1 : 0;
 
    case PR_BACKOFF:
      if (!limitState) {
        float p = rawPsi(), f = rawPhi();
        if (primAxis) f -= primDir * primMargin; else p -= primDir * primMargin;
        primMove(p, f, PSI_BACK_DPS, PHI_BACK_DPS);
        return 0;
      }
      if (primDir * (primStart - rawAxis(primAxis)) > primMax) return -1;
      axisVel(primAxis, -primDir * primSpeed, dt);
      return 0;
 
    case PR_MOVE:
      return driveTo(mvPsi, mvPhi, mvVPsi, mvVPhi, dt) ? 1 : 0;
 
    case PR_WAIT_ALIGN:               // alignment move before a coarser microstep
      return (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) ? 1 : 0;
  }
  return 0;
}
 
// =====================================================================
// Homing - sequence
// =====================================================================
 
enum HomeState { H_IDLE, H_RUN, H_STOPPING };
HomeState homeState = H_IDLE;
bool      homeArmed = false;        // CH8 must be released before a new start
 
enum HStep { HS_PREP, HS_ALIGN, HS_CLEAR, HS_PSI_FAST, HS_PSI_BACK, HS_PSI_SLOW, HS_PSI_RETREAT, HS_PSI_DROP, HS_PHI_TURN,
             HS_GOTO_BUMP, HS_BUMP_AWAY, HS_PHI_FAST, HS_PHI_BACK, HS_PHI_SLOW, HS_GOTO_ZERO };
uint8_t hStep;
uint8_t hPass;
float   psiPass[2];
float   psiRefRaw, newPsiOff, newPhiOff;
 
void hEnter(uint8_t s) {
  hStep = s;
  switch (s) {
    case HS_PREP:                       // make sure the wrist is stopped
      prim = PR_DECEL; primErr = F("prep");
      break;
    case HS_CLEAR:
      Serial.println(F("HOME: switch pressed, backing psi off"));
      primBackoff(0, 1.0f, PSI_BACK_DPS, PSI_MARGIN_DEG, PSI_BACK_MAX, F("switch never released (psi)"));
      break;
    case HS_PSI_FAST:
      Serial.print(F("HOME: psi pass ")); Serial.println(hPass + 1);
      primSeek(0, 1.0f, PSI_FAST_DPS, PSI_SEEK_MAX, false, F("psi switch not found"));
      break;
    case HS_PSI_BACK:
      primBackoff(0, 1.0f, PSI_BACK_DPS, PSI_MARGIN_DEG, PSI_BACK_MAX, F("switch never released (psi)"));
      break;
    case HS_PSI_SLOW:
      primSeek(0, 1.0f, PSI_SLOW_DPS, PSI_SLOW_MAX, true, F("psi switch lost on slow approach"));
      break;
    case HS_PSI_RETREAT:
      primBackoff(0, 1.0f, PSI_BACK_DPS, PSI_MARGIN_DEG, PSI_BACK_MAX, F("switch never released (psi)"));
      break;
    case HS_PSI_DROP:
      Serial.println(F("HOME: lowering psi before rotating phi"));
      primMove(psiPass[0] - PSI_TURN_CLEAR_DEG, rawPhi(), PSI_BACK_DPS, PHI_BACK_DPS);
      break;
    case HS_PHI_TURN:
      Serial.println(F("HOME: rotating phi to move the bump away"));
      primMove(rawPsi(), rawPhi() + PHI_HOME_DIR * PHI_CHECK_DEG, GOTO_PSI_DPS, GOTO_PHI_DPS);
      break;
    case HS_GOTO_BUMP:
      Serial.println(F("HOME: going to psi=89 (from below)"));
      primMove(psiRefRaw - (PSI_REF_DEG - PSI_BUMP_DEG), rawPhi(), PSI_BACK_DPS, PHI_BACK_DPS);
      break;
    case HS_BUMP_AWAY:
      Serial.println(F("HOME: switch pressed at psi=89, bump is there, rotating phi away"));
      primMove(rawPsi(), rawPhi() + PHI_HOME_DIR * PHI_CHECK_DEG, GOTO_PSI_DPS, GOTO_PHI_DPS);
      break;
    case HS_PHI_FAST:
      Serial.println(F("HOME: searching the bump (phi)"));
      primSeek(1, PHI_HOME_DIR, PHI_FAST_DPS, PHI_SEEK_MAX, false, F("bump not found (phi)"));
      break;
    case HS_PHI_BACK:
      primBackoff(1, PHI_HOME_DIR, PHI_BACK_DPS, PHI_MARGIN_DEG, PHI_BACK_MAX, F("switch never released (phi)"));
      break;
    case HS_PHI_SLOW:
      primSeek(1, PHI_HOME_DIR, PHI_SLOW_DPS, PHI_SLOW_MAX, true, F("bump lost on slow approach"));
      break;
    case HS_GOTO_ZERO:
      Serial.println(F("HOME: going to psi=0 phi=0"));
      primMove(newPsiOff, newPhiOff, GOTO_PSI_DPS, GOTO_PHI_DPS);   // raw target of real (0,0)
      break;
  }
}
 
void homingStart() {
  Serial.println(F("HOME: start"));
  homeState = H_RUN;
  hPass = 0;
  hEnter(HS_PREP);
}
 
void homingStop(const __FlashStringHelper* msg) {
  Serial.print(F("HOME: "));
  Serial.print(msg);
  Serial.println(F(" -> stopped, offsets unchanged"));
  homeState = H_STOPPING;
  if (msState == ALIGNING) msState = STOPPING;   // interrupted alignment: normal logic redoes it
}
 
void homingUpdate(float dt) {
  int8_t r = runPrim(dt);
  if (r < 0) {
    Serial.print(F("HOME ERROR: "));
    Serial.println(primErr);
    homingStop(F("error"));
    return;
  }
  if (r == 0) return;
 
  switch (hStep) {
    case HS_PREP:
      if (curMS != HOME_MS) {
        if (HOME_MS > curMS) {          // finer: exact rescale, no move needed
          applyMicrostep(HOME_MS);
        } else {                        // coarser: small alignment move first
          targetMS = HOME_MS;
          startAlign();                 // msState = ALIGNING, loop steps to the aligned position
          hStep = HS_ALIGN;
          prim  = PR_WAIT_ALIGN;
          primErr = F("align");
          break;
        }
        Serial.print(F("HOME: microstep set to 1/")); Serial.println(curMS);
      }
      if (limitState) hEnter(HS_CLEAR);
      else            hEnter(HS_PSI_FAST);
      break;
 
    case HS_ALIGN:
      applyMicrostep(HOME_MS);
      msState = RUNNING;
      Serial.print(F("HOME: microstep set to 1/")); Serial.println(curMS);
      if (limitState) hEnter(HS_CLEAR);
      else            hEnter(HS_PSI_FAST);
      break;
 
    case HS_CLEAR:     hEnter(HS_PSI_FAST); break;
    case HS_PSI_FAST:  hEnter(HS_PSI_BACK); break;
    case HS_PSI_BACK:  hEnter(HS_PSI_SLOW); break;
 
    case HS_PSI_SLOW:
      psiPass[hPass] = primEdge;
      Serial.print(F("HOME: psi trigger (raw) = ")); Serial.println(primEdge, 3);
      hEnter(HS_PSI_RETREAT);
      break;
 
    case HS_PSI_RETREAT:
      if (hPass == 0) {
        hPass = 1;
        hEnter(HS_PSI_DROP);
      } else {
        float d = psiPass[1] - psiPass[0];
        if (fabs(d) > PASS_MAX_DIFF) {
          Serial.println(F("HOME ERROR: psi passes too different (lost steps?)"));
          homingStop(F("error"));
          return;
        }
        if (d > BUMP_TOL_DEG)       Serial.println(F("HOME: pass 1 hit the bump, using pass 2"));
        else if (d < -BUMP_TOL_DEG) Serial.println(F("HOME: pass 2 hit the bump, using pass 1"));
        psiRefRaw = max(psiPass[0], psiPass[1]);       // the body triggers later than the bump
        hEnter(HS_GOTO_BUMP);
      }
      break;
 
    case HS_PSI_DROP:  hEnter(HS_PHI_TURN); break;
    case HS_PHI_TURN:
      if (limitState) {
        Serial.println(F("HOME ERROR: switch pressed after phi rotation (increase PSI_TURN_CLEAR_DEG)"));
        homingStop(F("error"));
        return;
      }
      hEnter(HS_PSI_FAST);
      break;
 
    case HS_GOTO_BUMP:
      if (limitState) hEnter(HS_BUMP_AWAY);
      else            hEnter(HS_PHI_FAST);
      break;
 
    case HS_BUMP_AWAY:
      if (limitState) {
        Serial.println(F("HOME ERROR: switch still pressed at psi=89 (check PSI_BUMP_DEG)"));
        homingStop(F("error"));
        return;
      }
      hEnter(HS_PHI_FAST);
      break;
 
    case HS_PHI_FAST:  hEnter(HS_PHI_BACK); break;
    case HS_PHI_BACK:  hEnter(HS_PHI_SLOW); break;
 
    case HS_PHI_SLOW:
      newPsiOff = psiRefRaw - PSI_REF_DEG;
      newPhiOff = primEdge - PHI_BUMP_DEG;   // bump edge is at phi = PHI_BUMP_DEG
      Serial.print(F("HOME: phi trigger (raw) = ")); Serial.println(primEdge, 3);
      hEnter(HS_GOTO_ZERO);
      break;
 
    case HS_GOTO_ZERO:
      psiOffset = newPsiOff;          // commit only now
      phiOffset = newPhiOff;
      homed = true;
      homeState = H_IDLE;
      Serial.print(F("HOME: done. psiOffset=")); Serial.print(psiOffset, 3);
      Serial.print(F(" phiOffset="));            Serial.println(phiOffset, 3);
      break;
  }
}
 
// =====================================================================
// Hi gesture - phi shake then psi nod around the start position
// =====================================================================
 
enum HiState { HI_IDLE, HI_RUN, HI_STOPPING };
HiState hiState = HI_IDLE;
bool    hiArmed = false;            // CH10 must be released before a new gesture
uint8_t hiIdx;                      // current waypoint
float   hiPsi0, hiPhi0;             // start position (raw)
 
// Waypoint k of the gesture (raw). Returns false when the gesture is over.
bool hiTarget(uint8_t k, float &psi, float &phi) {
  psi = hiPsi0;
  phi = hiPhi0;
  if (k < 2 * HI_PHI_SHAKES) { phi += (k & 1) ? -HI_PHI_DEG : HI_PHI_DEG; return true; }
  k -= 2 * HI_PHI_SHAKES;
  if (k < 2 * HI_PSI_SHAKES) {
    psi += (k & 1) ? -HI_PSI_DEG : HI_PSI_DEG;
    if (homed) psi = constrain(psi, PSI_MIN_DEG + psiOffset, PSI_MAX_DEG + psiOffset);
    return true;
  }
  return k == 2 * HI_PSI_SHAKES;    // last waypoint: back to the start
}
 
// Start the move to waypoint hiIdx. Returns false when the gesture is over.
bool hiNext() {
  float psi, phi;
  if (!hiTarget(hiIdx, psi, phi)) return false;
  primMove(psi, phi, HI_PSI_DPS, HI_PHI_DPS);
  return true;
}
 
void hiStart() {
  Serial.println(F("HI: hello!"));
  hiPsi0  = rawPsi();
  hiPhi0  = rawPhi();
  hiIdx   = 0;
  hiState = HI_RUN;
  hiNext();
}
 
void hiUpdate(float dt) {
  if (runPrim(dt) != 1) return;     // PR_MOVE never fails
  hiIdx++;
  if (!hiNext()) hiState = HI_IDLE;
}
 
// =====================================================================
// Control state machine (every UPDATE_MS)
// =====================================================================
 
void updateControl(float dt, bool signalOk) {
  if (signalOk) desiredMS = readDesiredMS();
 
  bool homeBtn = signalOk && safeChannel(CH_HOME) > 1750;
  bool hiBtn   = signalOk && safeChannel(CH_HI) > 1750;
  if (signalOk && safeChannel(CH_HOME) < 1250) homeArmed = true;
  if (signalOk && safeChannel(CH_HI) < 1250) hiArmed = true;
 
  if (!driversOn) {                                // nothing moves while disabled
    hardStop();
    if (msState == ALIGNING) msState = STOPPING;   // redo alignment when re-enabled
    if (homeState == H_RUN) homingStop(F("drivers off / signal lost"));
    homeState = H_IDLE;
    hiState   = HI_IDLE;
    return;
  }
 
  // ---- Homing (CH8) ----
  if (homeState == H_RUN) {
    if (!homeBtn) homingStop(F("CH8 released"));
    else          homingUpdate(dt);
    if (homeState == H_RUN) return;
  }
  if (homeState == H_STOPPING) {
    rampTo(0, 0, dt, MAX_STEP_RATE, homeAcc());
    if (isStopped()) homeState = H_IDLE;
    return;
  }
 
  // ---- Hi gesture ----
  if (hiState == HI_RUN) {
    if (!hiBtn) { Serial.println(F("HI: CH10 released -> stopped")); hiState = HI_STOPPING; }
    else        { hiUpdate(dt); return; }
  }
  if (hiState == HI_STOPPING) {
    rampTo(0, 0, dt, MAX_STEP_RATE, homeAcc());
    if (isStopped()) hiState = HI_IDLE;
    return;
  }
 
  // ---- Start requests ----
  if (homeBtn && homeArmed && msState == RUNNING) {
    homeArmed = false;
    homingStart();
    return;
  }
  if (hiBtn && hiArmed && msState == RUNNING) {
    hiArmed = false;
    hiStart();
    return;
  }
 
  // ---- Normal control + microstep changes ----
  switch (msState) {
    case RUNNING:
      if (desiredMS != curMS) { targetMS = desiredMS; msState = STOPPING; }
      updateMotion(dt, msState == RUNNING);
      break;
 
    case STOPPING:
      if (desiredMS == curMS) { msState = RUNNING; updateMotion(dt, true); break; }
      targetMS = desiredMS;
      updateMotion(dt, false);              // ramp down to zero
      if (isStopped()) {
        if (targetMS < curMS) startAlign(); // coarser: align first
        else { applyMicrostep(targetMS); msState = RUNNING; }
      }
      break;
 
    case ALIGNING:
      if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
        applyMicrostep(targetMS);
        msState = RUNNING;
      }
      break;
  }
}
 
// =====================================================================
// Debug
// =====================================================================
 
void printStatus(bool signalOk) {
  Serial.print(driversOn  ? F("EN:ON ")   : F("EN:OFF "));
  Serial.print(limitState ? F("LIM:HIT ") : F("LIM:free "));
  if (!signalOk) Serial.print(F("NOSIG "));
  Serial.print(homed ? F("HOMED ") : F("NOTHOMED "));
  if (homeState == H_RUN) { Serial.print(F("HOMING:")); Serial.print(hStep); Serial.print(' '); }
  if (hiState == HI_RUN) Serial.print(F("HI "));
  Serial.print(F("MS:1/")); Serial.print(curMS);
  if (msState == STOPPING) { Serial.print(F("->1/")); Serial.print(targetMS); Serial.print(F("(stop)")); }
  if (msState == ALIGNING) { Serial.print(F("->1/")); Serial.print(targetMS); Serial.print(F("(align)")); }
  Serial.print(F(" psi:"));  Serial.print(getPsi(), 2);
  Serial.print(F(" phi:"));  Serial.print(getPhi(), 2);
  Serial.print(F(" dpsi:")); Serial.print(psiRate, 1);
  Serial.print(F(" dphi:")); Serial.print(phiRate, 1);
  Serial.print(F(" bad="));  Serial.println(badFrames);
}
 
// =====================================================================
// Setup / Loop
// =====================================================================
 
void setup() {
  // Drivers disabled first
  pinMode(PIN_EN, OUTPUT);
  digitalWrite(PIN_EN, HIGH);
  driversOn = false;
 
  pinMode(PIN_MODE0, OUTPUT);
  pinMode(PIN_MODE1, OUTPUT);
  pinMode(PIN_MODE2, OUTPUT);
  setMicrostepPins(curMS);
  updateKinematics();
 
  pinMode(PIN_LIMIT, INPUT_PULLUP);
  limitRawPrev = limitRaw();
  limitState   = limitRawPrev;
 
  stepper1.setPinsInverted(MOTOR1_INVERT, false, false);
  stepper2.setPinsInverted(MOTOR2_INVERT, false, false);
  stepper1.setMinPulseWidth(2);          // DRV8825 needs >= 1.9 us
  stepper2.setMinPulseWidth(2);
  stepper1.setMaxSpeed(MAX_STEP_RATE);   // setSpeed() is clamped to this
  stepper2.setMaxSpeed(MAX_STEP_RATE);
 
  // Unknown absolute position: start at psi = phi = 0 (not homed)
  stepper1.setCurrentPosition(0);
  stepper2.setCurrentPosition(0);
  hardStop();
 
  Serial.begin(115200);       // USB to computer
  ibusSerial.begin(115200);   // iBUS from FS-iA6B
  Serial.println(F("Differential wrist ready, psi=phi=0 (not homed)"));
  Serial.println(F("CH1=phi CH2=psi CH5=Vmax CH6=Accel CH7=Enable CH8=Home CH9=Microstep CH10=Hi"));
}
 
void loop() {
  // Stepping (as often as possible)
  if (driversOn) {
    if (msState == ALIGNING) {
      stepper1.runSpeedToPosition();
      stepper2.runSpeedToPosition();
    } else {
      stepper1.runSpeed();
      stepper2.runSpeed();
    }
  }
 
  updateLimit();
 
  if (readIBus()) {
    haveFrame = true;
    lastFrame = millis();
  }
 
  uint32_t now = millis();
  bool signalOk = haveFrame && (now - lastFrame) < FAILSAFE_MS;
 
  // ---- FAILSAFE + EN from CH7 ----
  // No valid iBUS frame for FAILSAFE_MS -> motors disabled immediately.
  // After a loss (and at power-up) CH7 must be seen OFF before motors can be enabled again.
  static bool wasOk = false;
  if (!signalOk) {
    if (wasOk) Serial.println(F("# FAILSAFE: radio signal lost -> motors disabled"));
    wasOk       = false;
    enableArmed = false;
    setDriversEnabled(false);
  } else {
    if (!wasOk) Serial.println(F("# FAILSAFE: radio signal OK - set CH7 OFF then ON to enable motors"));
    wasOk = true;
    // Only a clear ON (>1750) enables. Anything else (OFF, or 1500 = receiver
    // neutral/failsafe output) disables. Only a real OFF (<1250) re-arms.
    uint16_t v = safeChannel(CH_ENABLE);
    if (v > 1750 && enableArmed) {
      setDriversEnabled(true);
    } else if (v <= 1750) {
      setDriversEnabled(false);
      if (v < 1250) enableArmed = true;
    }
  }
 
  // Control update
  static uint32_t lastUpdate = 0;
  if (now - lastUpdate >= UPDATE_MS) {
    float dt = (now - lastUpdate) / 1000.0f;
    lastUpdate = now;
    if (dt > 0.1f) dt = 0.1f;        // first call / long pause
    updateControl(dt, signalOk);
  }
 
  // Debug output
  static uint32_t lastPrint = 0;
  if (now - lastPrint >= PRINT_MS) {
    lastPrint = now;
    printStatus(signalOk);
  }
}