// ################################################################################
//
//  ESP32 Wii Classic Reach Robot v0.00 (R2)
//
//  Released:  24/07/2027
//
//  Author: TechKnowTone
//
// ################################################################################
/*
    TERMS OF USE: This software is furnished "as is", without technical support, and
    with no warranty, expressed or implied, as to its usefulness for any purpose. In
    no event shall the author or copyright holder be liable for any claim, damages,
    or other liability, whether in an action of contract, tort or otherwise, arising
    from, out of or in connection with the software or the use or other dealings in
    the software.

    Microcontroller: ESP32 Dev Module     v3.0.2

    This code controls a reach robot, driven by 4 servo motors. You can control it
    directly from keyboard commands via the serial monitor interface or via a Wii
    Classic controller using the I2C interface. With a series of simple commands you
    can direct a 'Move' engine to perform a series of tasks, including randomly
    selecting predefined positions.

    This version focuses on using the Wii controller. If a controller is detected it
    polls the device and responds accordingly. If the Wii controller is removed the
    robot moves to the reset position and powers off servos. Then waits for a
    controller to be re-connected.

    When no controller is connected the robot can be put into 'demo' mode by holding
    down the left SW0 button switch. Pressing either button after this will return
    the robot to rest.

    This code enables the robot to connect to multiple Wii Transcievers using ESP-NOW
    
    IMPORTANT - Espressif changed the way ESP-NOW works from v3.0, which broke the original
    code and caused compiler errors. This version has been modified to work with v3.0+.
    Enjoy!
*/
// Declare and initialise global variables
// #include <Arduino.h>
// #include <HardwareSerial.h>     // serial library
#include <Adafruit_NeoPixel.h>  // Neopixel library     v1.12.3
#include "Commands.h"           // public constants
#include <ESP32Servo.h>         // servo library        v3.0.5
// #include <Wire.h>               // I2C for display and Wii Classic
#include "SSD1306Wire.h"        // OLED display lib     v4.6.1

#include <esp_now.h>            // ESP-NOW WiFi link library
#include <WiFi.h>               // ESP WiFi library

// Configuration
String Release$ = "ESP32 Wii Classic Reach Robot vR2";
String Date$ = "24/07/2024";

// Display Settings
const int I2C_DISPLAY_ADDRESS = 0x3c;
const int SDA_PIN = 21;
const int SCL_PIN = 22;

// Initialize the oled display for address 0x3c
SSD1306Wire display(I2C_DISPLAY_ADDRESS, SDA_PIN, SCL_PIN);

// Define servo calibration constants
#define fwdArmMax 2162    // forward arm Max servo value, default = 1450
#define fwdArmMin 773     // forward arm Min servo value, default = 900
#define fwdArmVert 1318   // forward arm vertical servo value, default = 1100
#define gripClose 1109    // jaws closed servo value, default = 1000
#define gripOpen 1500     // jaws moderately open value (23%), default = 1500
#define gripWide 2011     // jaws wide open values, default = 2000
#define turntableCtr 1500 // turntable servo centre value, default = 1500
#define turntableMax 2062 // turntable servo Max value, default = 1800
#define turntableMin 838  // turntable servo Min value, default = 1200
#define vertArmMaxA 1894  // vertical arm Max 'A' servo value, default = 1900
#define vertArmMaxB 2200  // vertical arm Max 'B' servo value, default = 2000
#define vertArmMinA  942  // vertical arm Min 'A' servo value, default = 1650
#define vertArmMinB 1192  // vertical arm Min 'B' servo value, default = 1650
#define vertArmMinC 1922  // vertical arm Min 'C' servo value, default = 1900
#define servoMinPWM  500  // minimum servo limit when uncalibrated
#define servoMaxPWM 2400  // maximum servo limit when uncalibrated

// calculate fixed differential values
long fwdArmFwdDiff = fwdArmMax - fwdArmVert;
long fwdArmBwdDiff = fwdArmVert - fwdArmMin;
long vertArmFwdMaxDiff = vertArmMaxB - vertArmMaxA;
long vertArmFwdMinDiff = vertArmMinB - vertArmMinA;
long vertArmBwdMaxDiff = vertArmMaxB - vertArmMaxB; // 0
long vertArmBwdMinDiff = vertArmMinC - vertArmMinB;

#define Floor0 turntableCtr   // floor position for servo 0
#define Floor1 1725           // floor position for servo 1
#define Floor2 1135           // floor position for servo 2
#define Floor3 gripOpen       // floor position for servo 3
#define Home0 turntableCtr    // home position for servo 0
#define Home1 fwdArmVert      // home position for servo 1
#define Home2 vertArmMinC     // home position for servo 2
#define Home3 gripClose       // home position for servo 3
#define PWR_tMax 3000         // 3 second time-out for auto-power off
#define PWR_tPing 500         // 500ms time-out on receipt of a ping character
#define Reset0 turntableCtr   // RESET position for servo 0
#define Reset1 fwdArmMin      // RESET position for servo 1
#define Reset2 vertArmMinC    // RESET position for servo 2
#define Reset3 gripClose      // RESET position for servo 3
#define servoOffMax 0         // sets maximum thermal drift offset for servo 0
#define servoOffRmpDwn 100000 // sets thermal offset ramp down time in miliseconds
#define servoOffRmpUp 10000   // sets thermal offset ramps up time in miliseconds

// Define general constants
#define AddPntNum 16          // number of elements in the playAddPnt[] array
#define BatCal 377.1          // calibrated voltage multiplying factor, default 404.85 
#define BatCritical 2489      // critical battery threshold @6.6v, default = 2672, set == 0 to ignore
#define BatMax 3092           // A0 for fully charged battery voltage, == 8.2v
#define BatPin 36             // analog input pin ADC0 on GPIO36
#define BatUSB 1858           // if <= then USB supply assumed
#define gosubDepth 20         // depth of gosub stack
#define LEDPin 2              // ESP32 LED GPIO output pin
#define MemRows 500           // no. rows in the move memory array
#define MemWidth 4            // no. of bytes in a memory row, servo angles + steps
#define moveArraySize (MemRows * MemWidth)  // number of elements in the move sequence * 4
#define moveDefInterval 10000 // default interval for move loop = 10ms
#define NeoPin 19             // assign GPIO19 for RGB LEDs
#define posMax 32             // depth of movePos arrays
#define sw0Pin 13             // left switch SW0 assigned to GPIO13
#define sw1Pin 15             // right switch SW1 assigned to GPIO15
#define wiiAdd 0x52           // Wii controller I2C bus address

// define arrays
int forStack[10];           // stack for nested For...Next loops, line pointers
int gosubStack[gosubDepth]; // stack for nested Gosub calls, line pointers
int moves[moveArraySize];   // memory reserved for movement commands/data
int moveLabels[10];         // move play sequence labels
int movePosF[posMax];       // store for servo[1] target values
int movePosR[posMax];       // store for servo[0] target values
int movePosV[posMax];       // store for servo[2] target values
int playAddPnt[AddPntNum];  // store for line point branch vectors
int numStack[10];           // stack for nested For...Next loops, number of times
int servoPins[] = { 27,  26,  25,  33 };  // servo output pins assigned
int servoMax[] = {turntableMax,fwdArmMax,vertArmMaxB,gripWide};  // servo max pulse
int servoMin[] = {turntableMin, fwdArmMin, vertArmMinA, gripClose};  // servo max pulse
int servoCtr[] = {Reset0,Reset1,Reset2,Reset3}; // value sent to servos
int servoVal[] = {Reset0,Reset1,Reset2,Reset3}; // value sent to servos
int servoTgt[4];            // target values for servos
int snapshot[4];            // temporary values for a position to be remembered

// define Wii specific items
# define WiiLED A3          // LED used to indicate Wii plugged in
bool WiiBlock;              // = true blocks Wii controller demand actions
byte WiiCnt;                // received byte counter
uint8_t WiiData[6];         // array to store Wii output
byte WiiError;              // used to detect transmission errors
bool WiiJoy;                // = true when there is a joystick demand
int WiiLX,WiiLY;            // left joystick values
byte wiiPhase;              // used to perform wii functions at 50 Hz
int WiiRX,WiiRY;            // right joystick values

