// ################################################################################
//
//  Balance Bot Mk1 Release vR1.0
//
//  Released:  20/07/2024
//
//  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.

    micro: ESP32

    This code controls a self-balancing robot using a PID controller that responds to
    angular errors, based on a vertical setpoint. This robot has two wheels driven by
    DC motors, a 320x240 TFT display, a laser range finder and four RGB LEDs.
    
    This self-balancing version includes:
    - multi-tasking code, including 2nd core useage
    - PID controller for self-balancing
    - Face engine for robot expressions
    - display of data like angles and slots counts, etc
    - RGB LED patterns to match robots movement and modes
    - servo driven panoramic range finder using VL53L1X sensor
    - panoramic head movement
    - modes set by push button switches
    - wheel slot counters detect speed and distance
    
    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 libraries
#include <Arduino.h>          // needed when using an ESP32 micro
#include <HardwareSerial.h>   // serial library
#include <Wire.h>             //Include the Wire.h library so we can communicate with the gyro
#include "SparkFun_VL53L1X.h" // VL53L1X laser ranger library
#include <Adafruit_GFX.h>     // include Adafruit graphics library
#include <Adafruit_ILI9341.h> // include Adafruit ILI9341 TFT library
#include <FastLED.h>          // Neopixel library
#include <ESP32Servo.h>       // servo library

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

// Configuration
String Release = "BalanceBot MK1 R1)";
String Date = "20/07/2024";

// Define a task for core 0
TaskHandle_t Core0;
 
#define TFT_CS    17     // TFT CS  pin is connected to ESP32 GPIO17
#define TFT_RST   16     // TFT RST pin is connected to ESP32 GPIO16
#define TFT_DC     4     // TFT DC  pin is connected to ESP32 GPIO4
// initialize ILI9341 TFT library with hardware SPI module
// SCK (CLK) ---> GPIO18
// MOSI(DIN) ---> GPIO23
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// Define constants
#define AccOffX -116                // calibration offset applied to X-axis accelerometer (-116)
#define AccOffZ  586                // calibration offset applied to Z-axis accelerometer ( 586)
#define BatCal 383.9                // calibrated voltage multiplying factor
#define BatCritical 2534            // critical battery threshold @6.6v, default = 2672, set == 0 to ignore
#define BatMax 3148                 // A0 for fully charged battery voltage, == 8.2v
#define BatPin 36                   // analog input pin ADC15 on GPIO12
#define BatWarn 2687                // battery threshold of 7.0v gives low warning
#define BLACK 0                     // display monitor app pen colour
#define BLUE 5                      // display monitor app pen colour
#define ctr_X 120                   // face centre X position
#define ctr_Y 160                   // face centre Y position
#define scr_h 320                   // screen height
#define scr_w 240                   // screen width
#define DBLim 5.0                   // deadband limit (5.0)
#define DbLLX 108                   // BalanceBot joystick deadband lower limit
#define DbULX 148                   // BalanceBot joystick deadband upper limit
#define DbLLY 108                   // BalanceBot joystick deadband lower limit
#define DbULY 148                   // BalanceBot joystick deadband upper limit
#define ExpActive 7                 // Active balancing expression state
#define ExpCal 1                    // calibrating expression state
#define ExpHappy 2                  // Happy expression state
#define ExpHapAng 3                 // Happy angled expression state
#define ExpLying 4                  // Lying down expression state
#define ExpReady 5                  // Ready expression state
#define ExpReadyAnx 6               // Ready anxious tilting expression state
#define ExpSafe 8                   // Safe expression state
#define ExpStare 10                 // Stare straight ahead
#define ExpStart 0                  // Start expression state
#define ExpSleep 9                  // Sleeping expression state
#define GREEN 4                     // display monitor app pen colour
#define Head_30 885                 // fully left head angle, looking at left body corner (858)
#define Head_90 1510                // mid point head angle, looking directly forward (1430)
#define Head_150 2121               // fully right head angle, looking at right body corner (2060)
#define Head_Freq 100               // head servo PWM frequency
#define HeadPin 5                   // head servo is connected to GPIO5
#define I2Cdel 10                   // I2C bus timing delay for MPU6050
#define INT_P34 34                  // speed sensor left
#define INT_P35 35                  // speed sensor right
#define INT_PIN 0                   // VL53L1X GPIO interrupt to ADC input, needed but not actually used
#define INVERSE 2                   // display monitor app pen colour
#define LED_Pin 15                  // GPIO for RGB LEDs
#define LtofDet 350                 // max range for target detection in head scanning
#define LtofGnd 350                 // caliubration range for ground reflection
#define LtofLimit 510               // Range max value is always limited to this figure
#define LtofMax 400                 // out of range value for head scanning
#define LtofMin 10                  // minimum range value
#define LtofStop 140                // range value at which autonomous movement stops
#define NumLEDs 4                   // number of neopixel LEDs
//#define mainLoopTime 4000           // main loop timer in microseconds
#define MAGENTA 8                   // display monitor app pen colour
#define MPU_address 0x68            // MPU-6050 I2C address (0x68 or 0x69)
#define PinLftA 33                  // left driver A pin
#define PinLftB 25                  // left driver B pin
#define PinRhtA 26                  // right driver A pin
#define PinRhtB 27                  // right driver B pin
#define PwmBits 8                   // PWM counter width 8 bits, 0 - 255
// we assign high channel numbers so as not to interfer with servo allocation
#define PwmChLftA 12                // PWM channel assigned to left motor
#define PwmChLftB 13                // PWM channel assigned to left motor
#define PwmChRhtA 14                // PWM channel assigned to right motor
#define PwmChRhtB 15                // PWM channel assigned to right motor
#define PwmFreq 30000               // PWM frequency is 30 kHz
#define RED 3                       // display monitor app pen colour
#define RngDpth 41                  // size of RangeData[] array
#define sw0Pin 13                   // left switch SW0 assigned to GPIO13
#define sw1Pin 14                   // right switch SW1 assigned to GPIO14
#define TEXT_ALIGN_LEFT 0           // display monitor app text alignment
#define TEXT_ALIGN_RIGHT 1          // display monitor app text alignment
#define TEXT_ALIGN_CENTER 2         // display monitor app text alignment
#define TEXT_ALIGN_CENTER_BOTH 3    // display monitor app text alignment
#define TURQUOISE 7                 // display monitor app pen colour
#define WHITE 1                     // display monitor app pen colour
#define XSHUT_PIN 19                // GPIO19 controls VL53L1X XSHUT HIGH enable pin
#define YELLOW 6                    // display monitor app pen colour

// Declare PID controller coeficients
// As wheels are bigger diameter 69/51 values are reduced proportionately by 0.74
float max_target_speed = 20.0;      // Max target speed (20 - 160)
float pid_p_gain = 13.5;            // Gain setting for the P-controller (13.5)
float pid_i_gain = 0.4;             // Gain setting for the I-controller (0.4)
float pid_d_gain = 42.0;            // Gain setting for the D-controller (42.0)
float pid_out_max = 255.0;          // limit PID output to limit to PWM max
float PWM_StartMax = 18.0;          // PWM needed to overcome motor start stiction
float pid_sb_db = 0.0;              // Self-balancing dead band limit
float pid_s_gain = 0.0;             // Gain setting for the S-controller (0.0)
float turning_speed = 16.0;         // max turning speed (24 - 40)
float turn_speed = 0.0;             // incremental turning speed (0 - 40)

// create servo instances
Servo servoHead;          // define BalacnceBot MK1 head servo instance

// create a FastLED instance and define the total number of LEDs on the robot
CRGB LED[NumLEDs];        // LEDs connected to micro for o/p
CRGB LEDX[1];             // temp LED value

// create VL53L1X range sensor instance, and define its pins
// Note that INT_PIN is declared for the library, but not used in the code
SFEVL53L1X VL53L1X(Wire, XSHUT_PIN, INT_PIN);

// my Wii Transciever unique MAC: 50:02:91:68:F7:3F
// you need to build a Wii Transceiver and get its MAC address
uint8_t broadcastAddress[] = {0x50, 0x02, 0x91, 0x68, 0xF7, 0x3F};

// 250 byte max for Tx/Rx data
// It must match the receivers data structure
typedef struct ESPNOW_Buff250 {
    char ESPdata[250];
} ESPNOW_Buff250;

// Create an 8 byte message called NunchukDataTx, to hold I2C data messages
ESPNOW_Buff250 Tx_Buff;

// Create an 8 byte message to hold incoming sensor readings
ESPNOW_Buff250 Rx_Buff;

