TimerOne: a periodic interrupt and custom PWM from Timer1

// Satellite tracker: a stepper turns the dish in azimuth, a servo lifts it in
// elevation, and an LCD counts down to the pass.
//
//   pins 11..8   28BYJ-48 on its ULN2003 board: IN1, IN2, IN3, IN4
//   pin 7        elevation servo: 0 is the horizon, 90 straight up
//   pin 2        START button: predict and fly the next demo pass
//   pin 5        green lamp: the satellite is above the horizon
//   pin 6        buzzer: a chirp at rise and at set
//   A4, A5       16x2 LCD on its I2C backpack at 0x27
//
// # Where is the satellite?
//
// Real trackers use SGP4 with a fresh TLE (two-line element set) from a
// catalog, because a real orbit is squashed, tilted and slowly dragged down by
// the air. This sketch uses a simpler model that still shows every idea: a
// circular orbit 420 km up at 51.6 degrees inclination, a round Earth turning
// underneath, and an observer at 40 degrees north. Each update it finds the
// satellite in space, finds you in space, and turns the difference into an
// azimuth (compass bearing) and an elevation (angle above the horizon).
//
// # The demo pass
//
// Press START and the sketch sets up an orbit that passes over you a few
// minutes later, predicts when it will rise (AOS, acquisition of signal), how
// high it will get and when it will set (LOS, loss of signal), then counts
// down and tracks it. Time runs 30 times faster than real time, so a ten
// minute pass takes about twenty seconds. Each press flies a different pass,
// so press again to skip ahead to the next one.
//
// # Over the top
//
// Near the top of a high pass the azimuth swings fast, faster than a small
// stepper can follow. So for a pass above 70 degrees the tracker flips: it
// points the base the opposite way and tips the servo past vertical, which
// turns that fast swing into a gentle one.
//
// # Two jobs at once
//
// The orbit sums take a while in software floating point, and the LCD takes
// longer still: every character is six writes to its I2C backpack. A stepper
// that only moves when loop() comes round would stutter through all of that.
// So Timer1 interrupts 2000 times a second and steps the motor from there,
// and loop() only ever changes where it is heading.
//
// Point the dish north before power-up: a stepper does not know where it is.

#include <Wire.h>
#include <Servo.h>
#include <AccelStepper.h>
#include <LiquidCrystal_I2C.h>
#include <TimerOne.h>

const int PIN_START = 2;
const int PIN_LAMP = 5;
const int PIN_EL = 7;
const int PIN_BUZZER = 6;
const long STEPS_PER_TURN = 4096;   // half steps, through the 64:1 gearbox
const float WARP = 30.0;            // demo time runs this much faster

// The model, in kilometers, seconds and radians.
const float EARTH_KM = 6371.0;
const float ORBIT_KM = EARTH_KM + 420.0;
const float MU = 398600.4;          // Earth's gravitational parameter, km^3/s^2
const float EARTH_SPIN = 7.292116e-5;
const float INCLINATION = 51.6 * DEG_TO_RAD;
const float MY_LAT = 40.0 * DEG_TO_RAD;
const float PASS_IN = 420.0;        // seconds until the top of the pass
const float OFFSETS[] = {2.0, -4.0, 0.5, 6.0};   // how far north it passes, degrees

// AccelStepper takes the pins in the order IN1, IN3, IN2, IN4.
AccelStepper base(AccelStepper::HALF4WIRE, 11, 9, 10, 8);
Servo tilt;
LiquidCrystal_I2C lcd(0x27, 16, 2);

void stepMotor() { base.run(); }   // from the Timer1 interrupt

float meanMotion;                   // radians a second around the orbit
float raan, u0;                     // where this pass's orbit sits
float aosT, losT, maxEl, maxT;
bool flip = false;
int passNumber = 0;
enum Mode { READY, COUNTDOWN, TRACKING, DONE } mode = READY;
unsigned long startedAt = 0;
unsigned long shownAt = 0;
bool wasPressed = false;

// --- the orbit ------------------------------------------------------------------

void setUpPass(float offsetDeg) {
  // Put the ground track over latitude MY_LAT + offset, on our meridian, at
  // PASS_IN seconds from now.
  float lat = MY_LAT + offsetDeg * DEG_TO_RAD;
  float up = asin(sin(lat) / sin(INCLINATION));
  float alpha = atan2(cos(INCLINATION) * sin(up), cos(up));
  raan = EARTH_SPIN * PASS_IN - alpha;
  u0 = up - meanMotion * PASS_IN;
}

// Azimuth and elevation in degrees, t seconds after START.
void lookAngles(float t, float &az, float &el) {
  float u = u0 + meanMotion * t;
  float cu = cos(u), su = sin(u), cr = cos(raan), sr = sin(raan), ci = cos(INCLINATION);
  float x = ORBIT_KM * (cr * cu - sr * su * ci);
  float y = ORBIT_KM * (sr * cu + cr * su * ci);
  float z = ORBIT_KM * su * sin(INCLINATION);
  float th = EARTH_SPIN * t;                     // how far Earth has turned
  float ct = cos(th), st = sin(th), cl = cos(MY_LAT), sl = sin(MY_LAT);
  float rx = x - EARTH_KM * cl * ct;
  float ry = y - EARTH_KM * cl * st;
  float rz = z - EARTH_KM * sl;
  // Into south, east and up at the observer.
  float s = sl * ct * rx + sl * st * ry - cl * rz;
  float e = -st * rx + ct * ry;
  float up = cl * ct * rx + cl * st * ry + sl * rz;
  float range = sqrt(rx * rx + ry * ry + rz * rz);
  el = asin(up / range) * RAD_TO_DEG;
  az = atan2(e, -s) * RAD_TO_DEG;
  if (az < 0) az += 360;
}

// Step through the next twenty minutes to find rise, peak and set.
void predict() {
  aosT = -1;
  losT = -1;
  maxEl = -90;
  for (float t = 0; t < 1200; t += 20) {
    float az, el;
    lookAngles(t, az, el);
    if (el > 0 && aosT < 0) aosT = t;
    if (el > 0) losT = t;
    if (el > maxEl) {
      maxEl = el;
      maxT = t;
    }
  }
  flip = maxEl > 70;
}

// --- pointing ---------------------------------------------------------------------