// you need to build a Wii Transceiver and get its MAC address
uint8_t broadcastAddress[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

// 250 byte max for Tx/Rx data
// It will be used to create the Tx/Rx data structures
typedef struct ESPNOW_Buff250 {
    char ESPdata[250];
} ESPNOW_Buff250;

// Create a 250 byte Tx buffer
ESPNOW_Buff250 Tx_Buff;

// Create a 250 byte receive
ESPNOW_Buff250 Rx_Buff;

// Declare and initialise global variables
int Angle;                    // start with servo in the centre pos
String Any$;                  // temporary string
int anyAny = 0;               // any temporary variable
long AnyLong;                 // any temp value
int anyVal = 0;               // any temporary variable
byte APP;                     // > 0 when connected to Windows App
byte APP_Disp;                // APP display mode
byte APPs;                    // previous APP value
int16_t BatAvg;               // preload averaging function with high value
int16_t BatAvgMax;            // maximum average value in this period
byte BatCnt;                  // pointer to array values and critical timer
int16_t BatCritCnt;           // timer used in baterycritical testing
int16_t BatData[60];          // battery averages at 1 second intervals
int16_t BatDel;               // set a 10 second delay in 'life' prediction
int16_t BatDP;                // battery data pointer, 0 - 59
int16_t BatLast;              // previous battery average reading
bool BattNow;                 // == true causes battery reading
bool BatPwr;                  // true if battery supply detected, false if USB assumed
int16_t BatQ;                 // a measure of battery quality 1 - 5
int16_t BatRead;              // timer counter used in reading battery
unsigned long BatRep;         // time when to report battery reading over serial link
long BatSum;                  // preload averaging sum with 20x max value
unsigned long BatT0;          // time of first battery measurement in life prediction forecast
String BatTime$;              // time left displayed
unsigned long BatTms;         // time when battery reading was taken
int16_t BatV0;                // reference voltage used in prediction forecast
int16_t BatV1;                // lowest voltage used in prediction forecast
int16_t BatVol;               // instantaneous battery voltage
int16_t BlinkCnt;             // time delay used in micro LED blink function
uint8_t Brightness;           // Display brightness
bool button_BA;               // A button state tracker
int button_BA_Cnt;            // button held down counter
bool button_BB;               // B button state tracker
int button_BB_Cnt;            // button held down counter
bool button_BDD;              // BDD button state tracker
bool button_BDL;              // BDL button state tracker
int button_BDL_Cnt;           // button held down counter
bool button_BDR;              // BDR button state tracker
int button_BDR_Cnt;           // button held down counter
bool button_BDU;              // BDU button state tracker
int button_BDU_Cnt;           // button held down counter
bool button_BF;               // B+ START button state tracker
bool button_BH;               // BH button state tracker
bool button_BLT;              // LT button state tracker
bool button_BRT;              // RT button state tracker
bool button_BS;               // B- SELECT button state tracker
bool button_BX;               // X button state tracker
bool button_BY;               // Y button state tracker
int button_BY_Cnt;            // button held down counter
bool button_BZL;              // ZL button state tracker
bool button_BZR;              // ZR button state tracker
bool button_HME;              // HOME button state tracker
bool button_SEL;              // SELECT button state tracker
bool button_SRT;              // START button state tracker
bool Calibrated;              // limits functionality, set to true once calibrated
int clapCnt;                  // number of claps
int clapTaskNext;             // pointer to the next task in the Play Engine
int CmdFlag;                  // global flag used when monitoring Move actions
int CmdForNum;                // number of times a For... Next loop will run
int CmdForRow;                // points to 1st row in a For...Next loop
char cmdMode;                 // command mode
int cmdRec;                   // > 0 if a '~' has been received
int cmdSgn;                   // +/- 1 to handle negative numbers, default = 1
char cmdType;                 // command type
int cmdVal;                   // value associated with a cmdType
uint8_t colBlu[] = {0,0};     // colour array - blue
uint8_t colB;                 // colour flag - blue
uint8_t colGrn[] = {0,0};     // colour array - green
uint8_t colG;                 // colour flag - green
uint8_t colRed[] = {0,0};     // colour array - red
uint8_t colR;                 // colour flag - red
bool DispBack;                // flag used to draw a back arrow on the display
bool DispChng;                // flag invokes initialisation code
bool DispClr;                 // set == true to clear the memory contents
int16_t DispCnt;              // counter used in display updating
byte DispCX,DispCY;           // display cursor coordinates
int16_t DispDel;              // >0 display delay counter, stops updates over I2C
bool DispDisp;                // set = true to immediately display memory contents
int16_t DispFlash;            // flashing display counter
bool DispInv;                 // display inverted flag
byte DispMenu;                // group type, 0 = sensors, 1 = tasks, etc
int16_t DispMode;             // the type of display for a given mode
bool DispMon;                 // == true if connected to OLED Mirror app
bool DispNOP;                 // set == true to stop display I2C updates
int16_t DispNow;              // if > 0 display special modes
int16_t DispPnt;              // points at the current display mode
int16_t DispTx;               // counter used with display mirror app
String DispTx1$;              // display text line 1
String DispTx2$;              // display text line 2
bool Echo;                    // if true send print messages
bool ESC0;                    // SW0 ESC exit current Main Task flag
bool ESC1;                    // SW1 ESC exit current Main Task flag
int16_t ESP_NOW_BA;           // broadcast address pointer, -1 to start with
bool ESP_NOW_Init;            // == true once ESP-NOW has been initiaiised
int flashCnt;                 // timer used to flash Record mode LED
int forPnt;                   // forStack[] pointer
int gosubPnt;                 // gosubStack[] pointer
int incOffset;                // > 0 flag which controls thermal offset increments
int incS0;                    // servo[0] increment
int incS1;                    // servo[1] increment
int incS2;                    // servo[2] increment
int incS3;                    // servo[3] increment
unsigned long interval;       // main loop interval in microseconds
char keyChar;                 // any keyboard character
int keyPnt;                   // pointer to last servo affected by keyboard demand
int keyRec;                   // > 0 if a key has been received
int keyVal;                   // any keyboard value
bool LED_Blink;               // flag toggled to create blinking effects
int16_t LED_Cnt;              // general counter used in LED tasks
int16_t LED_Del;              // LED Task delay, adds pauses into the processes
int16_t LED_Inc;              // general increment used in LED tasks
int16_t LED_Last;             // previous LED task pointer
unsigned long LED_Timer;      // millisecond timer used by LED tasks
bool LED_Nop;                 // == true to block LED tasks, default = false
bool LEDOP;                   // state of LED pin HIGH/LOW
int16_t LED_Pnt;              // general LED pointer
bool LED_REC;                 // record of REC status used in LED tasks
bool LEDshow;                 // LED show flag for mouth LEDs
int16_t LED_SubTask;          // LED sub-task pointer
bool LED_SwDwn;               // == true if a button switch has been pressed
int16_t LED_Task;             // LED task pointer
byte LEDVal;                  // values used in LED tasks
int loadFlag;                 // Play Engine flag indicating type of loaded data
int LSV0;                     // previous recorded servo[0] value
int LSV1;                     // previous recorded servo[1] value
int LSV2;                     // previous recorded servo[2] value
int LSV3;                     // previous recorded servo[3] value
int Mem1st;                   // pointer to 1st memory row to be played by Walk engine
int MemMax;                   // number of rows loaded into memory
int MemPnt;                   // a pointer used in loading the Mem[] array
int MemRow;                   // Mem[] row pointer used in loading the array
bool moveCalc;                // set to true after servo values have been updated
int moveCmd;                  // the current move command
int moveCnt;                  // counter used in moving to target values
int moveForLp;                // counter value for For...Next loop
int moveForPnt;               // pointer value in For...Next loop
int moveInc;                  // temp move value
bool MOVING;                  // == true when servoces are being adjusted
unsigned long moveInterval;   // move engine interval in microseconds
bool moveIntervalAdj;         // true if acceleration and braking are to be used
unsigned long moveIntervalInc; // main loop interval increment in microseconds
unsigned long moveIntervalMin; // minimum move engine interval in microseconds
int moveLast;                 // previous movePnt value
int moveMem0,moveMem1,moveMem2; // temporary pushed values
unsigned long moveMicros;     // move engine interval timer in microseconds
int movePause;                // pause after move in loop cycles, default = 0;
int movePnt;                  // movement array pointer, default is -1
int moveReturn;               // used to store return line in move engine
int moveRpt;                  // flag set to report servo values
int moveRun;                  // -1 = run, 0 = stop, >0 = run n then stop
int moveTask;                 // main task pointer in movement engine
int moveSubTask;              // sub-task pointer used in moves
long moveWait;                // wait counter in cycles, could be quite long. ie. minutes
long Mult;                    // speed multiplying factor
String myMAC;                 // MAC address if this device
unsigned long next10ms;       // 10ms timer
unsigned long next20ms;       // 20ms timer
unsigned long next40ms;       // 40ms timer
unsigned long next200ms;      // 200ms timer
int Once;                     // only = 1 from full reset
long ON_Timer = 0;            // time from power-on in loop 10 ms loop cycles
long ON_Timer_Last = 0;       // records previous time event
int16_t Ping;                 // Rx 'ping' counter
int playCnt;                  // total number of elements stored in play memory
int playPause;                // inter-move delay in ms
int playPauseDef;             // default inter-move delay in ms, = 0
long playPnt;                 // pointer using in playEngine, default = 0
bool playRepeat;              // == true if continuous repeat play is wanted
bool playRun;                 // true when playEngine is active, otherwise false
int playSpeed;                // % of default speed, normally = 100
int playTask;                 // task pointer used by the playEngine
int playWait;                 // count down timer used in the Play Engine
int playTaskNext;             // next task to be performed after a wait task
unsigned long print40ms;      // 40ms variable print timer
bool printTimeVals;           // a flag that determines when servo values are printed
int PrintTgt;                 // 0 == serial port, 1 == WiFi ESP-NOW
String PrintTx = "";          // printed strings
int PWM_Freq;                 // servo PWM frequency, default = 100;
int PWR_timeout;              // disables servos after a time-out, when in manual mode
int rate;                     // joystick demand flag, dependent on shift buttons
bool REC;                     // record mode flag, true if in record mode, false otherwise
byte REC_Disp;                // RECORD display mode
byte ResetCnt;                // timer used to invoke soft RESET
int RndLast;                  // previous random number
int16_t Rx_Chk;               // Rx checksum
int16_t Rx_len;               // length of WiFi received data block
int16_t Rx_Pnt;               // data pointer user by Rx state machine
bool RxRec;                   // set true when a valid frame of data is received
bool RxRecvEvent;             // set == true when a OnDataRecv() call back event has occured
int16_t RxState;              // receiver state machine state
int16_t Rx_Task;              // task pointer user by Rx state machine
byte RxWiFi[6];               // array holding 6 bytes of Wii received data
bool SerialRx;                // == true if any character has been received
int servo2Mod;                // auto-modified servoVal[2]
bool ServoAttOnce;            // == true once servos have been attached once
bool servoEn;                 // false = OFF, true = ON enabled status
int servoLL = 544;            // servo min time setting
int servoNum = 0;             // target servo number 0 - 3 for setting LL/UL limits, default = 0
int servoOff0;                // thermal offset for servo 0
long servoOffDec;             // thermal offset decrement counter
long servoOffDecT;            // thermal offset decrement counter total count
long servoOffInc;             // thermal offset increment counter
long servoOffIncT;            // thermal offset increment counter total count
int servoPin;                 // servo output pin
int servoUL = 2400;           // servo max time setting
int shift;                    // any shift << or >> value
int16_t sw0Cnt;               // button switch counter
int16_t sw0DwnTime;           // button pressed down time
bool sw0LastState;            // previous state of button switch, HIGH/LOW
bool SW0_Nop;                 // == true to block SW0 read tasks, default = false
bool sw0State;                // state of read button switch pin
int16_t sw0Timer;             // timer used to detemine button sequences
int16_t sw1Cnt;               // button switch counter
int16_t sw1DwnTime;           // button pressed down time
bool sw1LastState;            // previous state of button switch, HIGH/LOW
bool SW1_Nop;                 // == true to block SW1 read tasks, default = false
bool sw1State;                // state of read button switch pin
int16_t sw1Timer;             // timer used to detemine button sequences
unsigned long t0, t1;         // general timers
int16_t Task10ms;             // 10ms subtask timer
int16_t Task20ms;             // 20ms subtask timer
int16_t Task40ms;             // 40ms subtask timer
int timeDelay = 1;            // delay in milliseconds
unsigned long TimeNow;        // temporary event timer used in button pressing
int16_t Tx_PingCnt;           // 1 second Tx ping timer
bool USBPwr;                  // true if POST detects USB power voltage
bool ValChg;                  // temporary flag set when a servo value is changed
int16_t WiFiCntC;             // C button counter for WiFi enable
int16_t WiFiConCnt;           // counter used in connection retry times
bool WiFiConnected;           // == true if a WiFi connection has been established
bool WiFiEn;                  // == true when WiFi is enabled
int16_t WiFiPing;             // Rx counter used to maintain connected state
bool WiFiPrt;                 // if == true then print WiFi associated messages
int16_t WiFiRestCnt;          // counter used to goi to rest if no WiFi demands
bool WiFiRx;                  // true when a block of data has been received
long WiFiRxBytes;             // number of bytes received over the WiFi link
int16_t WiFiRxErr;            // number of bytes received over the WiFi link
bool WiFiRxRec;               // true when a block of data has been successfully received
byte WiFiRxMacAddr[6];        // senders MAC address, which could be broadcast FF:FF:FF:FF:FF:FF
int16_t WiFiTryCnt;           // WiFi try to connect count on error count
int16_t WiFiTryNum;           // WiFi try to connect total count
bool WiFiTryOnce;             // == true if first pass of trying to connect
bool WiFiTx;                  // true when a block of data has been sent successfully
int16_t WiFiTx_CB;            // == 1 call back is clear to send, == -1 resend
int16_t WiFiTx_Chk;           // Tx checksum
int16_t WiFiTx_len;           // >0 when WiFi Tx buffer contains data to be sent
long WiFiTxBytes;             // number of bytes received over the WiFi link
int16_t WiFiTxErr;            // WiFi data error
int16_t WiFiTxErrCnt;         // WiFi Tx error count, leads to disconnection
byte WiFiTxMacAddr[6];        // transmit MAC address, which should match myMAC address
int16_t WiiType;              // device type, 0 == Nunchuk, 1 == Classic
int XbutCnt;                  // X button held down counter
int YbutCnt;                  // Y button held down counter

// Declare objects
Servo servoMain; // define temp Servo case
Servo servoInst[4]; // define Reach Robot servos
 
// When we setup the NeoPixel library, we tell it how many pixels, and which pin to use to send signals.
// Note that for older NeoPixel strips you might need to change the third parameter--see the strandtest
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(2, NeoPin, NEO_GRB + NEO_KHZ800);

// ---------------------------------------------------------------------------------

void setup() {
  // put your setup code here, to run once:
  // Allow allocation of all timers
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  
  pinMode(LEDPin, OUTPUT); digitalWrite(LEDPin,LEDOP);
  pinMode(WiiLED, OUTPUT); digitalWrite(WiiLED,HIGH);  // set output pin for WiiLED
  digitalWrite(WiiLED,HIGH);    // turn OFF WiiLED
  pinMode(sw0Pin,INPUT_PULLUP); // button switch sw0 Pin
  pinMode(sw1Pin,INPUT_PULLUP); // button switch sw1 Pin

  setDefaults();        // reset variables
  
  Serial.begin(115200); // high baud rate for sweeping functions
    
  // WiFi is only enabled if the left-hand SW0 button is pressed at/after RESET
  if (!digitalRead(sw0Pin)) {
    // initialise WiFi link
    WiFiEn = true;  // this flag controls access to WiFi functions
    WiFi.mode(WIFI_STA);
    // new code for ESP32 lib v3.0+
    while (!WiFi.STA.started()) {delay(10);}
    // Once started we can get the micros MAC address
    myMAC = WiFi.macAddress();
    Serial.println("my MAC= " + String(myMAC));
    // WiFi.disconnect(); // I don't think we need this
    
    // initialise the ESP-NOW link for the first time
    Init_ESP_NOW();
  }

  runPOST();
  
  // Ensure no button switches are pressed
  WaitWhilstDwn();
  SyncTimers();
}

// ---------------------------------------------------------------------------------

void(* resetFunc) (void) = 0; //declare reset function at address 0

// ---------------------------------------------------------------------------------

void loop() {
  //
  // the first part of this loop focuses on running the two movement engines
  // and the associated servo move to target values routine
  // the movement is controlled by a separate loop timer than can be speed adjusted

  //###############################################################################
  //
  //  WiFi comms check & respond
  //
  //###############################################################################
  // if WiFi is conencted then check Rx buffer on every cycle, so that we respond to
  // received packets as soon as possible
  if (RxRecvEvent) {OnDataRecvHandler();}   // respond to ESP-NOW data received events

  if (WiFiConnected) {
    if (WiFiRx) {readWiFiRx();} // WiFi Rx buffer contains unread data so deal with it before adding new
  }

  
  //###############################################################################
  //
  //  Move calculations
  //
  //###############################################################################
  if (moveCalc) {
    // this code is triggered by the move loop timer, buy only after the servo
    // values have been written, which is synchronise with the loop timer
    moveCalc = false; // reset the flag for the next cycle
    
    // move towards target values if moveCnt > 0
    if (moveCnt > 0) {
      MOVING = true;
      if (moveCnt > 1) {
        // in the middle of a move towards the target value(s)
        for (int zP = 0; zP < 4; zP++) {
          moveInc = (servoTgt[zP] - servoVal[zP])/moveCnt;
          servoVal[zP] += moveInc;
        }
      } else {
        // moveCnt == 1 so move to target values
        servoVal[0] = servoTgt[0]; servoVal[1] = servoTgt[1];
        servoVal[2] = servoTgt[2]; servoVal[3] = servoTgt[3];
      } 
      ValChg = true; // trigger a servo update
      if (servoVal[0] != servoTgt[0]) {incOffset = 10;} // increment thermal counter for servo0
      setVertMinMax(); // ensure vertical channel is within limits
      // decrement the move counter whilst 
      moveCnt--;
    } else {MOVING = false;}  // clear the movement flag once counter == 0
    // check to see if the move engine is active; if so run it
    if (REC) {
      // in record/play mode
      if (playRun) {
        playEngine(); // play engine is active
        PrintTx += "moveInterval=" + String(moveInterval) + "\n";
      }
    } else {
      // normal mode
      if (movePnt >= 0) {moveEngine();} // moveEngine is active
      if (playRun) {playEngine();}      // play engine is active
    }
  }
  

  //###############################################################################
  //
  //  10ms Move Servos if ValChg == true
  //
  //###############################################################################
  if ((micros() - moveMicros) >= moveInterval) {
    // normally runs every 10ms, but can be varied to suit speed of
    // robot movements
    moveMicros = micros();
        
    // if any servo value has changed we update all servos at this point
    if (ValChg) {
      // one or more servo values have changed, so update all of them
      if (!servoEn) {attachServos(0);}
      servoInst[0].writeMicroseconds(servoVal[0] + servoOff0);
      servoInst[1].writeMicroseconds(servoVal[1]);
      servoInst[2].writeMicroseconds(servo2Mod);
      servoInst[3].writeMicroseconds(servoVal[3]);

      // set the auto-power timeoutsince servos have been moved
      if (REC) {resetRECPause();} // timeout reset to 20 seconds
      ValChg = false; // clear this flag once we have updated servos
    }
    moveCalc = true; // set the flag to run the calculations in the next cycle
  }

  
  //###############################################################################
  //
  //  Read serial port
  //
  //###############################################################################
  // read keyboard input when not doing other things
  readSerial(); // empty Rx buffer
  if (keyVal != -1) {keyRec = 10;} // set a timer-out for adjustment speed

  
  //###############################################################################
  //
  //  10ms Tasks
  //
  //###############################################################################
  if (Task10ms > 0) {
    switch(Task10ms) {
      case 1: if (LEDshow) {pixels.show(); LEDshow = false;} break;
      case 2: readSW1(); break;  // read right button switch SW1
    } Task10ms--; // count down the sub-tasks from 3 - 1 as we loop
  } else if ((millis() - next10ms) >= 10) {
    // do these every 10ms
    next10ms = millis(); // set next time point
    Task10ms = 2;   // initiate 10ms task sequence
    readSW0();      // read left button switch SW0 as priority task

    // if moveEngine is moving then print servo values
    if ((movePnt >= 0) & (printTimeVals)) {printServoVals();}
    
    ON_Timer++; // increment ON timer
    if (keyRec > 0) {
      // keyboard released time-out so reset increment values
      keyRec--; if (keyRec == 0) {incS0 = 1; incS1 = 1; incS2 = 1;  incS3 = 1;}
    }
    if (WiiError == 0) {
      // Wii attached and responding so scan it
      wiiPhase--; 
      if (wiiPhase < 1) {
        //#####################################################################
        //
        // Wii Phase 0 - 20ms tasks
        //
        //#####################################################################
        // in wiiPhase 0 every 20ms. Here we decode the joystrick values
        wiiPhase = 2; // reset phase for alternate cycle, WiiPhase 1
        WiiUpdate(); // read Wii in this phase
        WiiLX = WiiLeftStickX();  // decode LX 0/32/63
        WiiLY = WiiLeftStickY();  // decode LY 0/32/63
        WiiRX = WiiRightStickX(); // decode RX 0/16/31
        WiiRY = WiiRightStickY(); // decode RY 0/16/31
        WiiJoy = ((WiiLX != 32) || (WiiLY != 32) || (WiiRX != 16) || (WiiRY != 16)); // true if any joystick is moved
      } else {
        //#####################################################################
        //
        // Wii Phase 1 - 20ms tasks
        //
        //#####################################################################
        // in wiiPhase 1 every 20ms or more often if WiiError > 0
        // here we respond to buttons and joystick demands
        if ((wiiPhase == 1) && !WiiBlock) {
          // ensures we are in phase 1 and that actions are not blocked
          // check buttons
          shift = 0; // clear the shift mask
          // check special functions and set bits in a shift mask
          if (WiiPressedRowBit(4,5)) {shift += B1000;} // LT - Set shift to reduce movement
          if (WiiPressedRowBit(5,7)) {shift += B0100;} // ZL - Set shift to further reduce movement
          if (WiiPressedRowBit(5,2)) {shift += B0010;} // ZR - Set shift to further reduce movement
          if (WiiPressedRowBit(4,1)) {shift += B0001;} // RT - Set shift to reduce movement

          //#####################################################################
          //
          // SELECT, HOME and START buttons
          //
          //#####################################################################
          if (!WiiPressedRowBit(5,2) && !WiiPressedRowBit(5,7)) {
            // check these if ZL and ZR are not pressed
            // if in RECORD mode, end points overwrite current values
              // START - goto Floor
              if (WiiPressedRowBit(4,2)) {moveGoFloor(); grabTgts();}
            else {button_BF = false;}
              // HOME - goto Home
              if (WiiPressedRowBit(4,3)) {moveGoHome(); grabTgts();}
            else {button_BH = false;}
              // SELECT - goto RESET/REST position
              if (WiiPressedRowBit(4,4)) {moveGoRESET(); grabTgts();}
            else {button_BS = false;}
          }

          // do the following if moveEngine is not active and not in RECORD mode
          if (movePnt < 0) {
            // check for major tasks
            if (!REC) {
              // only do these tasks in NORMAL mode
              // LT+SELECT so run moveEngine task
              if (WiiPressedRowBit(4,5) && WiiPressedRowBit(4,4)) {playXcuteDemo0();}
              else {button_SEL = false;}
              // LT+HOME so run moveEngine task
              if (WiiPressedRowBit(4,5) && WiiPressedRowBit(4,3)) {playXcuteDemo1();}
              else {button_HME = false;}
              // LT+SELECT so run moveEngine task
              if (WiiPressedRowBit(4,5) && WiiPressedRowBit(4,2)) {playXcuteDemo2();}
              else {button_SRT = false;}
            }
          }


          if (REC) {
            //#####################################################################
            //
            // RECORD mode - 20ms tasks
            //
            //#####################################################################
            // do button functions specific to record mode
            // digital joypad buttons:
              if (WiiPressedRowBit(4,6)) {playStop();} // BDD play reset position
            else {button_BDD = false;}
              if (WiiPressedRowBit(5,1)) {
                if (shift == 0) {playPrevious();}     // BDL play previous memory contents
                if (shift == B0001) {playSpeedDwn();} // BRT + BDL insert speed dec command
              }
            else {button_BDL = false; button_BDL_Cnt = 0;}
              if (WiiPressedRowBit(4,7)) {
                if (shift == 0) {playNext();}         // BDR play next memory contents
                if (shift == B0001) {playSpeedUp();}  // BRT + BDR insert speed inc command
              }
            else {button_BDR = false; button_BDR_Cnt = 0;}
              if (WiiPressedRowBit(5,0)) {playButton();} // BDU play memory contents
            else {button_BDU = false;
              if (button_BDU_Cnt > 0) {
//                Serial.println("BDU_Cnt=" + String(button_BDU_Cnt));
                button_BDU_Cnt = 0;
              }
            }
            
            // BA button pressed
            if (WiiPressedRowBit(5,4)) {
              if (shift == 0) {playAdd();}        // BA add new position to play memory
              if (shift == B1000) {playAppend();} // BLT + BA append position to play memory
              if (shift == B0100) {playDelete();} // BZL + BA delete current position in play memory
            } else {button_BA = false;}
            
            // BB button pressed
            if (WiiPressedRowBit(5,6)) {
                if (shift == 0) {playAddIncDelay();}      // BB so insert/increment a delay command
              else if (shift == B1000) {playDecDelay();}  // BB + BLT so decrement a delay command
            } else {button_BB = false; button_BB_Cnt = 0;}

            // BX button pressed
            if (WiiPressedRowBit(5,3)) {
              if (shift == 0) {playSnapshot();} // BX so record snapshot position
              if (shift == B1000) {playToSnapshot();} // BLT + BX so recall snapshot position
            } else {button_BX = false;}
            
            // BY button pressed
            if (WiiPressedRowBit(5,5)) {
              if (shift == 0) {playAddIncClap();}   // BY add/inc clap command to play memory
              if (shift == B1000) {playDecClap();}  // BY + BLT dec clap command to play memory
            } else {button_BY = false;}
              
            // LT & RT buttons pressed
            if (shift == B1001) {reportPlayVals();} // BLT + BRT report recorded values

            // ZL + START buttons pressed
            if ((shift == B0100) && WiiPressedRowBit(4,2)) {REC_toggle();} // BZL + SRT exit record mode

            // tidy up some button states
            if (!WiiPressedRowBit(4,2)) {button_SRT = false;}
            if (!WiiPressedRowBit(5,7)) {button_BZL = false;}

          } else {
            if (APP > 0) {
              //#####################################################################
              //
              // APP mode - connected to a PC
              //
              //#####################################################################
              // connected to Windows app so simply send Wii button state data over
              // the serial port
                if (WiiPressedRowBit(5,4)) {if (!button_BA) {PrintTx+= "B-A\n"; button_BA = true;}}
              else {if (button_BA) {PrintTx+= "N-A\n"; button_BA = false;}}
                if (WiiPressedRowBit(5,6)) {if (!button_BB) {PrintTx+= "B-B\n"; button_BB = true;}}
              else {if (button_BB) {PrintTx+= "N-B\n"; button_BB = false;}}
                if (WiiPressedRowBit(4,6)) {if (!button_BDD) {PrintTx+= "BDD\n"; button_BDD = true;}}
              else {if (button_BDD) {PrintTx+= "NDD\n"; button_BDD = false;}}
                if (WiiPressedRowBit(5,1)) {if (!button_BDL) {PrintTx+= "BDL\n"; button_BDL = true;}}
              else {if (button_BDL) {PrintTx+= "NDL\n"; button_BDL = false;}}
                if (WiiPressedRowBit(4,7)) {if (!button_BDR) {PrintTx+= "BDR\n"; button_BDR = true;}}
              else {if (button_BDR) {PrintTx+= "NDR\n"; button_BDR = false;}}
                if (WiiPressedRowBit(5,0)) {if (!button_BDU) {PrintTx+= "BDU\n"; button_BDU = true;}}
              else {if (button_BDU) {PrintTx+= "NDU\n"; button_BDU = false;}}
                if (WiiPressedRowBit(4,5)) {if (!button_BLT) {PrintTx+= "BLT\n"; button_BLT = true;}}
              else {if (button_BLT) {PrintTx+= "NLT\n"; button_BLT = false;}}
                if (WiiPressedRowBit(4,1)) {if (!button_BRT) {PrintTx+= "BRT\n"; button_BRT = true;}}
              else {if (button_BRT) {PrintTx+= "NRT\n"; button_BRT = false;}}
                if (WiiPressedRowBit(5,3)) {if (!button_BX) {PrintTx+= "B-X\n"; button_BX = true;}}
              else {if (button_BX) {PrintTx+= "N-X\n"; button_BX = false;}}
                if (WiiPressedRowBit(5,5)) {if (!button_BY) {PrintTx+= "B-Y\n"; button_BY = true;}}
              else {if (button_BY) {PrintTx+= "N-Y\n"; button_BY = false;}}
                if (WiiPressedRowBit(5,7)) {if (!button_BZL) {PrintTx+= "BZL\n"; button_BZL = true;}}
              else {if (button_BZL) {PrintTx+= "NZL\n"; button_BZL = false;}}
                if (WiiPressedRowBit(5,2)) {if (!button_BZR) {PrintTx+= "BZR\n"; button_BZR = true;}}
              else {if (button_BZR) {PrintTx+= "NZR\n"; button_BZR = false;}}
            } else {
              //#####################################################################
              //
              // NORMAL mode - freestyle
              //
              //#####################################################################
              // do button functions specific to normal mode
              if (WiiPressedRowBit(4,6)) {playXcuteBDD();} // BDD play a digital pad routine
              else {button_BDD = false;}
              if (WiiPressedRowBit(5,1)) {playXcuteBDL();} // BDL play a digital pad routine
              else {button_BDL = false;}
              if (WiiPressedRowBit(4,7)) {playXcuteBDR();} // BDR play a digital pad routine
              else {button_BDR = false;}
              if (WiiPressedRowBit(5,0)) {playXcuteBDU();} // BDU play a digital pad routine
              else {button_BDU = false;}
              if (WiiPressedRowBit(5,7) && WiiPressedRowBit(4,2)) {REC_toggle();} // BZL + SRT begin record mode
              if (WiiPressedRowBit(5,6)) {printValues();} // print out servo values
              else {button_BB = false;}
              if (WiiPressedRowBit(5,5)) {TogPrintMode();} // BY - toggle print mode
              else {button_BY = false; YbutCnt = 0;}

              // tidy up some button states
              if (!WiiPressedRowBit(4,2)) {button_SRT = false;}
              if (!WiiPressedRowBit(4,3)) {button_HME = false;}
              if (!WiiPressedRowBit(4,4)) {button_SEL = false;}
              if (!WiiPressedRowBit(5,7)) {button_BZL = false;}
            }
          }

          
          //#####################################################################
          //
          // Check Joystick demands
          //
          //#####################################################################
          // assumes left Joystick has centre at 32, deadband +/1 1 bit
          // assumes right Joystick has centre at 16, deadband +/1 1 bit
          rate = 0;
          if ((shift & B1001) > 0) {rate = 1;}  // 1/2 speed
          if ((shift & B0110) > 0) {rate = 2;}  // 1/4 speed
          if (WiiLX > 33) {JoyJawClose((WiiLX-32)>>rate);} 
          else if (WiiLX < 31) {JoyJawOpen((32-WiiLX)>>rate);} 
          if (WiiLY > 33) {JoyArmFwd((WiiLY-32)>>rate);} 
          else if (WiiLY < 31) {JoyArmBck((32-WiiLY)>>rate);} 
          if (WiiRX > 17) {JoyTurnRight((WiiRX-16)>>rate);} 
          else if (WiiRX < 15) {JoyTurnLeft((16-WiiRX)>>rate);} 
          if (WiiRY > 17) {JoyVertDwn((WiiRY-16)>>rate);} 
          else if (WiiRY < 15) {JoyVertUp((16-WiiRY)>>rate);}
        }
      }
    } else {
      // Wii not attached so keep polling the address every second
      wiiPhase--; if (wiiPhase < 1) {
        wiiInitialise();
        if (WiiError != 0) {wiiPhase = 100;}  // reset timer for 'try again'
      }
    }

    //#####################################################################
    //
    // Power time-out
    //
    //#####################################################################
    // test for PWR timeout every 10ms cycle
    if (PWR_timeout > 0) {
      // previously servo code received so detach servos after a timeout delay
      // disable this auto power-down by setting PWR_timeout = 0
      PWR_timeout--;
      if (PWR_timeout < 1) {
        if ((servoVal[0] != Reset0) || (servoVal[1] != Reset1) || (servoVal[2] != Reset2)) {
          // one or more servos not at reset position so move there first
          button_BS = false; moveGoRESET();
        } else {
          detachServos();
          PrintTx+= "Auto-Power OFF event\n";
        }
      }
    }
    // PrintTx+= "PWR=" + String(PWR_timeout) + "\n";
    
    if (incOffset > 0) {incMoveOffset();}
    else {decMoveOffset();}
  }

  
  //###############################################################################
  //
  //  20ms Tasks
  //
  //###############################################################################
  if (Task20ms > 0) {
    switch(Task20ms) {
      case 1: LED_Tasks(); break;     // various LED tasks
    } Task20ms--; // count down the sub-tasks from 3 - 1 as we loop
  } else if ((millis() - next20ms) >= 20) {
    // do these every 20ms
    next20ms = millis(); // set next time point
    Task20ms = 1;
  }

  
  //###############################################################################
  //
  //  40ms Tasks
  //
  //###############################################################################
  if (Task40ms > 0) {
    switch(Task40ms) {
      case 1: TestForApp(); break;
      case 2: Display_40ms(); break;
      case 3: readBattery(0); break;
      case 4: if (Ping > 0) {Ping--;} break;
      case 5: // if not connected to WiFi run then try to connect every second
        if (!WiFiConnected) {
          // only try to connect WiFi if no 'Pings' are being received
          if (Ping == 0) {
            WiFiConCnt--; if (WiFiConCnt < 1) {
              WiFiConCnt = 10; WiFiTryToConnect();
              if (WiFiPrt) {Serial.println("Trying to connect: " + getBAhex());}
            }
          }
        } break;
      case 6:
        if (WiFiPing > 0) {
          WiFiPing--;
          if (WiFiPing < 1) {
             // we have stopped receiving so disconnect WiFi
            WiFiDisconnect();
            ESP_NOW_BA = -1;
            WiFiTryOnce = true;
            if (WiFiPrt) {Serial.println("WiFiPing == 0");}
          }
        } break;
    } Task40ms--; // count down the sub-tasks from 5 - 1 as we loop
  } else if ((millis() - next40ms) >= 40) {
    // do these every 40ms
    next40ms = millis(); Task40ms = 4; if (WiFiEn) {Task40ms = 6;}
    // check display mirror auto time-out
    // when count reaches zero DispMon is set to false
    if (DispTx > 0) {DispTx--; if (DispTx == 0) {DispMon = false;}}
  }

  //###############################################################################
  //
  //  40 ms Print Task - Variable
  //
  //###############################################################################
  // this code uses either the serial port or the WiFi link to send messages
  if ((millis() - print40ms) >= 40) {
    print40ms = millis();
    if (!Echo) {PrintTx = "";}  // Echo is false so empty print buffer
    if (PrintTx.length() > 0) {
      // characters in string buffer so send some/all of them
      print40ms = millis() - 34;  // shorten the delay for more data to send
      Tx_PingCnt = 0;             // reset the WiFi ping counter
      if (PrintTgt == 0) {
        // default uses the serial port
        if (Serial.availableForWrite() >= 64) {
          // there is sufficient space in the print buffer, so print
          if (PrintTx.length() <= 64) {
            Serial.print(PrintTx);
            PrintTx = "";   // empty the buffer
          } else {
            Serial.print(PrintTx.substring(0,64));
            PrintTx = PrintTx.substring(64);
          }
        }
      } else {
        // text data has been received from WiFi so respond using ESP_NOW
        // ensure that we have had a call back from the previous frame
        if (WiFiConnected) {
          // only trannsmit PrintTx data if connected
          if (WiFiTx_CB == 1) {
            WiFiTx_CB = 0;  // clear the call back flag when sending
            if (PrintTx.length() <= 32) {
              WiFiTx_len = PrintTx.length();
              PrintTx.toCharArray(Tx_Buff.ESPdata,WiFiTx_len + 1);
              WiFiTxBytes += WiFiTx_len;  // track number of bytes sent
              WiFiTxGetChecksum(WiFiTx_len); Tx_Buff.ESPdata[WiFiTx_len] = WiFiTx_Chk; WiFiTx_len++;
              // t0 = micros();
              esp_now_send(broadcastAddress, (uint8_t *) &Tx_Buff, WiFiTx_len);
              PrintTx = "";   // empty the buffer
            } else {
              WiFiTx_len = 32;
              Any$ = PrintTx.substring(0,WiFiTx_len);
              Any$.toCharArray(Tx_Buff.ESPdata,WiFiTx_len + 1);
              WiFiTxBytes += WiFiTx_len;  // track number of bytes sent
              WiFiTxGetChecksum(WiFiTx_len); Tx_Buff.ESPdata[WiFiTx_len] = WiFiTx_Chk; WiFiTx_len++;
              // t0 = micros();
              esp_now_send(broadcastAddress, (uint8_t *) &Tx_Buff, WiFiTx_len);
              PrintTx = PrintTx.substring(32);
            }
          } else if (WiFiTx_CB == -1) {
            // an error has occured so resend the data
            if (WiFiTryCnt < 5) {
              WiFiTryCnt++;   // track the number of retries
              WiFiTx_CB = 0;  // clear the call back flag when sending
              // t0 = micros();
              esp_now_send(broadcastAddress, (uint8_t *) &Tx_Buff, WiFiTx_len);
            } else {
              // too many resends so disconnect WiFi link
              WiFiDisconnect();
              PrintTx += "Failed to Tx!\n";
            }
          }
        } else {
          // not connected so clear PrintTxdata
          PrintTx = "";
        }
      }
    }
  }


  //###############################################################################
  //
  //  200 ms Tasks (5Hz)
  //
  //###############################################################################
  if ((millis() - next200ms) >= 200) {
    next200ms = millis();
    // test for RESET condition
    if (sw1State == LOW) {
      ResetCnt++; if (ResetCnt >= 15) {
        // button held down for >= 3 seconds so force a soft reset
        // Serial.println("Reset");
        Display_Reboot(); delay(1000);
        resetFunc();    //call reset
      }
      // Serial.println("ResetCnt " + String(ResetCnt));
    } else {ResetCnt = 0;}  // clear the timer
  }
}

// ---------------------------------------------------------------------------------

void loopWiiWait() {
  // blocks Wii actions whilst Wii button demands exist, then returns to calling
  // function, other main loop tasks and timers are unaffected
  WiiBlock = true;
  while (WiiBlock) {
    // infinate loop until conditions are met
    loop(); yield();  // maintain main loop functions
    if (WiiError != 0) {WiiBlock = false;}
    if ((WiiData[4] == 255) && (WiiData[5] == 255)) {WiiBlock = false;}
  }
}

// --------------------------------------------------------------------------------

void OnDataRecv(const esp_now_recv_info_t *info, const uint8_t *incomingData, int len) {
  // Call-back function oocurs when data has been received
  // This needs to be treated like an interrupt, so speed is the essence here
  // memcpy moves data in a block, instead of individual bytes if using for() loop
  memcpy(WiFiRxMacAddr, info->src_addr, 6);   // get the senders MAC/broadcast address
  memcpy(WiFiTxMacAddr, info->des_addr, 6);   // get destination MAC address, which should be assigned peer
  memcpy(&Rx_Buff, incomingData, len);        // copy the received data into Rx_Buff array
  Rx_len = len - 1; Rx_Pnt = 0;               // len contain the length of data
  RxRecvEvent = true;                         // set the flag to say data received
}

// --------------------------------------------------------------------------------

void OnDataRecvHandler() {
  // Called from the main loop() when has been set, to handle the received data
  RxRecvEvent = false;
  if (WiFiPrt) {Serial.println("OnDataRecvHandler()");}
  // incoming data will always be two or more characters, ie. text == $+++
  if (Rx_len < 2) {if (WiFiPrt) {Serial.println("Rx Error - no Rx data");} return;}

  RxGetChecksum(Rx_len);  // generate a checksum from Rx data
  if (Rx_Chk != Rx_Buff.ESPdata[Rx_len]) {
    if (WiFiPrt) {Serial.println("Rx Error - checksum");}
  }
  // set flags before exiting
  WiFiRxBytes += Rx_len; Rx_Task = 0; WiFiRx = true;
  WiFiPing = 50;    // set link time-out to 2 seconds, Wii Transceiver will keep setting this with Wii data
  WiFiRxRec = true; // set flag indicating data has been recieved from Wii Transceiver
  if (WiFiPrt) {Serial.print("Bytes received: "); Serial.println(Rx_len);}
}

// --------------------------------------------------------------------------------

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  // Callback when data is sent over Wi-Fi, determines success or failure
  // t1 = micros();
  if (WiFiPrt) {Serial.println("OnDataSent()");}
  if (WiFiConnected) {
    // the link is already active so handle normal success/failure scenarios
    if (status == 0){
      if (WiFiPrt) {Serial.println("    Success " + String(t1-t0));}
      WiFiTx = true;    // flag WiFi Tx success
      WiFiTx_CB = 1;    // set flag to indicate that a call back state has occured
      WiFiTxErrCnt = 0; // clear the error flag in case of a glitch
      WiFiPing = 50;    // set link time-out to 2 seconds
    } else {
      if (WiFiPrt) {Serial.println("    Fail " + String(t1 - t0));}
      WiFiTx = false;   // flag WiFi Tx failure
      WiFiTx_CB = -1;   // set flag to indicate that a resend is required
      WiFiTxErrCnt++;
    }
  } else {
    // not connected so we must have tried to connect and may get an error
    if (status == 0){
      // successful so enter the connected state
      if (WiFiPrt) {Serial.println("    Connected!");}
      WiFiConnected = true;
      WiFiPing = 50;    // set link time-out to 2 seconds, Wii Transceiver will keep setting this with Wii data
      WiFiTxErrCnt = 0; // reset error count for this connection
    } else {
      if (WiFiPrt) {Serial.println("    Status = " + String(status));}
      WiFiTxErrCnt++;
    }
    WiFiTx_CB = 1;
  }
}