// Declare and initialise global variables
int16_t acc_cal_value = AccOffZ;    // Enter the accelerometer calibration value, default 1000
int16_t accel_delta;                // raw accelerometer instantanious increase
int16_t accel_ramp_rate;            // raw accelerometer ramp rate, used to limit impulse noise at small angles
int16_t accelerometer_data_raw;     // raw accelerometer data for the vertical axis
int16_t accelerometer_data_raw_i;   // instantaneous record for averaging
long acc_data_raw_av;               // cumulative sum for averaging
long acc_data_raw_Cnt;              // counter for averaging
int16_t AccRawX;                    // 2-byte value read from the MPU
int16_t AccRawY;                    // 2-byte value read from the MPU
int16_t AccRawZ;                    // 2-byte value read from the MPU
int AccSumX;                        // accumulator used in accel offset calibration
int AccSumY;                        // accumulator used in accel offset calibration
int AccSumZ;                        // accumulator used in accel offset calibration
int16_t AccZcnt;                    // counter used to trim Z axis offset drift
float angle_acc;                    // accelerometer angle average value
float angle_acc_Av;                 // accelerometer angle averaged
float angle_acc_i;                  // accelerometer angle instantaneous value
float angle_acc_last;               // previous averaged accelerometer angle
float angle_acc_sum;                // accelerometer angles summed for averaging
float angle_gyro;                   // compound gyro angle
String Any$;                        // temporary string
int AnyInt;                         // any value used for monitoring and debug
long AnyLong;                       // any temp value
unsigned long AnyUS;                // any value used for monitoring and debug
float AnyVal;                       // any value used for monitoring and debug
byte auto_byte;                     // overide value for Wii demands
int32_t BatAvg;                     // preload averaging function with high value
int32_t BatDist;                    // used battery discharge testing
int32_t BatSum;                     // preload averaging sum with 20x max value
int32_t BatVol;                     // instantaneous battery voltage
uint8_t Bright;                     // FastLED brightness
uint8_t Brightness;                // Display brightness
int16_t CalAcc;                     // if > 0 then perform accelerometer offset measurements
int16_t CalCnt;                     // counter used in accelerometer offset measurements
int16_t CalDel;                     // delay count used in TEST offset calibration
int16_t CalGyr;                     // if > 0 then perform gyro offset measurements
int16_t CalMPU;                     // if > 0 then perform offset measurements
int16_t CalPnt;                     // task pointer used in offset measurements
int16_t C_Button;                   // button pressed state; 0 = UP, 1 = Down
int16_t C_Cnt;                      // counter used in 'C' button detection
byte C_Dn;                          // counter for 'C' button down period
byte C_Up;                          // counter for 'C' button up period
char cmdMode;                       // command mode
int16_t cmdSgn;                     // cmdVal sign, 0 or -1
char cmdType;                       // command mode type
int16_t cmdVal;                     // value associated with a cmdType
int16_t CZ;                         // received button values
bool CZ_Wait;                       // flag used for button release wait function
byte DataPnt;                       // received serial data pointer
char Deg = 247;                     // ASCII for "°"
int16_t DIR_Lft;                    // direction sign, +ve forward, -ve backwards
int DIR_LTnd;                       // motor left direction trend counter
int16_t DIR_Rht;                    // direction sign, +ve forward, -ve backwards
int DIR_RTnd;                       // motor right direction trend counter
bool DispClr;                       // if == true then clear the display before updating it
int16_t DispCnt;                    // timer used to delay Display MOnitor updates
uint16_t DispCol = tft.color565(0,0,0); // display area drawing colour is black
int16_t DispDel;                    // display delay interval counter
int16_t DispLast;                   // tracks current DispMode
int16_t DispMode;                   // if > 0 then display text data defined by DispMode
bool DispMon;                       // == true if connected to OLED Mirror app
int16_t DispNext;                   // if > 0 then set display to DispNext once DispDel == 0
unsigned long Dispms;               // timer used with Display Monitor app
int16_t DispTx;                     // counter used with display mirror app
int16_t Dither;                     // overlay signal used to reduce stiction
bool DithON;                        // dither applied flag, true = on, false = off
int16_t EbwY;                       // centre Y for eyebrows
int16_t ESP_NOW_BA;                 // broadcast address pointer, -1 to start with
bool ESP_NOW_Init;                  // == true once ESP-NOW has been initiaiised
int16_t Exp;                        // expression state
int16_t ExpPnt;                     // pointer used in expression tasks
uint16_t eye_h;                     // height of an eye socket
int16_t eye_Ox;                     // eye X offset from socket ctr
int16_t eye_Oy;                     // eye Y offset from socket ctr
int16_t eye_rad;                    // rounded eye radius
uint16_t eye_s2;                    // eye separation /2
uint16_t eye_w;                     // width of an eye socket
uint16_t faceCol = tft.color565(255,255,0); // face drawing colour is yellow
int16_t FaceDel;                    // delay factor used in Face tasks
int16_t FaceShow;                   // face drawing task pointer for Core 0
int16_t FaceTask;                   // Face task pointer
int16_t Gear;                       // Determines drive and turn speed settings 1 - 4
int16_t GPmax,GPmin;                // start and end of graphing list, for Scope mode
int16_t GphMode;                    // distinguishes app used in graphing mode
long gyro_pitch_calibration_value;  // gyro drift compensation value
int16_t gyro_pitch_data_raw;        // gyro X value read from MPU6050
int16_t gyro_turn_data_raw;         // gyro Y value read from MPU6050
long gyro_yaw_calibration_value;    // gyro drift compensation value
int16_t gyro_yaw_data_raw;          // gyro Z value read from MPU6050
int16_t GyrRawX;                    // 2-byte value read from the MPU
int16_t GyrRawY;                    // 2-byte value read from the MPU
int16_t GyrRawZ;                    // 2-byte value read from the MPU
unsigned long GyT;                  // time of gyro readings in milliseconds
unsigned long GyTD;                 // time between successive gyro readings in milliseconds
bool HeadAtt;                       // = true when head servo has been attached
int16_t HeadDel;                    // delay counter for HEAD tasks
int16_t HeadSubTsk;                 // head subtask pointer
int16_t HeadTask;                   // task pointer for HEAD tasks
int16_t HeadTgt;                    // target head servo angle
int16_t HeadVal;                    // current head servo angle in microseconds
int16_t HeartBright;                // heart pulse brightness
int16_t HeartDel;                   // LED delay used for heart pulses
byte I2C_Err;                       // flag returned from I2C comms
bool I2C_LTOF;                      // true if VL53L1X is detected during POST
bool I2C_MPU;                       // true if MPU6050 is detected during POST
unsigned long IntLast[] = {0,0};    // previous interrupt times in micros()
unsigned long IntLast40[2];         // revolution period in ms, 20 slots, 40 counts
unsigned long IntMicros[2];         // times in micros()
bool  IntP34Ch;                     // if == true then a slot transition has occured
int16_t IntP34Cnt;                  // pulse count for pin INT_P34
int Int34RPM;                       // calculated rpm for INT_34 slot, actually rpm x 10
bool IntP34State;                   // latest state of the INT_P34 input HIGH/LOW
bool  IntP35Ch;                     // if == true then a slot transition has occured
int16_t IntP35Cnt;                  // pulse count for pin INT_P35
int Int35RPM;                       // calculated rpm for INT_35 slot, actually rpm x 10
bool IntP35State;                   // latest state of the INT_P35 input HIGH/LOW
unsigned long IntPd;                // change in slot periods
unsigned long IntPeriods[2];        // inter-slot periods in µs
int16_t JoyX;                       // received value
int16_t JoyY;                       // received value
long Joy_Y;                         // > 0 when joystick Y is pressed #######################
int JX;                             // global variable used with Joystick app
int JY;                             // global variable used with Joystick app
char keyChar;                       // any keyboard character
int16_t keyVal;                     // any keyboard value
int16_t LED_Cnt;                    // LED counter controls light sequence
int16_t LEDDel;                     // delay counter used in LED tasks
bool LEDshow;                       // if true then update LED strip
int16_t LEDTask;                    // task pointer for LED tasks
byte low_bat;                       // low battery voltage detected if > 0
bool LTOFcalc;                      // if == true then calculate speed of ranged object in m/s
int16_t LTOFfps;                    // ranger sensor measurement rate in fps
int16_t LTOFperiod;                 // range sensor inter-measurementg period in millis()
int16_t LTOFspd10;                  // speed of ranged target in m/s x 10
int16_t LTOFspd10L;                 // previous speed of ranged target in m/s x 10
int16_t LTOFspdCnt;                 // speed peak detector sustain limit
int16_t LTOFt0,LTOFt1;              // range sensor timers for fps
int16_t MainMode;                   // balance state
int16_t MainTask;                   // main mode task pointer, used by all modes
String Message$;                    // text sent to Display M<Onitor app if connected
int16_t MotorSwp;                   // flag used in TEST mode to enable motor sweeping
int16_t mouthY;                     // vertical position of mouth
int16_t  moveDir;                   // a flag, stationary = 0; forward = 1; reverse = -1
float mtsInc;                       // speed adjust increment
String myMAC;                       // MAC address if this device
unsigned long next_4ms;             // 4ms (250Hz) loop timer
bool Once;                          // if true don't refresh eyes
float PID_d;                        // derivative element of PID controller calculations
int16_t PID_d_En;                   // PID d-gain enable 1=ON, 0-OFF
int16_t PID_En_Cnt;                 // counter used to enter gain disable modes
float pid_error_temp;               // current PID error
int16_t PID_i_En;                   // PID i-gain enable 1=ON, 0-OFF
float pid_i_mem;                    // used in PID integral calcs
float pid_last_d_error;             // used in PID derivative calcs
float pid_output;                   // current PDI output
float pid_output_left;              // steering modified PDI output
float PID_Output_Power;             // used in soft start
float pid_output_right;             // steering modified PDI output
int16_t pid_output_trend;           // tracker used in self-balancing
float PID_p;                        // P element of the PID calculation
float pid_PWM;                      // current PID PWM output
float pid_setpoint;                 // setpoint angle when moving
int16_t PID_Tune;                   // > 0 when in tuning mode
int16_t Ping;                       // Rx 'ping' counter
bool POST;                          // == true until after runPOST()
int16_t Print40ms;                  // 40ms task pointer
int16_t PrintTgt;                   // 0 == serial port, 1 == WiFi ESP-NOW
String PrintTx = "";                // printed strings
int16_t PWM_Cnt;                    // counter used in TEST motor sweeping
bool PwmEn;                         // if == true motor PWM is enabled
int16_t PWM_Inc;                    // inc/dec value used in TEST motor sweeping
int16_t PWM_Lft;                    // left-hand PWM +/-0-255
int16_t PWM_Rht;                    // right-hand PWM +/-0-255
float PWM_Start;                    // value needed to ensure motor start
int Range;                          // range value limited to RangeMax
int16_t RangeAng;                   // angle at which minimum range was measured
int16_t RangeAngMin;                // angle at which minimum range was recorded
int16_t RangeCentre;                // distance value looking forward
int16_t RangeCentreTracker;         // slugged distance value looking forward
int16_t RangeData[RngDpth];         // range stores at 3° intervals
int16_t RangeDataDP;                // pointer to RangeData[], effectively an angle/3
int16_t RangeDataDPL;               // previous pointer to RangeData[], effectively an angle/3
bool RangeEn;                       // == true if VL53L1X ranging is active
int RangeDiff;                      // difference in RangeRaw from RangeLast
long RangeIntL;                     // L-H integrator used in head centring
long RangeIntLL;                    // previous averaged L-H integrator used in head centring
long RangeIntR;                     // R-H integrator used in head centring
long RangeIntRL;                    // previous averaged R-H integrator used in head centring
int RangeLast;                      // previous range measurement
int16_t RangeLeft;                  // distance value looking left
bool RangeLL;                       // == true if range < RangeMin
int RangeMax;                       // the current limited to Range measurements
int RangeMin;                       // the current lower limit of Range measurements
unsigned long RangeMinMin;          // min recorded range value during close range scanning 
unsigned long RangeMinVal;          // min range value during close range scanning 
int16_t RangeMinAng;                // angle at which RangeMin occured
int16_t RangeMinMem;                // RangeMin at end of each half sweep
bool RangeNew;                      // set == true when a new range is available
bool RangePnt;                      // true when a centre range value is recorded, or extremes of sweep
int RangeRate;                      // max rate of inter-measurement Range change, default = 64
int RangeRaw;                       // range value obtained from VL53L1X
int RangeRawF;                      // rate limited RangeRaw value
int16_t RangeRight;                 // distance value looking right
int16_t RangeTrig;                  // range trigger point used in head scanning
bool RangeUL;                       // == true if range exceeds RangeMax
int receive_counter;                // used to extend received data for 100 ms
byte received_byte;                 // received byte from WiFi reciever
byte received_mem;                  // snapshot of received byte value
long RPms;                          // time between receiving lasp R. request
int RPnn;                           // reporting reference
bool RP_ON;                         // flag used to control data reporting
int RPP;                            // reporting phase pointer
long RPt0 = millis();               // millis() tracker for R. requests
bool RP_Title;                      // == true if title sent to the PID controller graph
int16_t Rx_Chk;                     // Rx checksum
int16_t RxCZ;                       // buffered received CZ value
byte RxData;                        // received serial data value #################
int16_t RxJoyX;                     // buffered received JoyX value
int16_t RxJoyY;                     // buffered received JoyY value
int16_t Rx_len;                     // length of WiFi received data block
int16_t Rx_Pnt;                     // data pointer user by Rx state machine
bool RxRec = false;                 // true if a byte is recevied over WiFi
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
int16_t RTxTimeout;                 // counter used to auto-reset if WiFi fails
int16_t RxVal;                      // value received from serial Rx
byte RxWiFi[6];                     // array holding 6 bytes of Wii received data
int16_t safeCnt;                    // time-out counter used in safe mode function
int16_t safeMode;                   // start mode of safety state machine
int16_t safeModeLast;               // previous safe mode
int16_t safePntCnt;                 // print timer counter
bool SerialRx;                      // == true if any character has been received
float self_balance_pid_inc;         // values used to inc/dec balance setpoint (0.0015)
float self_balance_pid_setpoint;    // offset used to set natural balance point
int16_t ServoPnt;                   // servo target pointer for manual control
int16_t sktLftX;                    // eye socket left X
int16_t sktRhtX;                    // eye socket right X
int16_t sktY;                       // eye socket top Y
bool SLEEP;                         // set = true when going into sleep mode
int16_t sleepCnt;                   // sleep trigger counter
bool SlotEn;                        // if == true then constantly monitor wheel slot encoders
unsigned long SlotMs;               // 1ms timer used to control slot scanning
bool SlotRd;                        // == true when slots are being read, blocks Core interaction
byte startEn;                       // > 0 to perform motor calcs and enable output
int16_t StatAuto;                   // status display auto pointer
bool StatClr;                       // if == true then clear the status before updating it
int16_t StatCnt;                    // counter used with battery status display
int16_t StatDel;                    // status delay interval counter
int16_t StatLast;                   // tracks current StatMode
int16_t StatMode;                   // if > 0 then display ststus text data defined by StatMode
int16_t StatSet;                    // status display mode set by the user
String Status$;                     // text sent to the display as status
float setPointStart;                // setpoint start target for different gear settings
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;                       // if == true don't perform switch functions
bool sw0State;                      // state of read button switch pin
int16_t sw0Timer;                   // timer used to detemine button sequences
bool sw0Wup;                        // set == true for SW0 to wait for button release
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;                       // if == true don't perform switch functions
bool sw1State;                      // state of read button switch pin
int16_t sw1Timer;                   // timer used to detemine button sequences
bool sw1Wup;                        // set == true for SW1 to wait for button release
int16_t Task_8ms;                   //  8ms task pointer
int16_t Task20ms;                   // 20ms task pointer
int16_t Task40ms;                   // 40ms task pointer
bool TEST = false;                  // set == true during assembly, then false for normal use
uint16_t TextCol = tft.color565(255,255,255); // text drawing colour is light blue
int16_t Trip;                       // vertical trip timer
int16_t turning;                    // > 0 if turning demand received
int16_t Tx_PingCnt;                 // 1 second Tx ping timer
bool USB;                           // == true if initial battery is below 5.5v
uint8_t VL53L1X_OC;                 // VL53L1X ROI SPAD centre
uint16_t VL53L1X_ROIX;              // VL53L1X ROI SPAD width
uint16_t VL53L1X_ROIY;              // VL53L1X ROI SPAD height
unsigned long VL53L1X_Skip;         // read function skip timer
unsigned long VL53L1X_SkipDel;      // read function skip timer delay tracker
int16_t VL53L1X_Status;             // tracks value of status register
int16_t VL53L1X_Task;               // if > 0 then run VL53L1X tasks
unsigned long VL53L1X_Timer;        // general timer
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 with prolonged 'C' button activity
int16_t WiFiPing;                   // Rx counter used to maintain connected state
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
byte WiFiRxMacAddr[6];              // senders MAC address, which could be broadcast FF:FF:FF:FF:FF:FF
bool WiFiRxRec;                     // true when a block of data has been successfully received
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
bool WiiOveride;                    // set == true to overide Wii demands
int16_t WiiType;                    // device type, 0 == Nunchuk, 1 == Classic
int Z_Button;                       // button pressed state; 0 = UP, 1 = Down
byte Z_Dn;                          // button pressed state; 0 == UP, >= 1 == Down
bool Z_Mode;                        // joystick motion flag, normally false, if == true then slide mode

// define arrays
uint16_t eyeBtL[] = {0,0,0,0}; // left eyebrow target X0,Y0,X1,Y1
uint16_t eyeBtR[] = {0,0,0,0}; // right eyebrow target X0,Y0,X1,Y1
uint16_t eyeBwL[] = {1,1,1,1}; // left eyebrow data X0,Y0,X1,Y1
uint16_t eyeBwR[] = {1,1,1,1}; // right eyebrow data X0,Y0,X1,Y1
int16_t eyeLft[] = {0,0,0,0};  // left eye data X,Y,Rad,Color
int16_t eyeRht[] = {0,0,0,0};  // right eye data X,Y,Rad,Color
int16_t mthHght[] = {-1,-1,-1,-1,-1,-1,-1,-1};  // mouth teeth heights
int16_t mthHtgt[] = {-1,-1,-1,-1,-1,-1,-1,-1};  // mouth teeth height targets
int16_t mthVert[] = {-1,-1,-1,-1,-1,-1,-1,-1};  // mouth vertical offsets
int16_t mthVtgt[] = {-1,-1,-1,-1,-1,-1,-1,-1};  // mouth vertical offsets targets

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

void setup() {
  // Setup basic functions
  // Allow allocation of all timers
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  // Initialise PWM pins and output states
  pinMode(PinLftA, OUTPUT); digitalWrite(PinLftA,LOW);
  pinMode(PinLftB, OUTPUT); digitalWrite(PinLftB,LOW);
  pinMode(PinRhtA, OUTPUT); digitalWrite(PinRhtA,LOW);
  pinMode(PinRhtB, OUTPUT); digitalWrite(PinRhtB,LOW);
  
  // initialise other pins
  pinMode(BatPin,INPUT);        // ADC1_0  GPIO36 battery divider
  pinMode(INT_P34,INPUT);       // speed sensor left
  pinMode(INT_P35,INPUT);       // speed sensor right
  pinMode(sw0Pin,INPUT_PULLUP); // button switch sw0 Pin
  pinMode(sw1Pin,INPUT_PULLUP); // button switch sw1 Pin
  pinMode(XSHUT_PIN,OUTPUT); digitalWrite(XSHUT_PIN,LOW);     // switch VL53L1X OFF

  setDefaults();
  
  Serial.begin(115200);     //Start the serial port at 115200 baud
  Wire.begin();             //Start the I2C bus as master

  // initialise the FastLED component
  // a chain of 17 LEDs = 24 x 17 = 408 bits, taking 510us to show at 800kbps
  FastLED.addLeds<WS2812B, LED_Pin, GRB>(LED, NumLEDs);
  FastLED.setBrightness(Bright);

  // initialise the four PWM channels, setting each to max
  for (int zP = 0;zP < 4;zP++) {
    PwmSetup(zP);
    PwmAttach(zP);
    ledcWrite(zP, 0);
  }

  // initialise TFT display
  tft.begin();
  tft.setRotation(0); // set screen rotation
  tft.fillScreen(ILI9341_BLACK);

  // grab initial battery voltage before WiFi kills the ADC2 function
  // take 10 readings over a 10ms period
  for (int zI = 0;zI < 10;zI++) {BatVol+= analogRead(BatPin); delay(1);}
  BatVol/= 10;
    
  // initialise WiFi link
  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();
  
  // define the Core 0 task parameters
  // Core 0 is normally used for WiFi, but here we use it to read sensors
  // leaving Core 1 to create the LED patterns and clock them out
  // Note, this code starts and runs immediately, before loop() does
  xTaskCreatePinnedToCore(
    loop0,       // Task function.
    "Core0",     // name of task.
    10000,       // Stack size of task
    NULL,        // parameter of the task
    1,           // priority of the task
    &Core0,      // Task handle to keep track of created task
    0);          // pin task to core 0
  
  runPOST();                        // perform checks and calibration sequence

  // so off we go!...
  synchLoopTimers();
}

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

  // the controller will react to a soft RESET event
  // this function effectively restarts the code as if from power-up
void(* resetFunc) (void) = 0; //declare reset function at address 0

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