// ---------------------------------------------------------------------------------

void runPOST() {
  // called during start-up
  // note this version does not use the EEPROM
  emptySerial();
  Serial.println("\n\n" + Release$);
  Serial.println("Released: " + Date$);
  Serial.println(F("Starting POST..."));

  // Are we running WiFi?
  if (WiFiEn) {Serial.println("WiFi is enabled.");}
  else {Serial.println("WiFi is disabled.");}

  // Initialise display
  Display_Init();
  Display_Intro();
  Serial.println("Display initialised");

  // initilaise RGB LED array
  pixels.begin();
  pixels.fill(0,0,5);  // strip.fill(color, first, count)
  pixels.show(); // This sends the updated pixel color to the hardware.
  NeoPixel_Clear_Array(); NeoPixel_ShowArray(true);
  
  Serial.println("Centring Servos...");
  attachServos(100); delay(200); detachServos();
  
  Serial.println("Listening for Windows app...");
  testCtrlApp(); // see if connected to Windows app?
  if (cmdRec < 1) {
    // no serial pings or data
    Serial.println("No Windows app detected");
    Serial.println("Wii Classic mode assumed");
    servoMain.detach(); detachServos();
  } else {
    // serial link so send initialisation data
    reportLimits();
    reportHomeValues();
    reportRestValue();
  }

  // set up battery quick averaging
  readBattery(0); BatAvg = BatVol; BatSum = BatVol * 20;
  AnyLong = (BatAvg - BatCritical)*100/(BatMax - BatCritical);
  if (AnyLong > 100) {AnyLong = 100;} // limit max to 100%
  if (AnyLong < 0) {AnyLong = 0;}     // limit min to 0%
  Serial.println("Battery " + String(float(BatAvg)/BatCal) + "v\t" + String(AnyLong) + "%");
  randomSeed(BatVol); // use the battery voltage as the random seed factor

  // all done
  Serial.println("POST complete!");
  Once = 0;
}