void loop() {
  // this main loop runs on a 4ms timer to trigger the balancing calculations.
  // Once they are complete it then triggers a series of tasks to perform other
  // functions, at sub-multiples of 4ms. For example, an 8ms task is used to read
  // the button switches. All task must be completed within the 4ms period.

  if ((millis() - next_4ms) >= 4) {
    // ############################################################################
    // ############################################################################
    //
    // PID controller code
    //
    // ############################################################################
    // ############################################################################
    // this is the main task which performs balance calculations every 4ms (250Hz)
    next_4ms = millis();                      // reset the 4ms loop timer
    
    if (safeMode < 4) {
       received_byte = 0x00;                  // must be in safeMode >= 4 to received Wifi data
    } else {  
      if (RxRec) {
        // convert Wii data into single byte binary format for use later
        // the balancebot does not use proportional joystick control
        received_byte = 0x00;                 // start with no demand
        if(JoyX < DbLLX)    received_byte |= 0b00000001;  // If the variable JoyX is smaller then 108 set bit 0 of the send byte variable
        if(JoyX > DbULX)    received_byte |= 0b00000010;  // If the variable JoyX is larger then 148 set bit 1 of the send byte variable
        if(JoyY < DbLLY)    received_byte |= 0b00001000;  // If the variable JoyY is smaller then 108 set bit 3 of the send byte variable
        if(JoyY > DbULY)    received_byte |= 0b00000100;  // If the variable JoyY is larger then 148 set bit 2 of the send byte variable
        if((CZ & 1) == 0) received_byte |= 0b00010000;    // If Z button pressed set bit 4
        if((CZ & 2) == 0) received_byte |= 0b00100000;    // If C button pressed set bit 5
        // if(RxWiFi[0] < DbLLX)    received_byte |= 0b00000001; //If the variable received_data[0] is smaller then 80 set bit 0 of the send byte variable
        // if(RxWiFi[0] > DbULX)    received_byte |= 0b00000010; //If the variable received_data[0] is larger then 170 set bit 1 of the send byte variable
        // if(RxWiFi[1] < DbLLY)    received_byte |= 0b00001000; //If the variable received_data[1] is smaller then 80 set bit 3 of the send byte variable
        // if(RxWiFi[1] > DbULY)    received_byte |= 0b00000100; //If the variable received_data[1] is larger then 170 set bit 2 of the send byte variable
        // if((RxWiFi[5] & 1) == 0) received_byte |= 0b00010000; //If Z button pressed set bit 4
        // if((RxWiFi[5] & 2) == 0) received_byte |= 0b00100000; //If C button pressed set bit 5

        receive_counter = 0;                  // Reset the receive_counter variable
        safePntCnt = 30;                      // block data reporting
        StatCnt = 0;                          // hold off battery status reporting
      } RxRec = false;                        // clear the data received flag
      
      // for modes, like PILOT, we can overide the Wii demands to take control
      received_mem = received_byte;           // remember Wii Rx demand
      if (WiiOveride) {received_byte = auto_byte;}  // overide user demands
      
      if(receive_counter <= 25)receive_counter ++;  // The received byte will be valid for 25 program loops (100 milliseconds)
      else {
        // After 100 milliseconds the received byte is deleted
        received_byte = 0x00; PID_En_Cnt = 0;
        PrintTx += "RxRec Lost!\n";
      }
    }
    
    if (TEST) {
      // in TEST mode we do not run the PID calculations for balancing
      // instead we run calibration functions to provide user data
      if (CalMPU > 0) {CalOffsets();}
    }
    else if (CalGyr > 0) {CalGyr = Calibrate_Gyros(CalGyr); Exp = ExpCal;}
    else {
      //#############################################################################
      // Angle calculations
      //#############################################################################
      // in this robot it is the horizontal Z-axis which is used to measure pitch angles
      // it is -ve when leaning forwards and +ve when leaning backwards
      // we also use the Y-axis accelerometer to look for the tipping point, as it is least affected by normal movement and vibration
      // the Y-axis is -8192 when the robot is stood up vertical
      Wire.beginTransmission(MPU_address);                    // Start communication with the MPU
      Wire.write(0x3D);                                       // Start reading ACCEL_YOUT_H at register 3D hex
      I2C_Err = Wire.endTransmission();                       // End the transmission
      delayMicroseconds(I2Cdel);                              // Allow MPU time to respond

      Wire.requestFrom(MPU_address, 4);                       // Request 4 bytes from the MPU
      AccRawY = Wire.read()<<8|Wire.read();                   // Combine the two Y-axis bytes to make one integer
      AccRawZ = Wire.read()<<8|Wire.read();                   // Combine the two Z-axis bytes to make one integer
      accelerometer_data_raw_i = AccRawZ + acc_cal_value;     // Add the accelerometer calibration value
      accel_delta = abs(accelerometer_data_raw - accelerometer_data_raw_i);
      // test for rapid overshoot due to vibration around zero angle point
      if (accelerometer_data_raw < accelerometer_data_raw_i) {
        if (accel_delta > accel_ramp_rate) {accelerometer_data_raw += accel_ramp_rate;}
        else {accelerometer_data_raw += accel_delta;}
      }
      else if (accelerometer_data_raw > accelerometer_data_raw_i) {
        if (accel_delta > accel_ramp_rate) {accelerometer_data_raw -= accel_ramp_rate;}
        else {accelerometer_data_raw -= accel_delta;}
      }
      if(accelerometer_data_raw > 8192.0)accelerometer_data_raw = 8192.0;     // Prevent division by zero by limiting the acc data to +/-8192;
      if(accelerometer_data_raw < -8192.0)accelerometer_data_raw = -8192.0;   // Prevent division by zero by limiting the acc data to +/-8192;
    
      angle_acc = asin((float)accelerometer_data_raw/8192.0)* 57.296;         // Calculate the current angle according to the accelerometer
      
      //#############################################################################
      // Falling over?
      //#############################################################################
      // Test for robot falling over using the Y-axis accelerometer
      // to save on calculation time we simply look for a threshold value of 5792
      // calculate using, AccRawY = 8192 * cos(Angle / 57.296)
      // as teh Y-axis is affected by rough terrain we include a 200ms timeout function top absorb the shocks
    //  Serial.println(AccRawY);
      if (AccRawY < 5792) {              // If the robot tips over by 45° or more and the start variable is > 0
        // robot has reached tipping angle, allow 100ms of disturbance time for rough terrain
        if (startEn > 0) {Trip++; if (Trip > 25) {GoSafe();}} else {Trip = 0;}
      } else {Trip = 0;}
      
      
      // in this robot it is the X-axis gyro which gives us the pitch rotation
      Wire.beginTransmission(MPU_address);                    // Start communication with the gyro
      Wire.write(0x43);                                       // Start reading GYRO_XOUT_H at register 43
      I2C_Err = Wire.endTransmission();                       // End the transmission
      delayMicroseconds(I2Cdel);                              // Allow MPU time to respond

      Wire.requestFrom(MPU_address, 6);                       // Request 6 bytes from the gyro
      gyro_pitch_data_raw = Wire.read()<<8|Wire.read();       // Combine the two bytes to make one integer
      gyro_turn_data_raw = Wire.read()<<8|Wire.read();        // Combine the two bytes to make one integer
      gyro_yaw_data_raw = Wire.read()<<8|Wire.read();         // Combine the two bytes to make one integer

      // determine gyro intermeasurement time for increased precision
      if (GyT > 0) {
        GyTD = micros() - GyT;  // get the differential time between readings
        GyT = GyTD + GyT;       // set the timer for the next reading
      } else {
        // 1st reading after reset
        GyT = micros();
        GyTD = 4000;
      }
      
      // determine gyro angles based on gyro rate o/p and sampling time
      // gyros are set at +/-250 deg/sec for 16-bit ADC = 32,767 FSD
      // to convert gyro value to deg/sec we divide reading by 131.068 (ie. 32,767/250)
      // we have recorded time period in GyTD in microseconds, between readings
      // so angle is = GyAng * (GyTD/1000000)/131.068
      // in this case GyTD is nominally 4000 microseconds (ie. 4ms loop)
  //  Serial.println(GyTD);
      float zDiv = (float)GyTD/131068000; // nominally 0.0000305 at 4ms
      gyro_pitch_data_raw = -gyro_pitch_data_raw;             // reverse the sign of the gyro due to MPU mounting
      gyro_pitch_data_raw += gyro_pitch_calibration_value;    // Add the gyro calibration value
      angle_gyro += (float)gyro_pitch_data_raw * zDiv;        // Calculate the angle travelled during this 10ms loop and add this to the angle_gyro variable
      if (angle_gyro > 180.0) {angle_gyro = 180.0;}             // limit +ve gyro swing
      else if (angle_gyro < -180.0) {angle_gyro = -180.0;}      // limit -ve gyro swing
      
      //#############################################################################
      // MPU-6050 gyro offset compensation
      //#############################################################################
      // Despite drift offset compensation, it is vertually impossible to eliminate gyro drift
      // So here we use a small amount of accelerometer angle to correct for this
      // We can only use a small proportion as the accelerometer can be very noisy at teh balance point
    
      float angle_offset = -(angle_gyro - angle_acc)* 0.002; // was 0.02
      angle_gyro += angle_offset; // Correct the drift of the gyro angle with the accelerometer angle

      // when not balancing, correct for gross errors in gyro drift using angle_acc
      if ((safeMode < 2) && (abs(angle_gyro - angle_acc) > 1.0)) {angle_gyro = angle_acc;}
    
      //#############################################################################
      // Go-ACTIVE test
      //#############################################################################
      if (safeMode == 1) {
        // test for being raised to upright start point
        // after running the robot it was seen that the self-balance setpoint wanted
        // to be approx -1.7, so we set the trigger point around this, +/-0.5
        if((startEn == 0) && (angle_acc > -2.1) && (angle_acc < -1.1)){ // If the accelerometer angle is almost 0, default +/-0.5
          DispMode = 8; DispDel = 0;
          switch (MainMode) {
            case 0: DispNext =  5; break;               // display gyro angles in balance mode
            case 1: DispNext = 16; VL53L1X_ON(); break; // display LTOF range in pilot mode
          }
          angle_gyro = angle_acc;                                       // Load the accelerometer angle in the angle_gyro variable
          self_balance_pid_setpoint = -0.5;                             // reset any previous set points
          pid_output_trend = 0;                                         // zero the self balance trend data  
          pid_setpoint = 0.0;                                           // reset the Y demands on the PID setpoint
          pid_i_mem = 0;                                                // start with zero integrator term
          startEn = 1;                                                  // Set the start variable to start the PID controller
          PID_Output_Power = 20.0;                                      // soft start from 50%
          moveDir = 0;                                                  // reset the direction pointer
          safeMode = 2;                                                 // set safe mode flag to RUNNING
        }
      }
    
      //#############################################################################
      // PID controller calculations
      //#############################################################################
      // The balancing robot is angle driven. First the difference between the desired angel (setpoint) and actual angle (process value)
      // is calculated. The self_balance_pid_setpoint variable is automatically changed to make sure that the robot stays balanced all the time.
      pid_error_temp = angle_gyro - self_balance_pid_setpoint - pid_setpoint;

      // The (pid_setpoint - pid_output * 0.015) part functions as a speed limiting function.
      // Disable this function when first tuning your robot as it can cause instability, but make it active once tuned
      if (startEn) {  // only apply this function when balancing
        // the brake is only applied whem the output power is higher than the stiction level
        if (pid_output > 10.0 || pid_output < -10.0) {
          // brake factor = 0.015
          pid_error_temp += pid_output * 0.015 ;
        //  PrintTx += "Brake\n";
        }
      }
    
      if (PID_i_En > 0) {
        pid_i_mem += pid_i_gain * pid_error_temp;                       // Calculate the I-controller value and add it to the pid_i_mem variable
        if (pid_i_mem > pid_out_max) pid_i_mem = pid_out_max;           // Limit the I-controller to the maximum controller output
        else if (pid_i_mem < -pid_out_max) pid_i_mem = -pid_out_max;
      } else {pid_i_mem = 0;}
      
      //Calculate the PID output value
      PID_p = pid_p_gain * pid_error_temp;                              // always include p - proportional elemnet
      pid_output = PID_p;                                               // use a separate variable to accumulate the output
      
      if (PID_i_En > 0) {pid_output += pid_i_mem;}                      // optional i - integrator element
      
      if (PID_d_En > 0) {
        PID_d = pid_d_gain * (pid_error_temp - pid_last_d_error);       // optional d - differentiator element
        pid_output += PID_d;
      }
      if (pid_output > pid_out_max) {pid_output = pid_out_max;}         // Limit the PI-controller to the maximum controller output
      else if (pid_output < -pid_out_max) {pid_output = -pid_out_max;}
    
      pid_last_d_error = pid_error_temp;                                // Store the error for the next loop

      //#############################################################################
      // remap PID output to compensate for start friction
      //#############################################################################
      // pid_PWM is the PWM value to be applied to the DC motors
      // as these motors have stiction in the gearbox, we remap the output to begine at PWM_Start
      // we also add in a small deadband around the 0.0 value
      if (pid_output > 0.5) {pid_PWM = map(pid_output,0.0,255.0,PWM_Start,255.0);}
      else if (pid_output < -0.5) {pid_PWM = map(pid_output,-255.0,0.0,-255.0,-PWM_Start);}
      else {pid_PWM = 0.0;}

      //#############################################################################
      // limit pid_outputs to pid_out_max
      //#############################################################################
      if (pid_PWM > pid_out_max) {pid_PWM = pid_out_max;}     // Limit the PI-controller to the maximum controller output
      else if (pid_PWM < -pid_out_max) {pid_PWM = -pid_out_max;}
  
  
      //#############################################################################
      // limit output power during initial switch into self-balancing mode
      //#############################################################################
      if (PID_Output_Power < 100.0) {
        // PID_Output_Power can range from 0 - 100%, normally starts at 50%
        pid_PWM = (pid_PWM * PID_Output_Power)/100.0;
        PID_Output_Power += 1.0;  // slowly increase power output to 100%
      }
    
      //#############################################################################
      // Nunchuk Control calculations
      //#############################################################################
      turning = 0;                                              // Clear turning demand flag
      pid_output_left = pid_PWM;                             // Copy the controller output to the pid_output_left variable for the left motor
      pid_output_right = pid_PWM;                            // Copy the controller output to the pid_output_right variable for the right motor
    
      if(received_byte & 0b00000001){                            // If the first bit of the receive byte is set change the left and right variable to turn the robot to the left
        // Joystick pressed left -X
        if (PID_Tune == 0) {
          // in normal control mode
          turning = 1;
          if (turn_speed < turning_speed) {turn_speed += 0.15;} // progressively increase turning speed
          if (moveDir >= 0) {
            pid_output_left -= turn_speed;                      // Increase the left motor speed
            pid_output_right += turn_speed;                     // Decrease the right motor speed
          } else {
            pid_output_left += turn_speed;                      // Decrease the left motor speed
            pid_output_right -= turn_speed;                     // Increase the right motor speed
          }
        } else {
          // forced into PID tuning mode with 'C' button
          // Joy-X is used to adjust PID variables p, i & d
          received_byte  = received_byte & 0b11111110;
          switch(PID_Tune) {
            case 1:
              // PID p tuning mode 1.5 - 15.0
              if (pid_p_gain > 1.5) {pid_p_gain -= 0.1;}
              else {pid_p_gain = 1.5;}
              Serial.println(""); Serial.println(pid_p_gain,1);
              break;
            case 2:
              // PID i tuning mode 0.15 - 1.5
              if (pid_i_gain > 0.15) {pid_i_gain -= 0.01;}
              else {pid_i_gain = 0.15;}
              Serial.println(""); Serial.println(pid_i_gain,2);
              break;
            case 3:
              // PID d tuning mode 3.0 - 30.0
              if (pid_d_gain > 3.0) {pid_d_gain -= 0.1;}
              else {pid_d_gain = 3.0;}
              Serial.println(""); Serial.println(pid_d_gain,2);
              break;
          } safePntCnt = 30;                                    // temporarily block reporting
        }
      }
      
      else if (received_byte & 0b00000010){                      // If the second bit of the receive byte is set change the left and right variable to turn the robot to the right
        // Joystick pressed right +X
        if (PID_Tune == 0) {
          // in normal control mode
          turning = 1;
          if (turn_speed < turning_speed) {turn_speed += 0.15;} // progressively increase turning speed
          if (moveDir >= 0) {
            pid_output_left += turn_speed;                      // Decrease the left motor speed
            pid_output_right -= turn_speed;                     // Increase the right motor speed
          } else {
            pid_output_left -= turn_speed;                      // Increase the left motor speed
            pid_output_right += turn_speed;                     // Decrease the right motor speed
          }
        } else {
          // forced into PID tuning mode with 'C' button
          // JOy-X is used to adjust PID variables p, i & d
          received_byte  = received_byte & 0b11111101;
          switch(PID_Tune) {
            case 1:
              // PID p tuning mode 1.5 - 15.0
              if (pid_p_gain < 15.0) {pid_p_gain += 0.1;}
              else {pid_p_gain = 15.0;}
              Serial.println(""); Serial.println(pid_p_gain,1);
              break;
            case 2:
              // PID i tuning mode 0.15 - 1.5
              if (pid_i_gain < 1.5) {pid_i_gain += 0.01;}
              else {pid_i_gain = 1.5;}
              Serial.println(""); Serial.println(pid_i_gain,2);
              break;
            case 3:
              // PID d tuning mode 3.0 - 30.0
              if (pid_d_gain < 30.0) {pid_d_gain += 0.1;}
              else {pid_d_gain = 30.0;}
              Serial.println(""); Serial.println(pid_d_gain,2);
              break;
          } safePntCnt = 30;                                      // temporarily block reporting
        }
      } else {turn_speed = 0;}                                    // reset the turning component
    
      if (received_byte & 0b00000100){                             // If the third bit of the receive byte is set change the left and right variable to turn the robot to the right
        // Joystick pressed forward +Y
        if (PID_Tune == 0) {
          // in normal control mode
          moveDir = 1;
          
          // at a baud rate of 115200 we can send up to 42 characters every 4ms
        //  PrintTx += String(pid_setpoint,2) + "\t" + String(pid_output) + "\n";

          // in normal control mode
          if (pid_setpoint < setPointStart) {pid_setpoint += 0.05;}         // Quickly change the setpoint angle so the robot starts leaning forwards
          // apply power governor
          else if (pid_output < max_target_speed) {pid_setpoint += mtsInc;} // Slowly change the setpoint angle so the robot leans more forwards
          else if (pid_output > max_target_speed) {pid_setpoint -= mtsInc;} // Slowly change the setpoint angle so the robot leans less forwards
          //  PrintTx += String(pid_output) + "\n";
        } else {
          // in PID tuning mode
          if (Joy_Y < 50) {
            pid_setpoint += 0.05;
          } else {if (pid_setpoint > 0.0) {pid_setpoint -= 0.05;}}
          Joy_Y++;
        }
      }
      
      if(received_byte & 0b00001000){                                   // If the forth bit of the receive byte is set change the left and right variable to turn the robot to the right
        // Joystick pressed backward -Y
        if (PID_Tune == 0) {
          // in normal control mode
          moveDir = -1;
          if (pid_setpoint > -setPointStart) {pid_setpoint -= 0.05;}         // Quickly change the setpoint angle so the robot starts leaning forwards
          // apply power governor
          else if (pid_output > -max_target_speed) {pid_setpoint -= mtsInc;} // Slowly change the setpoint angle so the robot leans more forwards
          else if (pid_output < -max_target_speed) {pid_setpoint += mtsInc;} // Slowly change the setpoint angle so the robot leans less forwards

        } else {
          // in PID tuning mode
          if (Joy_Y < 50) {
            pid_setpoint -= 0.05;
          } else {if (pid_setpoint < 0.0) {pid_setpoint += 0.05;}}
          Joy_Y++;
        }
      }
    
      // No Y demand
      if(!(received_byte & 0b00001100)){
        // braking function, if no demand
        // slowly move the PID setpoint angle back to the zero position
        if (pid_setpoint > 0.5) {pid_setpoint -=0.05;}          // If the PID setpoint is larger than 0.5 reduce the setpoint with 0.05 every loop
        else if (pid_setpoint < -0.5) {pid_setpoint +=0.05;}    // If the PID setpoint is smaller than -0.5 increase the setpoint with 0.05 every loop
        else {
          pid_setpoint = 0.0;                                   // If the PID setpoint is smaller than 0.5 or larger than -0.5 set the setpoint to 0
          if(!(received_byte & 0b00000011)){moveDir = 0;}        // reset the direction pointer if not turning
        }
        Joy_Y = 0;                                              // Restore single-shot function in tuning mode
      }
    
      // Special 'C' button functions
      // used for PID tuning or special features
      if(received_byte & 0b00100000){
        // 'C' button pressed
        // check if user wants to inhibit PID gains?
        PID_En_Cnt++;
        if (PID_En_Cnt == 250) {
          // after 1 seconds if PID gains are disabled then re-enable them
          switch (PID_Tune) {
            case 1:
              // was in PID p tune mode 1
              // change to PID i tune mode 2
              PID_i_En = 1; PID_Tune = 2;
              break;
            case 2:
              // was in PID i tune mode 2
              // change to PID d tune mode 3
              PID_d_En = 1; PID_Tune = 3;
              break;
            case 3:
              // was in PID d tune mode 3
              // change to no tune mode 0
              PID_Tune = 0;
              break;
          }
        }
        if (PID_En_Cnt == 750) {
          // 'C' button held for more than 3 seconds so switch OFF PID i and d gains
          // Assume robot is on stand vertical so zero setpoints
          PID_i_En = 0; PID_d_En = 0; PID_Tune = 1;
          pid_setpoint = angle_gyro;  // use current angle as reference setpoint
          self_balance_pid_setpoint = 0.0;  // zero the self balancing offset
          Gear = 1; Load_Speeds(true);  // Select lowest speeds for tuning
        }
        // set higher speed mode?
        if ((Gear < 6) && (C_Button == 0) && (PID_Tune == 0)) {Gear++; Load_Speeds(true);}
        // tune values, uncomment to make one active
        C_Button = 1;
      } else {C_Button = 0;}
    
      // Special 'Z' button functions
      // used for PID tuning or special features
      if(received_byte & 0b00010000){
        // 'Z' button pressed
        // set low speed mode
        if ((Gear > 1) && (Z_Button == 0)) {Gear--; Load_Speeds(true);}
        Z_Button = 1;
      } else {Z_Button = 0;}

        
      //#############################################################################
      // Setpoint trims
      //#############################################################################
      // The self balancing point is adjusted when there is no forward or backwards movement from the transmitter.
      // This way the robot will always find it's balancing point
      // The trend function prevents the setpoint from being wound up via pushing or slopes
      if((pid_setpoint == 0.0) && (PID_Tune == 0)){                                                    //If the setpoint is zero degrees
        if(pid_PWM < -pid_sb_db){
          pid_output_trend--; if (pid_output_trend > 0) {pid_output_trend = 0;}
          if (pid_output_trend < -100) {pid_output_trend = -100;}
          else {self_balance_pid_setpoint += self_balance_pid_inc;}  //Increase the self_balance_pid_setpoint if the robot is still moving forewards
        }
        else if(pid_PWM > pid_sb_db){
          pid_output_trend++; if (pid_output_trend < 0) {pid_output_trend = 0;}
          if (pid_output_trend > 100) {pid_output_trend = 100;}
          else {self_balance_pid_setpoint -= self_balance_pid_inc;}   //Decrease the self_balance_pid_setpoint if the robot is still moving backwards
        } else {pid_output_trend = 0;}


        //#############################################################################
        // Z accelerometer offset long drift compensation
        //#############################################################################
        // the calibration value used for Z axis offset drift can vary over time
        // here we apply a long averaging adjustment to maintain that trim
        // this is only done whilst the robot is balancing, no demand
        if (safeMode == 4) {
               if (AccRawZ > 0) {AccZcnt++; if (AccZcnt >  500) {AccZcnt = 0; acc_cal_value--;}}
          else if (AccRawZ < 0) {AccZcnt--; if (AccZcnt < -500) {AccZcnt = 0; acc_cal_value++;}}
        }
      }
    
      //#############################################################################
      // Motor PWM applied
      //#############################################################################
      if (startEn) {
        PWM_Lft = pid_output_left; PWM_Rht = pid_output_right;
        MotorDriveTask();
        //  PrintTx += String(DIR_LTnd) + "," + String(DIR_RTnd) + "\n";
        //  PrintTx += String(Int34RPM) + "," + String(Int35RPM) + "\n";
      }
    }

    
    //#############################################################################
    // Safety modes
    //#############################################################################
    safety_Modes();   // track the condition of the robot every 4ms

    // at the end of the balancing code we set triggers for the other tasks
    // we decrement their task pointers to a -ve trigger point then set the 1st tasks
    Print40ms--; if (Print40ms <= -10) {Print40ms = 1;}
    Task_8ms--; if (Task_8ms <=  -2) {Task_8ms = 2;}
    Task20ms--; if (Task20ms <=  -5) {Task20ms = 4;}
    Task40ms--; if (Task40ms <= -10) {Task40ms = 6;}
  } else {
    // ############################################################################
    // ############################################################################
    //
    // background tasks
    //
    // ############################################################################
    // ############################################################################
    // perform the non-balancing tasks here
    // this section of code runs freely outside of the 4ms loop timer
    // tasks that run more frequently will be run ahead of less frequent ones

    //###############################################################################
    //
    //  Read slot sensors
    //
    //###############################################################################
    // slots are read asynchronously at a max rate rather than using interupts
    // this action is performed by both cores to improve accuracy
    // a 20 slot wheel has 40 edges, with max reading of 25 revs/sec = 1,500 rpm
    if (!SlotRd) {Read_Slots();}

    //###############################################################################
    //
    //  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) {WiFiReadRx();} // WiFi Rx buffer contains unread data so deal with it before adding new
    }
  
    readSerial();                 // check serial port on every loop cycle
  
    if (VL53L1X_Task > 0) {VL53L1X_Run();}  // perform laser measurement tasks
    
    //#############################################################################
    // 8ms tasks
    //#############################################################################
    if (Task_8ms > 0) {
      // tasks are performed in reverse order as the pointer is counting down
      switch(Task_8ms) {
        case 1: readSW1(); break;
        case 2: readSW0(); break;
      }
      Task_8ms--; // decrement task pointer towards 0
      
      //#############################################################################
      // 20ms tasks
      //#############################################################################
      // the 40ms tasks are performed after the 8ms tasks
    } else if (Task20ms > 0) {
        // 20ms tasks are performed in reverse order as the pointer is counting down
        switch(Task20ms) {
          case 1: battRead(); break;                      // monitor battery voltage
          case 2: doLEDTasks(); break;                    // perform LED task
          case 3: doHeadTasks(); break;                   // move head
          case 4: // select MainMode task
            switch(MainMode) {
              case 0: break;                              // null task
              case 1: MainPilotTask(); break;             // PILOT mode
              case 2: MainPlayTask(); break;              // PLAY mode
              case 3: MainAutoTask(); break;              // AUTONOMOUS mode
            } break;
        } Task20ms--;                                     // decrement task pointer towards 0
      
      //#############################################################################
      // 40ms tasks
      //#############################################################################
      // the 40ms tasks are performed after the 8ms tasks
    } else if (Task40ms > 0) {
        // 40ms tasks are performed in reverse order as the pointer is counting down
        switch(Task40ms) {
          case 1: ExpEngine(); break;
          case 2: if (DispTx > 0) {DispTx--; if (DispTx == 0) {DispMon = false;}} break;
          case 3: if (TEST) {MotorSweepTask();} break;
          case 4: if (Ping > 0) {Ping--;} break;    // time-out data received flag
          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 = 50; WiFiTryToConnect();
                  Serial.println("Trying to connect: " + getBAhex());
    //              Serial.println("Trying to connect WiFi");
                }
              }
            }
          //  PrintTx += "?"; // uncomment to test through channel
            break;
          case 6:
            if (WiFiPing > 0) {
              WiFiPing--;
              if (WiFiPing < 1) {
                 // we have stopped receiving so disconnect WiFi
                WiFiDisconnect();
                ESP_NOW_BA = -1;
                WiFiTryOnce = true;
    //            Serial.println("WiFiPing == 0");
              }
            } break;
        } Task40ms--;                               // decrement task pointer towards 0

    //###############################################################################
    //
    //  40 ms Print Task - Variable
    //
    //###############################################################################
    // this code uses either the serial port or the WiFi link to send messages
    // at 115200 baud we can send 11.5 chars/ms, so 64 chars takes 5.5ms.
    } else if (Print40ms > 0) {
      if (PrintTx.length() > 0) {
        // characters in string buffer so send some/all of them
        Print40ms = -6;       // 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) {
            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();
                Serial.println("Failed to Tx!");
              }
            }
          } else {
            // not connected so clear PrintTxdata
            PrintTx = "";
          }
        }
      }
    }
  }
}

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

void loop0(void * pvParameters) {
  // this function runs on Core 0, also used for ESP32 WiFi NOW
  // *** WARNING *** do not use for time consuming code
  // the following message, sent once at boot, confirms core 0 assignment
  Serial.print("Core0 task running on core "); Serial.println(xPortGetCoreID());

  for(;;){
    // infinite loop, just like loop() but running in Core 0
    // you must never use 'break' or 'return' statement here or it will crash
    // we tell the free RTOS system to call this function every 1 ms
  
  //    t1[2] = micros(); // end time
  //    t0[3] = micros(); // start time

    //###############################################################################
    //
    //  Read slot sensors
    //
    //###############################################################################
    // slots are read asynchronously at a max rate rather than using interupts
    // this action is performed by both cores to improve accuracy
    // a 20 slot wheel has 40 edges, with max reading of 25 revs/sec = 1,500 rpm
    if (!SlotRd) {Read_Slots();}

    //###############################################################################
    //
    //  Update RGB LEDs if needed
    //
    //###############################################################################
    // if the LEDshow flag is set we update the WS2812 strip as a priority
    if (LEDshow) {FastLED.show(); LEDshow = false;}  // update the strip

    else {
      // the 2.4" TFT display is 240 pixels wide and 320 pixels heigh
      // using serial SPI it still takes time to clear or refresh the whole are, so it
      // is subdivided into two primary areas, which are handled separately:
      // Face - 240 W x 256 H
      // Text - 240 W x  64 H
      //###############################################################################
      //
      //  Draw face on TFT display
      //
      //###############################################################################
      // this section of code draws the components of the face, starting with eyebrows
      // then eys and finally the mouth, in stages 1ms apart
      if (FaceShow > 0) {
        switch (FaceShow) {
          case 1:
            drawEyebrowLft(); // draw left eyebrow
            drawEyebrowRht(); // draw right eyebrow
            FaceShow++; break;
          case 2:
            // draw left eye
            if (Once) {tft.fillRoundRect(sktLftX,sktY,eye_w,eye_h,eye_rad,faceCol);}     // draw the back of the eye
            drawLftEye(); // draw the left eyeball
            FaceShow++; break;
          case 3:
            // draw right eye
            if (Once) {tft.fillRoundRect(sktRhtX,sktY,eye_w,eye_h,eye_rad,faceCol);}     // draw the back of the eye
            drawRhtEye(); // draw the right eyeball
            FaceShow++; break;
          case 4:
            drawMouth();    // draw the mouth using target values
            FaceShow = 0; Once = false; break;
        }
      }
      //###############################################################################
      //
      //  Draw text on lower display
      //
      //###############################################################################
      // every 1ms we check for text updates
      // the text region is further subdivided into:
      // Text   - 240 W x 48 H
      // Status - 240 W x 16 H
      // this is so that general text can be updated at a different rate to status text
      // variables altered in Core1 need to be buffered to prevent unwanted effects
      int16_t zDM = DispMode;                                 // Core1 to core0 handover
      if (zDM != 0) {
        if (DispDel > 0) {DispDel--;}
        else {
          if (DispClr) {Display_Clear(); DispClr = false;}    // blank out the lower portion 240x64 of the display
          if (DispLast != zDM) {Display_Clear();}             // clear background when mode changes
          
          // check for over-ride conditions
          if (!POST && !TEST) {
            // over-rides only work after runPOST() and not in TEST mode
            if (SLEEP)      {zDM = -3;} // SLEEPING
            if (CalGyr > 0) {zDM =  7;} // CALIBRATING
          }
          
          switch(zDM) {
            case -3: Display_Sleeping(); break;
            case -2: Display_BattLow(); break;
            case -1: Display_Intro(); break;
            case  1: Display_AccOffsets(); break;
            case  2: Display_GyrOffsets(); break;
            case  3: Display_Ready(); break;
            case  4: Display_AngleAcc(); break;
            case  5: Display_AngleGyro(); break;
            case  6: Display_Happy(); break;
            case  7: Display_Calibrating(); break;
            case  8: Display_Active(); break;
            case  9: Display_Safe(); break;
            case 10: Display_Gear(); break;
            case 11: Display_Balance(); break;
            case 12: Display_P_Only(); break;
            case 13: Display_Pilot(); break;
            case 14: Display_Play(); break;
            case 15: Display_Autonomous(); break;
            case 16: Display_Range(); break;
            
            default: DispMode = 0; DispDel = 10; break;
          }
          DispLast = zDM;                                           // track latest display mode
          if (DispNext != 0) {DispMode = DispNext; DispNext = 0;}   // switch to the next display
          if (DispMon) {Display_Status();}
        }
      }
      // variables altered in Core1 need to be buffered to prevent unwanted effects
      int16_t zSM = StatMode;                               // Core1 to core0 handover
      if (zSM != 0) {
        // has a delay been set?
        if (StatDel > 0) {StatDel--;}
        else {
          // update the status text
          if (StatClr) {Status_Clear(); StatClr = false;}   // blank out the status portion 240x16 of the display
          if (StatLast != zSM) {Status_Clear();}            // clear status background when status mode changes
          StatLast = zSM;                                   // track latest status mode
          switch(zSM) {
            case  0: StatDel = 100; break;                  // draw no status for 100ms
            case  1: Status_Batteries(); break;             // batteries status (default)
            case 80: Status_Auto(); break;                  // automatically selected status text
          }
          // do the following if the Display Monitor app is connected
          if (DispMon) {Display_Status();}

          // uncomment the following to display Expression codes bottom left
  //          Display_TextLft(0,48,String(Exp),2);
        }
      }
    }
  //    t1[3] = micros(); // end time
  //    t0[2] = micros(); // start time

    // return to RTOS system with a 1ms call back, should allow rpm sensing <750rpm
    vTaskDelay(1/portTICK_PERIOD_MS);
  }
}

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