// ---------------------------------------------------------------------------------

void setDefaults() {
  // load default values, at start and after a RESET
  Angle = 90;             // start with servo in the centre pos
  APP = 0;                // >0 when connected to Windows App
  APP_Disp = 0;           // APP display mode
  APPs = 0;               // previous APP value
  BatDP = 0;              // battery data pointer, 0 - 59
  BatTime$ = "--:--";     // time left displayed
  BlinkCnt = 0;           // time delay used in micro LED blink function
  Brightness = 255;       // Display brightness
  button_BA = false;      // A button state tracker
  button_BA_Cnt = 0;      // button held down counter
  button_BB = false;      // B button state tracker
  button_BB_Cnt = 0;      // button held down counter
  button_BDD = false;     // BDD button state tracker
  button_BDL = false;     // BDL button state tracker
  button_BDL_Cnt = 0;     // button held down counter
  button_BDR = false;     // BDR button state tracker
  button_BDR_Cnt = 0;     // button held down counter
  button_BDU = false;     // BDU button state tracker
  button_BDU_Cnt = 0;     // button held down counter
  button_BF = false;      // B+ START button state tracker
  button_BH = false;      // BH button state tracker
  button_BLT = false;     // LT button state tracker
  button_BRT = false;     // RT button state tracker
  button_BS = false;      // B- SELECT button state tracker
  button_BX = false;      // X button state tracker
  button_BY = false;      // Y button state tracker
  button_BZL = false;     // ZL button state tracker
  button_BZR = false;     // ZR button state tracker
  button_BY_Cnt = 0;      // button held down counter
  button_HME = false;     // HOME button state tracker
  button_SEL = false;     // SELECT button state tracker
  button_SRT = false;     // START button state tracker
  Calibrated = true;      // limits functionality, set to true once calibrated
  clapCnt = 0;            // number of claps
  CmdFlag = 0;            // global flag used when monitoring Walk actions
  CmdForNum = 0;          // number of times a For... Next loop will run
  CmdForRow = 0;          // points to 1st row in a For...Next loop
  cmdMode = ' ';          // command mode
  cmdRec = 0;             // > 0 if a '~' or any data has been received
  cmdSgn = 1;             // +/- 1 to handle negative numbers, default = 1
  cmdType = ' ';          // command type
  cmdVal = 0;             // value associated with a cmdType
  DispBack = false;       // flag used to draw a back arrow on the display
  DispChng = false;       // flag invokes initialisation code
  DispClr = false;        // set == true to clear the memory contents
  DispCnt = 50;           // counter used in display updating, 2 sec delay initially
  DispDel = 0;            // >0 display delay counter, stops updates over I2C
  DispDisp = false;       // set = true to immediately display memory contents
  DispFlash  = 0;         // flashing display counter
  DispInv = false;        // display inverted flag
  DispMenu = 0;           // group type, 0 = sensors, 1 = tasks, etc
  DispMode = 0;           // the type of display for a given mode
  DispMon = false;        // == true if connected to OLED Mirror app
  DispNOP = false;        // set == true to stop display I2C updates
  DispNow = 0;            // if > 0 display special modes
  DispPnt = 0;            // points at the current display mode, default = 0 batteries
  DispTx = 0;             // counter used with display mirror app
  DispTx1$ = "";          // display text line 1
  DispTx2$ = "";          // display text line 2
  Echo = true;            // if true send print messages
  ESP_NOW_BA = -1;        // broadcast address pointer, -1 to start with
  ESP_NOW_Init = false;   // == true once ESP-NOW has been initiaiised
  flashCnt = 0;           // timer used to flash Record mode LED
  forPnt = -1;            // forStack[] pointer, stack is empty
  gosubPnt = -1;          // gosubStack[] pointer, stack is empty
  incOffset = 0;          // > 0 flag which controls thermal offset increments
  interval = 10000;       // main loop interval in microseconds
  LED_Del = 0;            // LED Task delay, adds pauses into the processes
  LED_Nop = false;        // == true to block eye LED tasks, default = false
  LEDOP = false;          // state of LED pin HIGH/LOW
  LEDshow = false;        // LED show flag for mouth LEDs
  LED_SubTask = 0;        // LED sub-task pointer
  LED_SwDwn = false;      // == true if a button switch has been pressed
  LED_Task = -1;          // LED task pointer, -ve values reset variables
  LEDVal = 0;             // values used in LED tasks
  keyPnt = -1;            // pointer to last servo affected by keyboard demand
  keyVal = -1;            // any keyboard value
  Mem1st = 0;             // pointer to 1st memory row to be played by Walk engine, default = 0
  MemMax = MemRows;       // number of rows loaded into memory
  MemRow = -1;            // Mem[] row pointer used in loading the array
  moveCalc = false;       // set to true after servo values have been updated
  MOVING = false;         // == true when servoces are being adjusted
  moveInterval = moveDefInterval; // move engine interval nominally 10ms
  moveIntervalAdj = false; // true if acceleration and braking are to be used
  moveIntervalInc = 2700; // main loop interval increment in microseconds
  moveLast = -1;          // previous movePnt value
  moveMicros = micros() + moveInterval; // move engine interval timer in microseconds
  movePnt = -1;           // movement array pointer, default is -1
  for (int zP; zP < posMax; zP++) {
    moveLoadPosRFV(zP, Home0, Home1, Home2);
  }
  // calculate thermal offset counter values
  servoOffDecT = 0;
  if (servoOffMax != 0) {
    // a thermal offset has been defined so calculate timer values
    servoOffIncT = servoOffRmpUp / (10 * abs(servoOffMax));
    servoOffDecT = servoOffRmpDwn / (10 * abs(servoOffMax));
  }
  
  moveTask = 0;           // main task pointer in movement engineges
  Mult = 100;             // speed multiplying factor
  Once = 1;               // only = 1 from full reset
  Ping = 0;               // 'ping' counter
  playCnt = 4;            // total number of elements stored in play memory
  playPause = 0;          // inter-move delay in 10ms loops; default = 0ms
  playPauseDef = 0;       // default inter-move delay in ms, = 0
  playPnt = 0;            // pointer using in playEngine, default = 0
  playRepeat = false;     // == true if continuous repeat play is wanted
  playRun = false;        // true when playEngine is active, otherwise false
  playSpeed = 100;        // % of default speed, normally = 100
  playTask = 0;           // task pointer used by the playEngine
  PrintTgt = 0;           // 0 == serial port, 1 == WiFi ESP-NOW
  printTimeVals = false;  // a flag that determines when servo values are printed
  PrintTx = "";           // clear printed strings
  PWM_Freq = 100;         // servo PWM frequency, default = 100;
  PWR_timeout = 0;        // disables servos after a time-out, when no code received
  rate = 0;               // joystick demand flag, dependent on shift buttons
  REC = false;            // record mode flag, true if in record mode, false otherwise
  REC_Disp = 0;           // RECORD display mode
  ResetCnt = 0;           // timer used to invoke soft RESET
  RndLast = -1;           // previous random number
  Rx_Chk = 0;             // Rx checksum
  Rx_len = 0;             // length of WiFi received data block
  RxRec = false ;         // set true when a valid frame of data is received
  RxRecvEvent = false;    // set == true when a OnDataRecv() call back event has occured
  RxState = 0;            // receiver state machine state
  Rx_Task = 0;            // pointer user by Rx state machine
  SerialRx = false;       // == true if any character has been received
  ServoAttOnce = false;   // == true once servos have been attached once
  servoNum = 0;           // target servo number 0 - 3 for setting LL/UL limits, default = 0
  servoOff0 = 0;          // offset for servo 0
  servoOffDec = 10000;    // offset decrement counter
  servoOffInc = 0;        // offset increment counter
  servoPin = -1;          // servo output pin is undefined
  servoCtr[0] = Reset0;
  servoCtr[1] = Reset1;
  servoCtr[2] = Reset2;
  servoCtr[3] = Reset3;

  setLimits();
  
  servoVal[0] = Reset0;
  servoVal[1] = Reset1;
  servoVal[2] = Reset2;
  servoVal[3] = Reset3;
  snapshot[0] = 5000;     // reset temporary values for a position to be remembered
  sw0Cnt = 0;             // button switch counter
  sw0DwnTime = 0;         // button pressed down time
  sw0LastState = HIGH;    // previous state of button switch, HIGH/LOW
  SW0_Nop = false;        // == true to block SW0 read tasks, default = false
  sw0State = HIGH;        // state of read button switch pin
  sw0Timer = 0;           // timer used to detemine button sequences
  sw1Cnt = 0;             // button switch counter
  sw1LastState = HIGH;    // previous state of button switch, HIGH/LOW
  SW1_Nop = false;        // == true to block SW1 read tasks, default = false
  sw1State = HIGH;        // state of read button switch pin
  sw1Timer = 0;           // timer used to detemine button sequences
  Task10ms = 0;           // 10ms subtask timer
  Task20ms = 0;           // 20ms subtask timer
  Tx_PingCnt = 0;         // 1 second Tx ping timer
  USBPwr = false;         // true if POST detects USB power voltage
  ValChg = false;         // flag set when a servo value is changed
  WiFiConnected = false;  // == true if a WiFi connection has been established
  WiFiEn = false;         // true when WiFi is enabled with prolonged 'Z' button activity
  WiFiPing = 0;           // Rx counter used to maintain connected state
  WiFiPrt = false;        // if == true then print WiFi associated messages
  WiFiRx = false;         // true when a block of data has been received
  WiFiRxBytes = 0;        // number of bytes receiv ed over the WiFi link
  WiFiRxErr;              // number of receive errors in the WiFi link
  WiFiRxRec = false;      // true when a block of data has been successfully received
  WiFiTryCnt = 0;         // WiFi try to connect count
  WiFiTryNum = 0;         // WiFi try to connect total count
  WiFiTryOnce = true;     // == true if first pass of trying to connect
  WiFiTx = false;         // true when a block of data has been sent successfully
  WiFiTxBytes = 0;        // number of bytes receiv ed over the WiFi link
  WiFiTx_CB = 1;          // == 1 call back is clear to send, == -1 resend
  WiFiTx_Chk = 0;         // Tx checksum
  WiFiTx_len = 0;         // >0 when WiFi Tx buffer contains data to be sent
  WiFiTxErr = 0;          // WiFi data error
  WiFiTxErrCnt = 0;       // WiFi Tx error count, leads to disconnection
  WiiType = 0;            // device type, 0 == Nunchuk, 1 == Classic
  WiiBlock = false;       // = true blocks Wii controller demand actions
  WiiError = 1;           // used to detect transmission errors
  WiiJoy = false;         // = true when there is a joystick demand
  WiiLX = 32;             // left joystick values
  WiiLY = 32;             // left joystick values
  wiiPhase = 100;         // used to perform wii functions at 50 Hz
  WiiRX = 16;             // right joystick values
  WiiRY = 16;             // right joystick values
  XbutCnt = 0;            // X button held down counter

  wiiClearData();         // set Wii Rx data to defaults for Wii Classic
  setLimits();            // set limits based on calibrated state
  clearMem(1);            // clear the contents of the moves[] memory
}

// ----------------------------------------------------------------------

void SyncTimers() {
  // synchronises timers when delay() functions have been used
  while (millis() < 50) {yield();}  // ensure there is some time on the timers

  moveMicros = micros() - moveInterval;
  next10ms = millis() -  9;
  next20ms = millis() - 21;
  next40ms = millis() - 43;
  next200ms = millis() - 45; // reset 200ms loop timer with 5ms phase to 10ms timer
  print40ms = millis() - 8;   // reset 40ms printer loop timer with 6ms phase to 10ms timer
}

// ---------------------------------------------------------------------------------