void Load_Speeds (bool zDisp) {
  // loads speed settings depending on 'Gear' value, 1 - 4
  switch (Gear) {
    case 1: max_target_speed =  70.0; setPointStart = 1.0; mtsInc = 0.005; turning_speed = 24.0; break;
    case 2: max_target_speed =  85.0; setPointStart = 1.2; mtsInc = 0.005; turning_speed = 28.0; break;
    case 3: max_target_speed = 100.0; setPointStart = 1.4; mtsInc = 0.005; turning_speed = 32.0; break;
    case 4: max_target_speed = 115.0; setPointStart = 1.6; mtsInc = 0.005; turning_speed = 36.0; break;
    case 5: max_target_speed = 130.0; setPointStart = 1.8; mtsInc = 0.005; turning_speed = 40.0; break;
    case 6: max_target_speed = 145.0; setPointStart = 2.0; mtsInc = 0.005; turning_speed = 40.0; break;
  }
  //  Serial.print("\nGear = "); Serial.println(Gear);  // report gear change
  safePntCnt = 25;      // delay data reporting for the above message
  if (!zDisp) {return;} // don't update the display
  
  if (DispMode != 10) {DispNext = DispMode; }
  DispMode = 10; DispDel = 0;
}

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

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;
  // incoming data will always be two or more characters, ie. text == $+++
  if (Rx_len < 2) {Serial.println("Rx Error - no Rx data"); return;}

  WiFiRxGetChecksum(Rx_len);  // generate a checksum from Rx data
  if (Rx_Chk != Rx_Buff.ESPdata[Rx_len]) {
    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
  //  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();
  //  Serial.print("Send Status: ");
  if (WiFiConnected) {
    // the link is already active so handle normal success/failure scenarios
    if (status == 0){
  //      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 {
  //      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
      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
      PrintTgt = 1;     // when a connection is made set print output to WiFi by default
    } else {
  //      Serial.println("Status = " + String(status));
      WiFiTxErrCnt++;
    }
    WiFiTx_CB = 1;
  }
}

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

void Report_Data() {
  // called from safety modes to report data during PID tuning, etc
  // use sparingly as WiFi link can't support much traffic
  // remove line commment // to enable a specific print function
  safePntCnt--;
  if (safePntCnt < 1) {
    safePntCnt = 25;  // reset print line timer
  //    Serial.println(AnyVal);
  //    Serial.print("Acc A "); Serial.println(angle_acc,1);
  //    Serial.println(angle_acc,1);
  //    Serial.print("Batt.Vt = "); Serial.println(battery_voltage);
  //    Serial.print(left_motor); Serial.print("  "); Serial.println(right_motor);
  //    Serial.print("Gyro A "); Serial.println(angle_gyro);
  //    Serial.print("PID.Bal.Off = "); Serial.println(self_balance_pid_inc,4);
  //    Serial.print("PID o/p = "); Serial.println(pid_output,2);
  //    Serial.print(pid_output,2); Serial.print("   "); Serial.println(pid_output_trend);
  //    Serial.print(pid_output_left,2); Serial.print("  "); Serial.println(left_motor);
  //    Serial.println(pid_output_left,2);
  //    Serial.print("PID.SP = "); Serial.println(pid_setpoint);
  //    Serial.print("SBSP "); Serial.println(self_balance_pid_setpoint,3);
  //    Serial.print(self_balance_pid_setpoint,4); Serial.print("   "); Serial.println(pid_output_trend);
  //    Serial.print("Steps = "); Serial.print(Step_Cnt_Left);
  //    Serial.print("   "); Serial.println(Step_Cnt_Right);
  //    Serial.print("TL = "); Serial.print(left_motor);
  //    Serial.print("    TR = "); Serial.println(right_motor);
  }
}

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

void reset_SafeMode() {
  //Reset safe mode values
  safeMode = 0; safeCnt = 0; safePntCnt = 0;
  Gear = 1; Load_Speeds(true);                                // Return to slow speed modes
  setMainMode(0);
}

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

void runPOST() {
  // start-up initialisation processes, mainly testing I2C addresses
  // 0x68 MPU 6050 3-axis motion sensor
  // 0x29 VL53L1X laser range finder
  Serial.println("\n\n" + Release + "  " + Date + "\n");
  Serial.println(F("Running POST..."));
  
  // Print TEST mode?... holding down SW1 during boot forces this condition
  if (!TEST) {if (digitalRead(sw0Pin) == LOW){TEST = true;}}
  if (TEST) {Serial.println("TEST mode active");}

  // initilaise RGB LED array
  FastLED.clear();
  FastLED.show(); // This sends the updated pixel color to the hardware.
  Serial.println(F("RGB LEDs cleared."));

  // initialise the face system
  initFace();
  setBrows(4); setMouth(1);
  drawFace(); Once = false;
  
  // display BalanceBot Mk1 intro
  DispMode = -1;

  // centre the head
  HeadCtrNow();
  
  // Test for MPU 6050 3-axis motion sensor and initialise
  Wire.beginTransmission(MPU_address);
  I2C_Err = Wire.endTransmission();
  if (I2C_Err == 0) {
    Serial.println(F("Initialising Gyros..."));
     I2C_MPU = true; MPU_6050_Initialise();
  } else {I2C_MPU = false; Serial.println(F("MPU6050 Not Found!"));}
  
  // Test for VL53L1X laser sensor and initialise
  pinMode(XSHUT_PIN,OUTPUT); digitalWrite(XSHUT_PIN,HIGH);  // enable VL53L1X sensor
  Wire.beginTransmission(0x29);
  I2C_Err = Wire.endTransmission();
  if (I2C_Err == 0) {
    I2C_LTOF = true; Serial.println(F("Initialising VL53L1X..."));
    VL53L1X_ON();                 // turn it ON briefly to confirm that it is there
  } else {I2C_LTOF = false; Serial.println(F("VL53L1X Not Found!"));}

  // set up battery quick averaging
  BatVol = analogRead(BatPin);
  BatAvg = BatVol; BatSum = BatVol * 50;  // preload the battery sum for averaging
  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(BatVol) + "\t" + String(float(BatAvg)/BatCal) + "v\t" + String(AnyLong) + "%");
  randomSeed(BatVol); // use the battery voltage as the random seed factor
  // if battery is less than 5.5v then USB mode is assumed, which makes servos inoperable
  if (BatAvg <= 2111) {USB = true; Serial.println(F("USB Power assumed"));}

  VL53L1X_OFF();   // turn the laser OFF by default if not in TEST mode
  WaitSwUp();

  Serial.println(F("POST completed"));
  if (TEST) {
    CalGyr = 0; // disable auto-gyro calibration on reset, when in TEST mode
    CalMPU = 1; // perform offset calibration measurements continuously
  }
  delay(2000);  // pause whilst displaying Intro text
  POST = false; // clear the POST flag
}

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

void safety_Modes() {
  // this function handles safety from the outset, beginning in mode 0.
  //
  // -9 - short random LED burst whilst in safeMode 0
  // -4 - sleeping on its back, waiting for movement
  // -3 - sleep state, waiting for movement
  // -2 - non-responsive calibration state
  // -1 - non-responsive state, battery nearly flat
  //  0 - waiting for BalanceBot to be placed on its back
  //  1 - waiting for BalanceBot to be raised to the upright position
  //  2 - going active, set red LEDs ON
  //  3 - dead band adjustment
  //  4 - fully active, BalanceBot can receive Nunchuk data
  //
  if (TEST) {return;} // not needed in TEST mode
  
  if (safeMode != safeModeLast) {Serial.print("\nSafeMode = "); Serial.println(safeMode);}
  safeModeLast = safeMode;
  if (CalGyr > 0) {safeCnt = 0; safeMode = -2; return;} // don't perform safety checks whilst calibrating
  
  switch(safeMode) {
    case 4:
      // robot is active, so flash heart LEDs if data received
      // send various values over Wi-Fi for debugging
      Report_Data(); StatMode = 80;
      break;
    case 3:
      // deadband adjustment for 1 second whilst getting average acceleration
      safeCnt++;
      if (safeCnt > 250) {
        // having ignored the first 250 readings we now move to safeMode = 4
        acc_data_raw_av += accelerometer_data_raw_i; acc_data_raw_Cnt++;
        safeMode = 4; Exp = ExpActive;
        Reset_Slots(); SlotEn = true; // enable slot sensor counting
      }
  //      if (safeCnt > 1000) {safeMode = 4; safePntCnt = 0;}
      Report_Data();
      break;
    case 2:
      // robot is going active, motors have just been enabled
      safeMode = 3; safeCnt = 0;
      acc_data_raw_av = 0; acc_data_raw_Cnt = 0; break;
    case 1:
      // robot is ready, and responding to tilting towards balance point when moved
      // check for user forcing a calibration task
      if (angle_acc >= -75.0) {
        if (DispMode == 3) {DispDel = 0;}   // display angle mode immediately
        DispMode = 5; StatMode = 80;        // display angle and auto
        Exp = ExpReadyAnx;
      } else {
        // lying down, may go to sleep
          DispNext = 3; StatMode = 1;         // display Ready and Batt
          safeCnt++; Exp = ExpReady;
          if (safeCnt >= 2500) {
            // unattended, so go to sleep lying down
            safeMode = -4; SLEEP = true;
          }
      }
      if (StatSet > 0) {StatMode = 80;}
      break;
    case 0:
      // Safe mode - drive systems disabled, LEDs on green
      // robot must be horizontal and steady for 1 seconds
      // check for on its stand, if so flash LEDs occasionally
      if (abs(angle_acc) <= 3.0) {
        // near vertical, so after a delay drop into Happy mode
        safeCnt++; Exp = ExpHappy;
        if (safeCnt > 750)  {DispMode = 6; DispDel = 0;} // display HAPPY
        if (safeCnt > 2500) {
          // unattended, so go to sleep
          safeMode = -3; SLEEP = true;
        }
      } else if (angle_acc <= -75.0) {
        // lying down, so switch to READY after a delay
        if (abs(angle_acc - angle_acc_last) < 3.0) safeCnt++;
        angle_acc_last = angle_acc;
        Exp = ExpLying;
        if (safeCnt >= 750) {
          // horizontal for 3 seconds so display ready and move into that state
          if (DispMode == 5) { DispDel = 0;}
          DispMode = 3; safeMode = 1; Set_LEDTask(2); // display READY
          Exp = ExpReady; safeCnt = 0;
        } 
      } else {
        // away from the vertical so display gyro angle
        if (DispMode != 5) {DispDel = 0;} // change to the angle display quickly
        safeCnt = 0; DispMode = 5;
        Exp = ExpHapAng;
      } break;
    case -1:
      // non-responsive state, but drive motors may still be active
      // after 10 seconds the robot will sleep, falling backwards if upright
      safePntCnt--; if (safePntCnt < 1) {
        // send messages over WiFi or via USB port
        safePntCnt = 20;  // reset print line timer
      } sleepCnt++;
      angle_acc_last = angle_acc; break;
    case -2:
      // coming out of calibration mode, so allow time for displaying gyro offsets
      safeCnt++; if (safeCnt > 250) {safeCnt = 0; safeMode = 0;}
      break;
    case -3:
      // in sleep state, waiting for some kind of movement
      if ((abs(angle_acc - angle_acc_last) > 2.0) || !SLEEP) {safeMode = 0; safeCnt= 0; DispDel = 10;  SLEEP = false;}
      break;
    case -4:
      // sleeping on its back, waiting for some kind of movement
      // if a button is pressed then the SLEEP flag is cleared and it wakes up
      if ((angle_acc >= -80.0) || !SLEEP) {safeMode = 1; safeCnt= 0; DispDel = 10;  SLEEP = false;}
      break;
  }
}

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

void setDefaults() {
  // load default values, declared here in case of a soft reset
  accel_delta = 0;        // raw accelerometer instantanious increase
  accel_ramp_rate = 30;   // raw accelerometer ramp rate, used to limit impulse noise at small angles
  angle_gyro = 0.0;       // compound gyro angle
  auto_byte = 0;          // overide value for Wii demands
  AccZcnt = 0;            // counter used to trim Z axis offset drift
  BatAvg = 4095;          // preload averaging function with high value
  BatAvg = 0;             // preload averaging function with high value
  BatSum = 72320;         // preload averaging sum with 20x max value
  Bright = 255;           // FastLED brightness
  Brightness = 128;       // Display brightness
  CalAcc = 0;             // if > 0 then perform accelerometer offset measurements
  CalDel = 0;             // delay count used in TEST offset calibration
  CalGyr = 1;             // if > 0 then perform gyro offset measurements
  CalMPU = 0;             // if > 0 then perform offset measurements
  C_Button = 0;           // button pressed state; 0 = UP, 1 = Down
  C_Cnt = 0;              // counter used in 'C' button detection
  C_Dn = 0;               // counter for 'C' button down period
  cmdMode = ' ';          // command mode
  cmdSgn = 1;             // cmdVal sign, 1 or -1
  cmdType = ' ';          // command mode type
  cmdVal = 0;             // value associated with a cmdType
  DataPnt = 0;            // received serial data pointer
  DIR_Lft = 0;            // direction sign, +ve forward, -ve backwards
  DIR_LTnd = 0;           // motor left direction trend counter
  DIR_Rht = 0;            // direction sign, +ve forward, -ve backwards
  DIR_RTnd = 0;           // motor right direction trend counter
  DispClr = false;        // if == true then clear the display before updating it
  DispCnt = 0;            // timer used to delay Display MOnitor updates
  DispDel = 0;            // display delay interval counter
  DispLast = 0;           // tracks current DispMode
  DispMode = 0;           // if > 0 then display text data defined by DispMode
  DispMon = false;        // == true if connected to OLED Mirror app
  DispNext = 0;           // if > 0 then set display to DispNext once DispDel == 0
  DispTx = 0;             // counter used with display mirror app
  Dither = 25;            // overlay signal used to reduce stiction
  Exp = ExpStart;         // start expression state
  ExpPnt = 0;             // pointer used in expression tasks
  eye_Ox = 0;             // eye X offset from socket ctr
  eye_Oy = 0;             // eye Y offset from socket ctr
  eye_rad = 7;            // rounded eye radius
  eye_s2 = 9;             // eye separation /2
  FaceDel = 0;            // delay factor used in Face tasks
  FaceShow = 0;           // face drawing task pointer for Core 0
  FaceTask = 0;           // Face task pointer
  Gear = 1;               // Determines drive and turn speed settings 1 - 4
  GphMode = 0;            // distinguishes app used in graphing mode, default = 0
  GyT = 0;                // time of gyro readings in milliseconds
  HeadAtt = false;        // = true when head servo has been attached
  HeadDel = 0;            // delay counter for HEAD tasks
  HeadSubTsk = 0;         // head subtask pointer
  HeadTask = 0;           // task pointer for HEAD tasks
  HeadTgt = Head_90;      // target head servo angle
  HeadVal = Head_90;      // current head servo angle in microseconds
  HeartBright = 4;        // heart pulse brightness
  HeartDel = 50;          // LED delay used for heart pulses
  Joy_Y = 0;              // > 0 when joystick Y is pressed #######################
  JX = 0;                 // global variable used in turning, with Joystick app
  JY = 0;                 // global variable used in turning, with Joystick app
  LED_Cnt = 0;            // LED counter controls light sequence
  LEDDel = 0;             // delay counter used in LED tasks
  LEDshow = false;        // if true then update LED strip
  LEDTask = 0;            // task pointer for LED tasks
  low_bat = 0;            // low battery voltage detected if > 0
  LTOFcalc = false;       // if == true then calculate speed of ranged object in m/s
  LTOFspd10 = 0;          // speed of ranged target in m/s x 10
  LTOFspd10L = 0;         // previous speed of ranged target in m/s x 10
  LTOFspdCnt = 0;         // speed peak detector sustain limit
  MainMode = 0;           // balance state
  MainTask = 0;           // main mode task pointer, used by all modes
  Message$ = "";          // text sent to Display M<Onitor app if connected
  MotorSwp = 0;           // flag used in TEST mode to enable motor sweeping
  mouthY = ctr_Y;         // vertical position of mouth
  moveDir = 0;            // a flag, stationary = 0; forward = 1; reverse = -1
  mtsInc = 0.005;         // speed adjust increment
  Once = true;            // if true don't refresh eyes
  PID_d_En = 1;           // PID d-gain enable 1=ON, 0-OFF
  PID_En_Cnt = 0;         // counter used to enter gain disable modes
  PID_i_En = 1;           // PID i-gain enable 1=ON, 0-OFF
  pid_output = 0.0;       // current PDI output
  pid_output_trend = 0;   // tracker used in self-balancing
  pid_PWM = 0;            // current PID PWM output
  PID_Tune = 0;           // > 0 when in tuning mode
  POST = true;            // == true until after runPOST()
  PrintTgt = 0;           // 0 == serial port, 1 == WiFi ESP-NOW
  PrintTx = "";           // printed strings
  PWM_Cnt = 0;            // counter used in TEST motor sweeping
  PwmEn = false;          // if == true motor PWM is enabled
  PWM_Inc = 1;            // inc/dec value used in TEST motor sweeping
  PWM_Lft = 0;            // left-hand PWM +/-0-255
  PWM_Rht = 0;            // right-hand PWM +/-0-255
  PWM_Start = PWM_StartMax; // value needed to ensure motor start
  RangeEn = false;        // == true if VL53L1X ranging is active
  RangeMax = LtofLimit;   // the current limited to Range measurements
  RangeMin = LtofMin;     // the current lower limited to Range measurements
  RangeRate = 64;         // max rate of inter-measurement Range change, default = 64
  RangeRawF = 1000;       // rate limited RangeRaw value (default)
  RPms = 0;               // time between receiving lasp R. request
  RPnn = 0;               // reporting reference
  RP_ON = false;          // flag used to control data reporting
  RPP = 0;                // reporting phase pointer
  RP_Title = false;       // == true if title sent to the PID controller graph
  Rx_Chk = 0;             // Rx checksum
  RxCZ = 0;               // buffered received CZ value
  RxData = 0;             // received serial data value #################
  RxJoyX = 0;             // buffered received JoyX value
  RxJoyY = 0;             // buffered received JoyY value
  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
  RTxTimeout = 0;         // counter used to auto-reset if WiFi fails
  RxVal = -1;             // value received from serial Rx
  safeCnt = 0;            // time-out counter used in safe mode function
  safeMode = 0;           // start mode of safety state machine
  safeModeLast = -1;      // previous safe mode
  safePntCnt = 0;         // print timer counter
  self_balance_pid_inc = 0.001;     // values used to inc/dec balance setpoint (0.0015)
  self_balance_pid_setpoint = 0.0;  // offset used to set natural balance point
  SerialRx = false;       // == true if any character has been received
  ServoPnt = 0;           // servo target pointer for manual control
  setPointStart = 1.0;    // setpoint start target for different gear settings
  SLEEP = false;          // set = true when going into sleep mode
  sleepCnt = 0;           // sleep trigger counter
  SlotEn = false;         // if == true then constantly monitor wheel slot encoders
  SlotRd = false;         // == true when slots are being read, blocks Core interaction
  startEn = 0;            // > 0 to perform motor calcs and enable output
  StatAuto = 0;           // status display auto pointer
  StatClr = false;        // if == true then clear the status before updating it
  StatCnt = 0;            // counter used with battery status display
  StatDel = 0;            // status delay interval counter
  StatLast = 0;           // tracks current StatMode
  StatMode = 1;           // if != 0 then display ststus text data defined by StatMode
  StatSet = 0;            // status display mode set by the user
  Status$ = "";           // text sent to the display as status
  sw0DwnTime = 0;         // button pressed down time
  sw0LastState = HIGH;    // previous state of button switch, HIGH/LOW
  sw0_Nop = false;        // if == true don't perform switch functions
  sw0State = HIGH;        // state of read button switch pin
  sw0Timer = 0;           // timer used to detemine button sequences
  sw0Wup = false;         // set == true for SW0 to wait for button release
  sw1Cnt = 0;             // button switch counter
  sw1LastState = HIGH;    // previous state of button switch, HIGH/LOW
  sw1_Nop = false;        // if == true don't perform switch functions
  sw1State = HIGH;        // state of read button switch pin
  sw1Timer = 0;           // timer used to detemine button sequences
  sw1Wup = false;         // set == true for SW1 to wait for button release
  turning = 0;            // >0 if turning demand received
  Task_8ms = 0;           //  8ms task pointer
  Task20ms = 0;           // 20ms task pointer
  Task40ms = 0;           // 40ms task pointer
  Trip = 0;               // vertical trip timer
  Tx_PingCnt = 0;         // 1 second Tx ping timer
  USB = false;            // == true if initial battery is below 5.5v
  VL53L1X_OC = 199;       // VL53L1X ROI SPAD centre
  VL53L1X_ROIX = 16;      // VL53L1X ROI SPAD width
  VL53L1X_ROIY = 16;      // VL53L1X ROI SPAD height
  VL53L1X_SkipDel = 33;   // read function skip timer delay tracker, nominally 33
  VL53L1X_Task = 0;       // if > 0 then run LV53L1X tasks
  WiFiCntC = 0;           // C button counter for WiFi enable
  WiFiConCnt = 0;         // counter used in connection retry times
  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
  WiFiRx = false;         // true when a block of data has been received
  WiFiRxBytes = 0;        // number of bytes receiv ed over the WiFi link
  WiFiRxErr = 0;          // 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
  WiiOveride = false;     // set == true to overide Wii demands
  WiiType = 0;            // device type, 0 == Nunchuk, 1 == Classic
  Z_Button = 0;           // button pressed state; 0 = UP, 1 = Down
  Z_Dn = 0;               // button pressed state; 0 = UP, 1 = Down
  Z_Mode = false;         // joystick motion flag, normally false, if == true then slide mode

  Load_Speeds(false);
  Reset_Slots();
}

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

void synchLoopTimers() {
  // reset main loop timers
  while (millis() < 4){}  // we must wait until millis() is at least 4 from reset

  Dispms = millis();      // timer used with Display Monitor app
  next_4ms = millis();    // set loop timer
  SlotMs = millis();      // 1ms timer used to control slot scanning
//  Serial.print("synchLoopTimers()");
}

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