// ################################################################################
//
//  Uni-Bot vR1
//
//  Released:  15/08/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 Dev Module  v3.0.2

    This code is used to control a unicycle robot, using the SimpleFOC librabry to
    drive three BLDC motors. Two of which are connected to reaction wheels, and the
    third is connected to a ground wheel.

    The code works in one of two isolated modes, to make is simpler to maintain. The
    mode of operation is selected on RESET by the orientation of the robot.
    
    If the robot is RESET in its vertical orientation, say on its stand, it assumes
    'Test Mode', in which all registers of the MPU6050 are read, and it responds to
    serial demands from special Windows apps. Test mode is also set if the robot is
    pointed upwards towards teh sky.

    If the robots forward facing reaction wheel is pointed downwards, after 1 sec it
    assumes 'Demo mode', in which only the registers of the MPU6050 needed for
    balance, are read, and it responds to demands from a Wi-Fi controller.

    When connected serially to the Monitor+ app, either via USB or over Wi-Fi the
    robot sends display data and responds to mouse clicks within the Monitor+ app. It
    can also send data for use in scope mode.

    Release R1:
    * has a modified start procedure to make it easier to balance initially.
    * works with Wii_ESP32C3_Transceiver
    
    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 <Wire.h>             // include the Wire.h library so we can communicate with the gyro
#include <SimpleFOC.h>        // simple FOC library         v2.3.4
#include <FastLED.h>          // Neopixel library           v3.7.1

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

// Configuration
String Release = "Uni-Bot vR1";
String Date = "15/08/2024";

// Define a task for core 0
TaskHandle_t Core0;

// Define constants
#define AcAvgDivMax 500         // max averaging divider, 500 == 5 second average
#define AccOffX 0               // calibration offset applied to X-axis accelerometer (0)
#define AccOffY  55             // calibration offset applied to Y-axis accelerometer (0)
#define AccOffZ 160             // calibration offset applied to Z-axis accelerometer (159)
#define BatCal 287.7            // calibrated voltage multiplying factor
#define BatCritical 2895        // critical battery threshold 10.0v, 0%
#define BatHigh 3402            // high charge, voltage == 11.4v, 80%
#define BatMax 3841             // A0 for fully charged battery voltage, == 12.3v, 100%
#define BatPin 36               // analog input pin ADC15 on GPIO12
#define BatWarn 3175            // battery threshold of 10.5v gives low warning, 20%
#define GyAvgDivMax 500         // max averaging divider, 500 == 5 second average
#define GyrOffX -542            // calibration offset taken from X-axis gyroscope (0)
#define GyrOffY -627            // calibration offset taken from Y-axis gyroscope (0)
#define GyrOffZ -272            // calibration offset taken from Z-axis gyroscope (0)
#define I2Cdel 20               // I2C bus timing delay for MPU6050
#define LED_Pin 15              // GPIO for RGB LED
#define MotorAEn 32             // motorA PWM enable is GPIO 32
#define motorA_VelMax 60.0      // maximum motorA velocity
#define motorAVolMax 4.0        // maximum PWM power_supply_voltage/2
#define MotorBEn  4             // motorB PWM enable is GPIO 4
#define motorB_VelMax 60.0      // maximum motorB velocity
#define motorBVolMax 4.0        // maximum PWM power_supply_voltage/2
#define MotorCEn 19             // motorC PWM enable is GPIO 19
#define motorC_VelMax 45.0      // maximum motorC velocity
#define motorC_VdMax 2.2        // motorC maximum rate of velocity change
#define motorCVolMax 4.0        // maximum PWM power_supply_voltage/2
#define MPU_address 0x68        // MPU-6050 I2C address (0x68 or 0x69)
#define NumLEDs 2               // number of RGB LEDs used

// define constants for Display Monitor app
#define BLACK 0
#define BLUE 5
#define GREEN 4
#define INVERSE 2
#define MAGENTA 8
#define RED 3
#define TEXT_ALIGN_LEFT 0
#define TEXT_ALIGN_RIGHT 1
#define TEXT_ALIGN_CENTER 2
#define TEXT_ALIGN_CENTER_BOTH 3
#define TURQUOISE 7
#define WHITE 1
#define YELLOW 6

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

// initialise simpleFOC components
// the GMB2804H-100T gimbal motors have 7 pole pairs
BLDCMotor motorA = BLDCMotor(7);                        // create a motor instance with 7 pole pairs
BLDCMotor motorB = BLDCMotor(7);                        // create a motor instance with 7 pole pairs
BLDCMotor motorC = BLDCMotor(7);                        // create a motor instance with 7 pole pairs
// here we assign GPIO pins to each SimpleFOC PWM driver instance, as IN1,IN2,IN2,En
// if you find that a motor turns in the wromng direction, then swap the 1st and 3rd pin numbers
BLDCDriver3PWM driverA = BLDCDriver3PWM(26,25,33,32);   // 3-phase PWM pins, phA, phB, phC, Enable
BLDCDriver3PWM driverB = BLDCDriver3PWM(16,17,18, 4);   // 3-phase PWM pins, phA, phB, phC, Enable
BLDCDriver3PWM driverC = BLDCDriver3PWM(23,27,13,19);   // 3-phase PWM pins, phA, phB, phC, Enable

// Create Wi-Fi structure
uint8_t broadcastAddress[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

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

// Create a 250 byte Tx buffer
ESPNOW_Buff250 Tx_Buff;

// Create a 250 byte receive
ESPNOW_Buff250 Rx_Buff;

// PID controller factors, determined from PID tuning
// these are set later in code with a call to setPIDgains()
float Ptch_p_gain;    // Gain setting for Pitch P-controller (1.0)
float Ptch_i_gain;    // Gain setting for Pitch I-controller (0.0)
float Ptch_i_max;     // limit Pitch I-controller output(2.0)
float Ptch_d_gain;    // Gain setting for Pitch D-controller (0.0)
float Ptch_out_max;   // limit Pitch PID output to limit to PWM max
float PtchStartAng;   // initial target angle for balancing Pitch
float Roll_p_gain;    // Gain setting for Roll P-controller (9.0)
float Roll_i_gain;    // Gain setting for Roll I-controller (0.2)
float Roll_i_max;     // limit Roll I-controller output(0.0)
float Roll_d_gain;    // Gain setting for Roll D-controller (0.0)
float Roll_out_max;   // limit Roll PID output to limit to PWM max
float RollStartAng;   // initial target angle for balancing Roll

// Declare and initialise global variables
long AcAvgDiv;                  // dividing factor used in averaging
int16_t AccDif;                 // difference between subsequent readings of AcY
bool AccOffEn;                  // if == false then display RAW Acc values
int16_t AccRampX;               // max allowable rate of change of AcX
int16_t AccRampY;               // max allowable rate of change of AcY
int16_t AccRampZ;               // max allowable rate of change of AcZ
int16_t A_Cnt;                  // counter used Test mode
int16_t AcX;                    // MPU raw X-acceleration value
long AcXAvg;                    // averaged AcX value to reduce noise
long AcXAvgL;                   // long averaged AcX value to reduce noise
int16_t AcXL;                   // previous MPU X-acceleration value
long AcXSum;                    // averaging accumulator
long AcXSumL;                   // long averaging accumulator
int16_t AcY;                    // MPU raw Y-acceleration value
long AcYAvg;                    // averaged AcY value to reduce noise
long AcYAvgL;                   // long averaged AcY value to reduce noise
int16_t AcYDwnCnt;              // counter used in orientation auto-mode switching
int16_t AcYL;                   // previous MPU Y-acceleration value
long AcYSum;                    // averaging accumulator
long AcYSumL;                   // long averaging accumulator
int16_t AcYUpCnt;               // counter used in orientation auto-mode switching
int16_t AcZ;                    // MPU raw Z-acceleration value
long AcZAvg;                    // averaged AcZ value to reduce noise
long AcZAvgL;                   // longaveraged AcZ value to reduce noise
int16_t AcZL;                   // previous MPU Z-acceleration value
long AcZSum;                    // averaging accumulator
long AcZSumL;                   // long averaging accumulator
String Any$;                    // temporary string
float AnyFloat;                 // any temp float value
long AnyLong;                   // any temp long value
int16_t AutoOFF;                // auto-power OFF timer
long AvCnt;                     // Av. filter width
int32_t BatAvg;                 // preload averaging function with high value
int16_t BatAvgMax;              // maximum average value in this period
byte BatCnt;                    // pointer to array values and critical timer
int16_t BatCritCnt;             // timer used in baterycritical testing
int16_t BatData[60];            // battery averages at 1 second intervals
int16_t BatDel;                 // set a 1 second delay in 'life' prediction
int16_t BatDP;                  // battery data pointer, 0 - 59
int16_t BatLast;                // previous battery average reading
int16_t BatPc;                  // battery percentage of charge
int16_t BatQ;                   // a measure of battery quality 1 - 5
int32_t BatSum;                 // preload averaging sum with 20x max value
unsigned long BatT0;            // time of first battery measurement in life prediction forecast
String BatTime$;                // time left displayed
unsigned long BatTms;           // time when battery reading was taken
int16_t BatV;                   // battery voltage * 10, as in 10.0v == 100
int32_t BatV0;                  // reference voltage used in prediction forecast
int32_t BatV1;                  // lowest voltage used in prediction forecast
float BatVfp;                   // battery voltage as a decimal number. ie. 11.2v
float BatVfpIni;                // battery voltage used to initialise SimpleFOC
int32_t BatVol;                 // instantaneous battery voltage
int16_t B_Cnt;                  // counter used Test mode
float Brake;                    // > 0.0 if brake function is applied
float Brake_Force;              // brake factor to be applied
float Brake_Vel;                // motorC_Vel beyond which brake force is applied
uint8_t Bright;                 // FastLED brightness
uint8_t Brightness;             // Display brightness
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
int32_t cmdVal;                 // value associated with a cmdType
byte Col_B,Col_G,Col_R;         // current LED colours
bool ComTest;                   // == true if Commutate Test is active
int16_t ComType;                // type of commutate test, 0 = saw, 1 = start/stop
bool Cooler;                    // set = true to run motor cool test
int16_t CZ;                     // received button values
bool CZ_Wait;                   // flag used for button release wait function
char Deg = 176;                 // degrees character in Monitor+
bool DEMO;                      // if == true then Demo Mode has been triggered
bool DispBack;                  // flag used to draw a back arrow on the display
unsigned long DispBatWarn;      // timer usesd to prevent battery warning repeating too often
bool DispChng;                  // flag invokes initialisation code
bool DispClr;                   // set == true to clear the memory contents
int16_t DispCnt;                // counter used in display updating
int16_t DispCX,DispCY;          // display cursor coordinates
int16_t DispDel;                // >0 display delay counter, stops updates over I2C
int16_t DispDelGbl;             // global DispDel offset, used to adjust display frame rates
bool DispDisp;                  // set = true to immediately display memory contents
int16_t DispH;                  // display rectangle width
bool DispLock;                  // if == true then don't allow display mode changes
byte DispMenu;                  // group type, 0 = sensors, 1 = tasks, etc
int16_t DispMode;               // the type of display for a given mode
int16_t DispModeCnt;            // counter used in display updating
bool DispMon;                   // == true if connected to OLED Mirror app
int16_t DispNext;               // used to transition to another DispMode
bool DispNOP;                   // set == true to stop display I2C updates
int16_t DispNow;                // if > 0 display special modes
int16_t DispNxtCnt;             // counter used in auto-branching of display
int16_t DispOveride;            // if DispLock == true then use this overide mode
unsigned long DispOverun;       // timer used to prevent the display function being over-run
int16_t DispPnt;                // points at the current display mode
int16_t DispTask;               // task pointer for display functions
int16_t DispTx;                 // counter used with display mirror app
int16_t DispVal;                // general display value, passed from tasks
int16_t DispW;                  // display rectangle width
int16_t DispX,DispY;            // co-ordinates for mirror app mouse click
int16_t DmB,DmX,DmY;            // button and X,Y values associated with a display monitor mouse click
int16_t DM_Brake;               // 'Brake' display assigned Display Mode
int16_t DM_Commutate;           // 'Commutate' display assigned Display Mode
int16_t DM_Control;             // 'Control' display assigned Display Mode
int16_t DM_Filters;             // 'Filters' display assigned Display Mode
int16_t DM_Gears;               // 'Gears' display assigned Display Mode
int16_t DM_Min,DM_Max;          // min/max Display Mode pointer values
int16_t DM_MPU_Acc;             // 'MPU Acc.' display assigned Display Mode
int16_t DM_MPU_AccRamps;        // 'Acc Ram,p.' display assigned Display Mode
int16_t DM_MPU_Gyros;           // 'MPU Gyros' display assigned Display Mode
int16_t DM_PtchPID;             // 'Pitch PID' display assigned Display Mode
int16_t DM_PtchSM;              // 'PitchSafeMode' display assigned Display Mode
int16_t DM_RollPID;             // 'Pitch PID' display assigned Display Mode
int16_t DM_RollSM;              // 'RollSafeMode' display assigned Display Mode
int16_t DM_TurnCtrl;            // 'Turn Control' display assigned Display Mode
int16_t DM_Velocity;            // 'Velocity' display assigned Display Mode
int16_t ESP_NOW_BA;             // broadcast address pointer, -1 to start with
bool ESP_NOW_Init;              // == true once ESP-NOW has been initiaiised
unsigned long FOCms;            // FOC 1ms timer
int16_t Gear;                   // speed control flag
bool G_once;                    // set == true to force initial graphing mode, default = false
int16_t GPmax,GPmin;            // start and end of graphing list, for Scope mode
int16_t GPnn;                   // pointer to current graphing items
bool GP_Title;                  // if == true then report a title in Scope graphing mode
long GyAvgDiv;                  // dividing factor used in averaging
int GyCal;                      // counter used to determine gyro calibration period
bool GyrOffEn;                  // if == false then display RAW Gyro values
unsigned long GyT;              // time of gyro readings in milliseconds
unsigned long GyTD;             // time between successive gyro readings in milliseconds
int16_t GyX;                    // MPU raw X-gyroscope value
long GyXAvg;                    // averaged GyX value to reduce noise
long GyXAvgL;                   // long averaged GyX value to reduce noise
int16_t GyXL;                   // previous MPU X-gyroscope value
int16_t GyXOff;                 // offset to be added to raw X-gyroscope value
int16_t GyXOffCnt;              // offset counter for gyro centering
long GyXSum;                    // averaging accumulator
long GyXSumL;                   // long averaging accumulator
int16_t GyY;                    // MPU raw Y-gyroscope value
long GyYAvg;                    // averaged GyY value to reduce noise
long GyYAvgL;                   // long averaged GyY value to reduce noise
int16_t GyYL;                   // previous MPU Y-gyroscope value
int16_t GyYOff;                 // offset to be added to raw Y-gyroscope value
int16_t GyYOffCnt;              // offset counter for gyro centering
long GyYSum;                    // averaging accumulator
long GyYSumL;                   // long averaging accumulator
int16_t GyZ;                    // MPU raw Z-gyroscope value
long GyZAvg;                    // averaged GyZ value to reduce noise
long GyZAvgL;                   // long averaged GyZ value to reduce noise
int16_t GyZL;                   // previous MPU Z-gyroscope value
int16_t GyZOff;                 // offset to be added to raw Z-gyroscope value
int16_t GyZOffCnt;              // offset counter for gyro centering
long GyZSum;                    // averaging accumulator
long GyZSumL;                   // long averaging accumulator
byte I2CErr;                    // flag returned from I2C comms
bool I2C_MPU;                   // true if MPU6050 is detected during POST
unsigned long incMs;            // ms increment used in velocity control
bool JoyActive;                 // flag used to prevent stack overflow during moves
int16_t JoyD;                   // direction value 0 - 8
int16_t JoyDL;                  // direction 1 counter
int16_t JoyLX;                  // Wii Classic left joystick demand
int16_t JoyLY;                  // Wii Classic right joystick demand
int16_t JoyMode;                // direction of travel flag, 0 = stationary, 1 = forward, -1 = backward
int16_t JoySpd;                 // speed vector = max(JoyX,JoyY)
int16_t JoyV;                   // joystick vector  = SQRT(SQ(JX) + SQ(JY))
int16_t JoyX;                   // received value
int JoyXV;                      // converted value 0 - 127
int16_t JoyXC;                  // received value offset
int16_t JoyY;                   // received value
int16_t JoyYC;                  // received value offset
int JoyYi;                      // tracking filter for received JoyY value
int JoyYV;                      // converted value 0 - 128
boolean JoyYT;                  // tracking filter flag; true if not on track
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
unsigned long LED20ms;          // used to generate 20ms LED tasks in Core 1
bool LED40ms;                   // a 40ms timer derived from the LED20ms event
int16_t LED_Cnt;                // general counter used in LED tasks
int16_t LED_Del;                // LED Task delay, adds pauses into the processes
int16_t LED_Flash;              // if > 0 then flash both LEDs bright white for n counts
bool LED_Flg;                   // LED flag used in simple flashing mode
int16_t LED_Inc;                // LED Task increment variable
bool LEDshow;                   // if true then update LED strip
int16_t LED_SubTsk;             // LED sub-task pointer
int16_t LED_Task;               // LED task pointer
float LpfA;                     // motor A lowpass filter co-efficient 
float LpfB;                     // motor B lowpass filter co-efficient 
float LpfC;                     // motor C lowpass filter co-efficient 
int16_t MainMode;               // determined after RESET, based on orientation
float maxTgtSpd;                // max speed for forward/reverse drive
float maxTgtSpd1;               // Gear 1 max speed for forward/reverse drive
float maxTgtSpd2;               // Gear 2 max speed for forward/reverse drive
float maxTgtSpd3;               // Gear 3 max speed for forward/reverse drive
int16_t Motor_Freq;             // motor.move() call frequency in Hz
int16_t motorA_Ph;              // motor A test phase flag
float motorATgt;                // motorA velocity target used when changing direction
float motorA_Vd;                // motorA change in velocity
float motorA_VdMax;             // maximum rate applied to motor A
float motorA_Vel;               // motorA velocity
float motorAVol;                // motorA voltage limit
int16_t motorB_Ph;              // motor B test phase flag
float motorBTgt;                // motorB velocity target used when changing direction
float motorB_Vd;                // motorB change in velocity (accelerating force)
float motorB_Vel;               // motorB velocity
float motorBVol;                // motorB voltage limit
int16_t motorC_Ph;              // motor C test phase flag
float motorC_Spd;               // motorC speed factor, based on Nunchuk demand
float motorCTgt;                // motorC velocity target used when changing direction
float motorC_Vd;                // motorC change in velocity (accelerating force)
float motorC_Vel;               // motorC velocity
float motorCVol;                // motorC voltage limit
bool motorInit;                 // == true if SimpleFOC has been initialised for each motor
int16_t motorPnt;               // motor pointer in velocity mode, 0=A, 1=B, 2=C
int16_t moveDir;                // move direction flag. 0 == stationary
bool MPUCal;                    // == true during calibration
String myMAC;                   // MAC address if this device
unsigned long next10ms;         // 10ms (100Hz) loop timer
unsigned long next20ms;         // 20ms (50Hz) loop timer
unsigned long next40ms;         // 40ms (25Hz) loop timer
unsigned long nextMs;           // n-ms variable loop timer
bool OpA_En;                    // == true if motor FOC is being applied, deafult = false
bool OpB_En;                    // == true if motor FOC is being applied, deafult = false
bool OpC_En;                    // == true if motor FOC is being applied, deafult = false
int16_t OvrSpd;                 // counter to generate ov erspeed warning
unsigned long PIDloop;          // the loop period of the PID code
int16_t PIDtask;                // task pointer to PID task sequence, 0 = OFF
unsigned long PIDus;            // PID loop timer in microseconds
int16_t Ping;                   // Rx 'ping' counter
float PtchAccAng;               // Pitch angle measured by the Y accelerometer
float Ptch_D;                   // Pitch PID D-gain component
bool Ptch_d_En;                 // Pitch PID d-gain enable true=ON, false=OFF
bool Ptchen;                    // if == true then Pitch PID mode is enabled
float PtchErr;                  // current Pitch PID error
bool PtchGo;                    // == true when Pitch axis is within starting angle
float PtchGyrAng;               // angle measured by Z gyroscope and corected, as true angle
float PtchLastErr;              // previous Pitch PID error, used in D calculations
bool Ptch_i_En;                 // Pitch PID i-gain enable true=ON, false=OFF
float Ptch_i_mem;               // used in Pitch PID integral calcs
float PtchLastDErr;             // used in Pitch PID derivative calcs
float Ptch_Op;                  // Pitch PID output
float Ptch_P;                   // Pitch PID p-gain component
float PtchPwr;                  // factor used to constrain the Pitch gains initially
int16_t PtchSafeCnt;            // counter used in Pitch SafeMode code
int16_t PtchSafeDel;            // Pitch SafeMode skip delay, default = 0
int16_t PtchSafeMode;           // controls the state of the control system
bool PtchSb;                    // if == true then perform self-balance setpoint adjustments (true)
float PtchSp;                   // primary Pitch PID setpoint
int16_t PtchStepT;              // Pitch impulse step test duration in x5ms
float PtchStepV;                // Pitch impulse step test amplitude in degrees
unsigned long print40ms;        // 40ms task pointer - variable
int16_t PrintTgt;               // 0 == serial port, 1 == WiFi ESP-NOW
String PrintTx = "";            // printed strings
bool PwrPlus;                   // flag used to temporarily boost pitch PWM power
bool readWiiCall;               // flag prevents call stack overflow
float ReadyAng;                 // angle at which safemode goes into ready state
long Report20ms;                // timer used in serial port reporting, for debug
int16_t RMSang;                 // AS5600 scaled angle read from regist 0x0C
int16_t RMSraw;                 // AS5600 raw angle read from regist 0x0C
uint8_t RMS_Stat;               // AS5600 stastus register value
float RollAccAng;               // Roll angle measured by the Z accelerometer
float RollBrake;                // braking effect applied to the reaction wheel
float Roll_D;                   // Roll PID D-gain component
bool Roll_d_En;                 // Roll PID d-gain enable true=ON, false=OFF
bool Rollen;                    // if == true then Roll PID mode is enabled
float RollErr;                  // current Roll PID error
bool RollGo;                    // == true when Roll axis is within starting angle
float RollGyrAng;               // angle measured by Z gyroscope and corected, as true angle
float RollLastErr;              // previous Roll PID error, used in D calculations
bool Roll_i_En;                 // Roll PID i-gain enable true=ON, false=OFF
float Roll_i_mem;               // used in Roll PID integral calcs
float RollLastDErr;             // used in Roll PID derivative calcs
float Roll_Op;                  // Roll PID output
float Roll_P;                   // Roll PID p-gain component
float RollPwr;                  // factor used to constrain the Roll gains initially
int16_t RollSafeCnt;            // counter used in SafeMode code
int16_t RollSafeDel;            // Roll SafeMode skip delay, default = 0
int16_t RollSafeMode;           // controls the state of the control system
bool RollSb;                    // if == true then perform self-balance setpoint adjustments (true)
float RollSp;                   // primary Roll PID setpoint
unsigned long runSpace;         // spare time at the end of a PID loop, checks for over-run, default > 0
unsigned long runSpaceRec;      // recorded runSpace time
int16_t Rx_Chk;                 // Rx checksum
bool RxClass;                   // set == true when a Wii Classic command has been receieved
bool RxNunch;                   // set == true when a Wii Nunchuk command has been receieved
int16_t RxCZ;                   // buffered received CZ 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;                     // set true when a valid frame of data is received
bool RxRecvEvent;             // set == true when a OnDataRecv() call back event has occured
int16_t RxState;                // receiver state machine state
int16_t Rx_Task;                // task pointer user by Rx state machine
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
bool SAFE;                      // if == true then Safe Mode has been triggered
int16_t SbPtchCnt;              // counter used in Pitch auto self-balance point adjustment
float SbPtchSp;                 // Pitch PID self-balance set points
float SbPtchSpInc;              // Pitch self-balance auto-increment
float SbPtchSpIncI;             // initial Pitch self-balance auto-increment
int16_t SbPtchTrend;            // Pitch auto-bal;ance trend factor
int16_t SbRollCnt;              // counter used in Roll auto self-balance point adjustment
float SbRollSp;                 // Roll PID self-balance set points
float SbRollSpInc;              // Roll self-balance auto-increment
float SbRollSpIncI;             // initial Roll self-balance auto-increment
bool SerialRx;                  // == true if any character has been received
unsigned long t0,t1;            // debug timers
unsigned long t2,t3;            // debug timers
int16_t Task10ms;               // 10ms task pointer
int16_t Task20ms;               // 20ms task pointer
int16_t Task40ms;               // 40ms task pointer
int16_t TaskMs;                 // n-ms task pointer
bool TEST = false;              // set == true during assembly, then false for normal use
bool Test5ms;                   // set = true if fast 'Test' loop is required
int16_t TestDel;                // TestMode skip delay, default = 0
int16_t TestMode;               // controls the state of the control system
int16_t TestTask;               // task pointer used in Test mode
float TgtAng;                   // target setpoint angle for forward/reverse drive
float TgtAng1;                  // Gear 1 target setpoint angle for forward/reverse drive
float TgtAng2;                  // Gear 2 target setpoint angle for forward/reverse drive
float TgtAng3;                  // Gear 3 target setpoint angle for forward/reverse drive
bool TgtSet;                    // pitch setpoint target achieved flag
float toppleAng;                // angle at which Pitch/Roll are toppling over
int16_t Turn;                   // != 0 when Uni-Bot is turning left/right
bool Turning;                   // == true when Yaw gyro detects turning
float TurnMaxVd;                // max acceleration used in turning
float TurnMinVd;                // min acceleration used in turning
int16_t TurnTask;               // task pointer used in steering
int16_t Tx_PingCnt;             // 1 second Tx ping timer
bool USB;                       // == true if initial battery is below 5.5v
bool VelTest;                   // == true if Velocity Test is active
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
bool WiFiRxRec;                 // true when a block of data has been successfully received
byte WiFiRxMacAddr[6];        // senders MAC address, which could be broadcast FF:FF:FF:FF:FF:FF
int16_t WiFiTryCnt;             // WiFi try to connect count on error count
int16_t WiFiTryNum;             // WiFi try to connect total count
bool WiFiTryOnce;               // == true if first pass of trying to connect
bool WiFiTx;                    // true when a block of data has been sent successfully
int16_t WiFiTx_CB;              // == 1 call back is clear to send, == -1 resend
int16_t WiFiTx_Chk;             // Tx checksum
int16_t WiFiTx_len;             // >0 when WiFi Tx buffer contains data to be sent
long WiFiTxBytes;               // number of bytes received over the WiFi link
int16_t WiFiTxErr;              // WiFi data error
int16_t WiFiTxErrCnt;           // WiFi Tx error count, leads to disconnection
byte WiFiTxMacAddr[6];        // transmit MAC address, which should match myMAC address
uint8_t WiiBYAX;                // Wii Classic right-hand XYAB buttons
uint8_t WiiDigi;                // Wii Classic digital pad bits
uint8_t WiiSHS;                 // Wii Classic centre select,home,start bits
int16_t WiiType;                // device type, 0 == Nunchuk, 1 == Classic
float YawAccAng;                // Yaw angle measured by the X accelerometer
float YawGyrAng;                // angle measured by X gyroscope and corected, as true angle
float YawGyrAngLast;            // previous YawGyrAng
int16_t YawTimeout;             // timeout used in Yaw angle display
byte Z_Dn;                      // button pressed state; 0 == UP, >= 1 == Down
bool Z_Mode;                    // joystick motion flag, normally false, if == true then slide mode

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

void setup() {
  // Setup code, runs once:
  
  // initialise pins
  pinMode(BatPin,INPUT);                                    // ADC1_0  GPIO36 battery divider
  pinMode(MotorAEn,OUTPUT); digitalWrite(MotorAEn,LOW);     // motorA PWM is OFF
  pinMode(MotorBEn,OUTPUT); digitalWrite(MotorBEn,LOW);     // motorB PWM is OFF
  pinMode(MotorCEn,OUTPUT); digitalWrite(MotorCEn,LOW);     // motorC PWM is OFF

  // take initial battery reading, to set ESP32 mux
  BatVol = analogRead(BatPin);

  resetDMs();               // reset display mode references
  setPIDgains();            // set the default PID gains
  setDefaultsA();           // set initial conditions for primary variables
  setDefaultsB();           // set initial conditions for variables
  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 2 LEDs = 24 x 2 = 48 bits, taking 58us to show at 800kbps
  FastLED.addLeds<WS2812B, LED_Pin, GRB>(LED, NumLEDs);
  FastLED.setBrightness(Bright);

  // initilaise RGB LEDs to green
  LED[0].setRGB(0,20,0); LED[1].setRGB(0,20,0);
  FastLED.show(); // This sends the updated pixel color to the hardware.
  Serial.println(F("RGB LED green."));
    
  // 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));

  // initialise the ESP-NOW link for the first time
  Init_ESP_NOW();

  // define the Core 0 task parameters
  // Core 0 is normally exclusively used for WiFi, but here we use it to control
  // RGB 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


  // initialise I/O
  runPOST();

  synchLoopTimers();
}

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

  //###############################################################################
  //  Soft RESET
  //###############################################################################
  // 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() {
  // the main loop runs common tasks and branches to either Demo or Test modes
  readSerial();                 // check serial port on every loop cycle

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

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

  //###############################################################################
  //  MainMode branch, Demo or Test
  //###############################################################################
  // branch to mode specific tasks
  if (MainMode == 0) {
    loopDemo();
    // if (PIDtask > 0) {if (PIDtask < 7) {return;}}  // give max priority to PID functions
  } else {
    loopTest();
  }

  //###############################################################################
  // 20ms tasks
  //###############################################################################
  if (Task20ms > 0) {
    // tasks are performed in reverse order as the pointer is counting down
    switch(Task20ms) {
        case 1: // if connected to WiFi read Wii data and respond
          if (WiFiConnected) {readWii();} break;
    } Task20ms--; // decrement task pointer towards 0
  } else if ((millis() - next20ms) >= 20) {
    next20ms = millis(); Task20ms = 1; Report20ms++;

    // check for auto=power off
    if (AutoOFF > 0) {
      AutoOFF--; if (AutoOFF < 1) {
        motorA_Vel = 0.0; OpA_En = false; digitalWrite(MotorAEn,LOW); motorAVol = 0.0; motorA.voltage_limit = motorAVol;
        motorB_Vel = 0.0; OpB_En = false; digitalWrite(MotorBEn,LOW); motorBVol = 0.0; motorB.voltage_limit = motorBVol;
        motorC_Vel = 0.0; OpC_En = false; digitalWrite(MotorCEn,LOW); motorCVol = 0.0; motorC.voltage_limit = motorCVol;
      }
    }
    // check for being placed face down
    if (AcY < -7000) {
      // the MPU6050 Y-axis accelerometer ia looking at the ground
      // so assume Uni-Bot is resting on its front reaction wheel
      // after 200 ms go into Demo Mode if not on USB power
      if (AcYDwnCnt < 10) {AcYDwnCnt++;} else {gotoDemoMode();}
    } else {AcYDwnCnt = 0; DEMO = false;}

    // check for being pointing upwards
    if (AcY > +7000) {
      // the MPU6050 Y-axis accelerometer ia looking at the sky
      // after 200 ms go into Safe Mode if not on USB power
      if (AcYUpCnt < 10) {AcYUpCnt++;} else {gotoSafeMode();}
    } else {AcYUpCnt = 0; SAFE = false;}
  } else {

    //#############################################################################
    // 40ms tasks
    //#############################################################################
    // these 40ms tasks run after the 20ms tasks are comnpleted
    if (Task40ms > 0) {
      // tasks are performed in reverse order as the pointer is counting down
      switch(Task40ms) {
        case 2: battRead(); break;
        case 3: if (Ping > 0) {Ping--;} break;      // time-out app data received flag
        case 4: // if not connected to WiFi run then try to connect every second
          if (!WiFiConnected) {
            // only try to connect WiFi if no 'Pings' are being received
            if (Ping == 0) {
              WiFiConCnt--;
              if (WiFiConCnt < 1) {
                WiFiConCnt = 10; WiFiTryToConnect();  // 400 ms retry time-out
                if (WiFiTryOnce) {Serial.println("Trying to connect: " + getBAhex());}
  //              Serial.println("Trying to connect WiFi");
              }
            }
          } break;
        case 5:
          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
    } else if ((millis() - next40ms) >= 40) {
      next40ms = millis(); Task40ms = 5;
      // if DispMon is active we run a time-out on the link
      if (DispTx > 0) {DispTx--; if (DispTx == 0) {DispMon = false;}}
    }
  }

  //###############################################################################
  //  40 ms Print Task - Variable
  //###############################################################################
  // this code uses either the serial port or the WiFi link to send messages
  // at 115200 baud we can send 11.5 chars/ms, so 64 chars takes 5.5ms.
  if ((millis() - print40ms)  >= 40) {
    print40ms = millis();
    if (PrintTx.length() > 0) {
      // characters in string buffer so send some/all of them
      print40ms = millis() - 34;  // shorten the delay to 6ms 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 loopDemo() {
  // Main self-balancing Demo loop code, runs repeatedly when in this mode.
  //#############################################################################
  // motor.move() tasks
  //#############################################################################
  // in this mode we update motor.move() every 1ms (1kHz)
  // if ((millis() - nextMs) >= 2) {
  if (TaskMs > 0) {
    switch (TaskMs) {
      case 3: if (OpA_En) {t0 = micros(); motorA.move(motorA_Vel); t1 = micros() - t0;} break;
      case 2: if (OpB_En) {motorB.move(motorB_Vel);} break;
      case 1: if (OpC_En) {motorC.move(motorC_Vel);} break;
    } TaskMs--;
  // } else if ((millis() - nextMs) >= 2) {
  } else if (millis() > nextMs) { // do these tasks every 1 ms
    nextMs = millis(); TaskMs = 3;
    // if (Report20ms >= 5) {Report20ms = 0; if (OpB_En) {PrintTx+= "T\n";} else {PrintTx+= "F\n";}} // report this every 100ms
  }

  //#############################################################################
  // PID tasks
  //#############################################################################
  // PID tasks cycle continuously at 200Hz, once initiated by SafeMode(s)
  // their aim is to self-balance the robot. Steering is handled separately
  if (PIDtask > 0) {
    switch(PIDtask) {
      case 1: PID_readAcc(); break;
      case 2: PID_readGyr(); break;
      case 3: CalcAng(); break;
      case 4: PID_Ptch(); break;
      case 5: PID_Roll(); break;
      case 6: Control(); break;
      case 7: break;
      default:
        // to start the PID loop function, set PIDtask to a value > 7
        PIDus = micros(); PIDtask = 0; runSpace = 0;
        break;
    }
    // sequence and loop at 200Hz (5ms)
    if (PIDtask < 7) {PIDtask++; return;}
    else if ((micros() - PIDus) >= PIDloop) {
      // it's time to start another PID loop
      PIDus = micros(); PIDtask = 1;
     // runSpace should have recorded the free time space in the PID loop code
      // which occurs when all PIDtasks have been done.
      // Display_PIDoverun();  // uncomment this line to test the display
      if (runSpace == 0) {Display_PIDoverun();}
      else {runSpaceRec = runSpace;}  // record the slack space in us
      runSpace = 0;
    } else {
      // record runSpace spare time to check for over-run
      if (runSpace == 0) {runSpace = PIDloop - (micros() - PIDus);}
    }
  }

  //#############################################################################
  // 10ms tasks
  //#############################################################################
  // the following tasks are run in sequence, as a cascade. If not self-balancing
  // PIDtask == 0, then all tasks are performed, but when PIDtasks are active
  // then only the Safe Mode checks are performed.
  if (Task10ms > 0) {
    switch(Task10ms) {
      case 1: Pitch_Safe_Mode(); break;
      case 2: Roll_Safe_Mode(); break;
      // the following tasks only run when the PID controller(s) are inactive
      case 3: CalcAng(); break;
      case 4: readAccelAll(); break;
      case 5: readGyrosAll(); break;
    }
    Task10ms--;
  } else if ((millis() - next10ms) >= 10) {
    // reset task timer
    next10ms = millis();
    if (PIDtask > 0) {Task10ms = 2;} else {Task10ms = 5;}
   }
}

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

void loopTest() {
  // Main Test loop code, runs repeatedly when in this more.
  //#############################################################################
  // n-ms duplicate tasks
  //#############################################################################
  // triggered by the 10ms task setting a TaskMs flag
  // each motor.move() function takes 42us at update frequencies of >= 300Hz
  // update frequencies below 200Hz cause significant vibrations, affecting MPU
  // measurements.
  if (TaskMs > 0) {
    if (millis() >= nextMs) {
      TaskMs--;
      if (TaskMs > 0) {nextMs = millis() + incMs;} else {nextMs = 0;}  // clear/set the flag
      if (OpA_En) {t0 = micros(); motorA.move(motorA_Vel); t1 = micros() - t0;}
      if (OpB_En) {motorB.move(motorB_Vel);}
      if (OpC_En) {motorC.move(motorC_Vel);}
    }
  } else if (TaskMs < 0) {
    // run at max motor.move() update rate of 1kHz
    if (TaskMs > -4) {
      t0 = micros();
      switch(TaskMs) {
        case -1: if (OpA_En) {motorA.move(motorA_Vel);} break;
        case -2: if (OpB_En) {motorB.move(motorB_Vel);} break;
        case -3: if (OpC_En) {motorC.move(motorC_Vel);} break;
      } t1 = micros() - t0;
      TaskMs--;
    } else if (millis() > nextMs) {
      nextMs = millis(); TaskMs = -1;
    }
  }

  //#############################################################################
  // 5ms Test_Mode task
  //#############################################################################
  if (Test5ms) {
    // in some Test modes we need to run at 5ms intervals, so these modes set
    if ((millis() - nextMs) >= 5) {Test5ms = false; Test_Mode();}  // Test mode tasks
  }

  //#############################################################################
  // 10ms tasks
  //#############################################################################
  if (Task10ms > 0) {
    // tasks are performed in reverse order as the pointer is counting down
    switch(Task10ms) {
      case 1: if (I2C_MPU) {Control();} break;       // turn controller
      case 2: if (I2C_MPU) {CalcAng();} break;        // vertical angle calc
      case 3: if (I2C_MPU) {AvgGyrAll();} break;      // average sensor Gyro values
      case 4: if (I2C_MPU) {AvgAccelAll();} break;    // average sensor Accel values
      case 5: if (I2C_MPU) {readAccelAll();} break;   // read MPU6050 accelerometers
      case 6: if (I2C_MPU) {readGyrosAll();} break;   // read MPU6050 gyroscopes
    }
    Task10ms--; // decrement task pointer towards 0
  } else if ((millis() - next10ms) >= 10) {
    // reset task timer and task pointer
    next10ms = millis(); Task10ms = 6;
    // if (Rollen) {PID(); Task10ms = 0;}             // PID controller enabled?
    Test_Mode();                                      // Test mode tasks
    if (ComTest) {nextMs = millis(); Test5ms = true;} // set flag for faster test cycle

    if ((OpA_En) || (OpB_En) || (OpC_En)) {
      if (Motor_Freq > 100) {
        // increase calls to motor.move()
        switch(Motor_Freq) {
          case  200: nextMs = millis() + 5; TaskMs = 1; break;
          case  333: incMs = 3; nextMs = millis() + 3; TaskMs = 2; break;
          case  400: incMs = 2; nextMs = millis() + 2; TaskMs = 3; break;
          case  500: incMs = 2; nextMs = millis() + 2; TaskMs = 4; break;
          case  999: if (TaskMs > -1) {TaskMs = -1; nextMs = millis();} break; // we only need to set this once
        }
      }
    }
  }
}

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

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

    //###############################################################################
    //  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 LED strip
    } else {
      // check 20ms timer
      if (millis() >= LED20ms) {
        LED20ms = millis() + 20;
        LED_Tasks();          // LED tasks are called every 20ms
        LED40ms = !LED40ms;   // toggle the 40ms flag
        if (LED40ms) {
          // this will run every 2 x 20ms = 40ms
          if (DispMon) {Display_40ms();}
        }
      }
    }

    // return to RTOS system with a 1ms call back
    vTaskDelay(1/portTICK_PERIOD_MS);
  }
}

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

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;}

  RxGetChecksum(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, 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! to " + getBAhex());
      WiFiConnected = true;
      WiFiPing = 50;    // set link time-out to 2 seconds, Wii Transceiver will keep setting this with Wii data
      WiFiTxErrCnt = 0; // reset error count for this connection
    } else {
      // Serial.println("Status = " + String(status));
      WiFiTxErrCnt++;
    }
    WiFiTx_CB = 1;
  }
}

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

void resetDMs() {
  // call at the outset or whenever changing modes, as display lists are
  // different for Demo and Test modes
  DM_Brake = -9;            // 'Brake' display assigned Display Mode
  DM_Commutate = -9;        // 'Commutate' display assigned Display Mode
  DM_Control = -9;          // 'Control' display assigned Display Mode
  DM_Filters = -9;          // 'Filters' display assigned Display Mode
  DM_Gears = -9;            // 'Gears' display assigned Display Mode
  DM_Min = -9;              // min Display Mode pointer values
  DM_Max = -9;              // max Display Mode pointer values
  DM_MPU_Acc = -9;          // 'MPU Acc.' display assigned Display Mode
  DM_MPU_Gyros = -9;        // 'MPU Gyros' display assigned Display Mode
  DM_PtchPID = -9;          // 'Pitch PID' display assigned Display Mode
  DM_PtchSM = -9;           // 'PitchSafeMode' display assigned Display Mode
  DM_RollPID = -9;          // 'Pitch PID' display assigned Display Mode
  DM_RollSM = -9;           // 'RollSafeMode' display assigned Display Mode
  DM_TurnCtrl = -9;         // 'Turn Control' display assigned Display Mode
  DM_Velocity = -9;         // 'Velocity' display assigned Display Mode
}

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

void runPOST() {
  // start-up initialisation processes, mainly testing I2C addresses
  // 0x68 MPU 6050 3-axis motion sensor
  Serial.print("\n\n" + Release + "  " + Date + "\n");
  Serial.println(F("Running POST..."));

  // set up battery reading
  initBatteryRead();
  
  // Test for MPU 6050 3-axis motion sensor and initialise
  Wire.beginTransmission(MPU_address);
  I2CErr = Wire.endTransmission();
  if (I2CErr == 0) {
    Serial.println(F("Initialising MPU6050..."));
    MPU_6050_Initialise();
  }
  if (I2CErr == 0) {I2C_MPU = true; Serial.println(F("MPU6050 Initialised"));}
  else {I2C_MPU = false; Serial.println(F("MPU6050 Not Found!"));}

  // if not on USB power then initialise SimpleFOC drivers
  // only apply a small PWM voltage so as not to jolt the wheels
  if (!USB) {
    set_LED_Task(2);  // set LED task to orange blinks
    motorInitA(1.0);
    motorInitB(1.0);
    motorInitC(1.0);
    motorInit = true;
    delay(500);
  } else {motorInit = false;}
  // now turn all motors off
  OpA_En = false; digitalWrite(MotorAEn,LOW);   // motorA PWM is OFF
  OpB_En = false; digitalWrite(MotorBEn,LOW);   // motorB PWM is OFF
  OpC_En = false; digitalWrite(MotorCEn,LOW);   // motorC PWM is OFF

  // determine MainMode
  // 0 = Demo mode, self-balancing + Wi-Fi
  // 1 = Test mode (default)
  //
  CheckOrientation();

  // POST complete
  set_LED_Task(0);                        // set LED task to blue pulse
  Serial.println(F("POST complete"));
}

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

void setDefaultsA() {
  // load default values, declared here in case of a soft reset
  AcAvgDiv = 0;             // dividing factor used in averaging
  AccOffEn = true;          // if == false then display RAW Acc values
  A_Cnt = 0;                // counter used Test mode
  AccRampX = 0;             // max allowable rate of change of AcX, 0 == Off
  AccRampY = 0;             // max allowable rate of change of AcY, 0 == Off
  AccRampZ = 0;             // max allowable rate of change of AcZ, 0 == Off
  AcX = 0;                  // MPU raw X-acceleration value
  AcXAvg = 0;               // averaged AcX value to reduce noise
  AcXSum = 0;               // averaging accumulator
  AcY = 0;                  // MPU raw Y-acceleration value
  AcYAvg = 0;               // averaged AcX value to reduce noise
  AcYDwnCnt = 0;            // counter used in orientation auto-mode switching
  AcYUpCnt = 0;             // counter used in orientation auto-mode switching
  AcYSum = 0;               // averaging accumulator
  AcZ = 0;                  // MPU raw Z-acceleration value
  AcZAvg = 0;               // averaged AcX value to reduce noise
  AcZSum = 0;               // averaging accumulator
  AutoOFF = 0;              // auto-power OFF timer
  BatAvg = 4095;            // preload averaging function with high value
  BatAvgMax = 0;            // maximum average value in this period
  BatCnt = 0;               // pointer to array values and critical timer
  BatCritCnt = 0;           // timer used in baterycritical testing
  BatDel = 50;              // set a 10 second delay in 'life' prediction
  BatSum = 72320;           // preload averaging sum with 20x max value
  BatT0 = 0;                // time of first battery measurement in life prediction forecast
  BatTime$ = "--:--";       // time left displayed
  BatV0 = 0;                // reference voltage used in prediction forecast
  BatV1 = 0;                // lowest voltage used in prediction forecast
  BatVol = 0;               // instantaneous battery voltage
  BatVfpIni = 0.0;          // battery voltage used to initialise SimpleFOC
  Bright = 255;             // FastLED brightness
  Brightness = 128;         // Display brightness
  C_Cnt = 0;                // counter used in 'C' button detection and Test mode
  C_Dn = 0;                 // counter for 'C' button down period
  cmdMode = ' ';            // command mode
  cmdSgn = 1;               // cmdVal sign, 0 or -1
  cmdType = ' ';            // command mode type
  cmdVal = 0;               // value associated with a cmdType
  CZ = 0;                   // received button values
  CZ_Wait = false;          // flag used for button release wait function
  DEMO = false;             // if == true then Demo Mode has been triggered
  DispBack = false;         // flag used to draw a back arrow on the display
  DispChng = false;         // flag invokes initialisation code
  DispClr = false;          // set == true to clear the memory contents
  DispCnt = 0;              // counter used in display updating
  DispDel = 0;              // >0 display delay counter, stops updates over I2C
  DispDelGbl = 0;           // global DispDel offset, used to adjust display frame rates
  DispDisp = false;         // set = true to immediately display memory contents
  DispLock = false;         // if == true then don't allow display mode changes
  DispMenu = 0;             // group type, 0 = sensors, 1 = tasks, etc
  DispMode = 0;             // the type of display for a given mode
  DispModeCnt = 0;          // counter used in display updating
  DispMon = false;          // == true if connected to OLED Mirror app
  DispNext = 0;             // used to transition to another DispMode
  DispNOP = false;          // set == true to stop display I2C updates
  DispNow = 0;              // if > 0 display special modes
  DispNxtCnt = 0;           // counter used in auto-branching of display
  DispOveride = 0;          // if DispLock == true then use this overide mode
  DispPnt = 0;              // points at the current display mode, default = 0 batteries
  DispTask = 0;             // task pointer for display functions
  DispTx = 0;               // counter used with display mirror app
  DispVal = 0;              // general display value, passed from tasks
  ESP_NOW_BA = -1;          // broadcast address pointer, -1 to start with
  ESP_NOW_Init = false;     // == true once ESP-NOW has been initiaiised
  Gear = 1;                 // speed control flag, default = 0
  G_once = true;            // set == true to force initial graphing mode, default = false
  GPmax = 0;                // end of graphing list, for Scope mode
  GPmin = 0;                // start of graphing list, for Scope mode
  GPnn = 4;                 // pointer to current graphing items, default = 4, Roll PID & motor vel
  GP_Title = true;          // if == true then report a title in Scope graphing mode
  GyAvgDiv = 0;             // dividing factor used in averaging
  GyCal = 250;              // counter used to determine gyro calibration period
  GyrOffEn = true;          // if == false then display RAW Gyro values
  GyX = 0;                  // MPU raw X-gyroscope value
  GyXAvg = 0;               // averaged GyX value to reduce noise
  GyXOff = 0;               // offset to be added to raw X-gyroscope value
  GyXOffCnt = 0;            // offset counter for gyro centering
  GyXSum = 0;               // averaging accumulator
  GyY = 0;                  // MPU raw Y-gyroscope value
  GyYAvg = 0;               // averaged GyY value to reduce noise
  GyYOff = 0;               // offset to be added to raw Y-gyroscope value
  GyYOffCnt = 0;            // offset counter for gyro centering
  GyYSum = 0;               // averaging accumulator
  GyZ = 0;                  // MPU raw Z-gyroscope value
  GyZAvg = 0;               // averaged GyZ value to reduce noise
  GyZOff = 0;               // offset to be added to raw Z-gyroscope value
  GyZOffCnt = 0;            // offset counter for gyro centering
  GyZSum = 0;               // averaging accumulator
  I2CErr = 0;               // flag returned from I2C comms
  JoyActive = true;         // flag used to prevent stack overflow during moves
  JoyDL = 0;                // direction 1 counter
  JoyMode = 0;              // direction of travel flag, 0 = stationary, 1 = forward, -1 = backward
  JoySpd = 0;               // speed vector = max(JoyX,JoyY)
  JoyX = 128;               // received value, default
  JoyXV = 0;                // converted value 0 - 127
  JoyY = 128;               // received value, default
  JoyYi = 0;                // tracking filter for received JoyY value
  JoyYV = 0;                // converted value 0 - 128
  JX = 0;                   // global variable used in turning, with Joystick app
  JY = 0;                   // global variable used in turning, with Joystick app
  LED_Flash = 0;            // if > 0 then flash both LEDs bright white for n counts
  LED_Flg = false;          // LED flag used in simple flashing mode
  LED_Inc = 1;              // LED Task increment variable
  LEDshow = false;          // if true then update LED strip
  MainMode = 1;             // determined after RESET, based on orientation, 0 = Demo, 1 = Test
  Motor_Freq = 100;         // motor.move() call frequency in Hz
  motorA_Vel = 0.0;         // motorA velocity
  motorAVol = 4.0;          // motor voltage limit
  motorB_Vel = 0.0;         // motorB velocity
  motorBVol = 4.0;          // motor voltage limit
  motorC_Vel = 0.0;         // motorC velocity
  motorCVol = 4.0;          // motor voltage limit
  motorPnt = -1;            // motor pointer in velocity mode, 0=A, 1=B, 2=C
  moveDir = 0;              // move direction flag. 0 == stationary
  MPUCal = false;           // == true during calibration
  OpA_En = false;           // == true if motor FOC is being applied, deafult = false
  OpB_En = false;           // == true if motor FOC is being applied, deafult = false
  OpC_En = false;           // == true if motor FOC is being applied, deafult = false
  PIDloop = 5000;           // the loop period of the PID code in microseconds
  PIDtask = 0;              // task pointer to PID task sequence, 0 = OFF
  Ping = 0;                 // 'ping' counter
  PrintTgt = 0;             // 0 == serial port, 1 == WiFi ESP-NOW
  PrintTx = "";             // printed strings
  Ptch_d_En = true;         // PID d-gain enable true=ON, false=OFF
  Ptchen = false;           // if == true then Pitch PID mode is enabled
  PtchGo = false;           // == true when Pitch axis is within starting angle
  Ptch_i_En = true;         // PID i-gain enable true=ON, false=OFF
  PtchSafeCnt = 0;          // counter used in Pitch SafeMode code
  PtchSafeDel = 0;          // Pitch SafeMode skip delay, default = 0
  PtchSafeMode = 0;         // controls the state of the control system
  PtchSb = true;            // if == true then perform self-balance setpoint adjustments (true)
  PtchSp = 0.0;             // primary PID setpoint
  PtchStepT = 0;            // Pitch impulse step test duration in x5ms (default = 0)
  PwrPlus = false;          // flag used to temporarily boost pitch PWM power
  readWiiCall = false;      // flag prevents call stack overflow
  ReadyAng = 15.0;          // angle at which safemodes goes into ready state
  Report20ms = 0;           // timer used in serial port reporting, for debug
  Roll_d_En = true;         // PID d-gain enable true=ON, false=OFF
  Rollen = false;           // if == true then Roll PID mode is enabled
  RollGo = true;            // == true when Roll axis is within starting angle
  Roll_i_En = true;         // PID i-gain enable true=ON, false=OFF
  RollSafeCnt = 0;          // counter used in Roll SafeMode code
  RollSafeDel = 0;          // Roll SafeMode skip delay, default = 0
  RollSafeMode = 0;         // controls the state of the control system
  RollSb = true;            // if == true then perform self-balance setpoint adjustments (true)
  RollSp = 0.0;             // primary PID setpoint
  runSpace = 1;             // spare time at the end of a PID loop, checks for over-run, default > 0
  runSpaceRec = 0;          // recorded runSpace time
  RxClass = false;          // set == true when a Wii Classic command has been receieved
  Rx_Chk = 0;               // Rx checksum
  RxCZ = 0;                 // buffered received CZ value
  RxJoyX = 0;               // buffered received JoyX value
  RxJoyY = 0;               // buffered received JoyY value
  Rx_len = 0;               // length of WiFi received data block
  RxNunch = false;          // set == true when a Wii Nunchuk command has been receieved
  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
  SAFE = false;             // if == true then Safe Mode has been triggered
  SerialRx = false;         // == true if any character has been received
  Task10ms = 0;             // 10ms task pointer
  Task40ms = 0;             // 40ms task pointer
  Test5ms = false;          // set = true if fast 'Test' loop is required
  TaskMs = 0;               // n-ms task pointer
  TestDel = 0;              // TestMode skip delay, default = 0
  TestMode = -1;            // controls the state of the control system
  TestTask = 0;             // task pointer used in Test mode
  TgtSet = false;           // pitch setpoint target achieved flag
  toppleAng = 35.0;         // angle at which Pitch/Roll are toppling over
  Turn = 0;                 // != 0 when Uni-Bot is turning left/right
  TurnTask = 0;             // task pointer used in steering
  Tx_PingCnt = 0;           // 1 second Tx ping timer
  USB = false;              // == true if initial battery is below 5.5v
  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 'C' 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
  WiiBYAX = 0b00001111;     // Wii Classic right-hand XYAB buttons
  WiiDigi = 0b00001111;     // Wii Classic digital pad bits
  WiiSHS  = 0b00000111;     // Wii Classic centre select,home,start bits
  WiiType = 0;              // device type, 0 == Nunchuk, 1 == Classic
  Z_Dn = 0;                 // button pressed state; 0 = UP, 1 = Down
}

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

void setDefaultsB() {
  // load default values, declared here in case of a MainMode change
  B_Cnt = 0;                // counter used in Test mode
  Cooler = false;           // set = true to run motor cool test
  ComTest = false;          // == true if Commutate Test is active
  ComType = 0;              // type of commutate test, 0 = saw, 1 = start/stop
  VelTest = false;          // == true if Velocity Test is active
}

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

void setPIDgains() {
  // called after reset to set the PID gains to their degault values
  AvCnt = 1;              // Av. filter depth-
  Brake_Force = 0.00;    // brake factor to be applied
  Brake_Vel = 1.0;        // motorC_Vel beyond which brake force is applied
  LpfA = 1.0;             // motor A lowpass filter co-efficient, 1.0 == Off 
  LpfB = 1.0;             // motor A lowpass filter co-efficient, 1.0 == Off
  LpfC = 1.0;             // motor C lowpass filter co-efficient, 1.0 == Off
  maxTgtSpd1 = 1.0;       // Gear 1 max speed for forward/reverse drive
  maxTgtSpd2 = 1.5;       // Gear 2 max speed for forward/reverse drive
  maxTgtSpd3 = 2.0;       // Gear 3 max speed for forward/reverse drive
  motorA_VdMax = 0.80;    // maximum rate applied to motor A
  motorC_Spd = 0.0;       // motorC speed factor, based on Nunchuk demand
  Ptch_p_gain = 16.0;     // Gain setting for Pitch P-controller (1.0)
  Ptch_i_gain = 16.0;     // Gain setting for Pitch I-controller (0.0)
  Ptch_i_max = 45.0;      // limit Pitch I-controller output(2.0)
  Ptch_d_gain = 70.0;     // Gain setting for Pitch D-controller (0.0)
  Ptch_out_max = 45.0;    // limit Pitch PID output to limit to PWM max
  PtchStartAng = 0.00;    // initial target angle for balancing Pitch (0.0)
  SbPtchSpIncI = 0.025;   // initial Pitch self-balance auto-increment
  RollBrake = 0.0;        // braking effect applied to the reaction wheel
  Roll_p_gain = 3.00;     // Gain setting for Roll P-controller (3.0)
  Roll_i_gain = 0.07;     // Gain setting for Roll I-controller (0.0)
  Roll_i_max = 10.0;      // limit Roll I-controller output(0.2)
  Roll_d_gain = 12.0;     // Gain setting for Roll D-controller (0.0)
  Roll_out_max = 60.0;    // limit Roll PID output to limit to PWM max
  RollStartAng = -2.10;   // initial target angle for balancing Roll (ideally 0.0)
  SbRollSpIncI = 0.010;   // initial Roll self-balance auto-increment
  TgtAng1 = 0.90;         // Gear 1 target setpoint angle for forward/reverse drive
  TgtAng2 = 0.95;         // Gear 2 target setpoint angle for forward/reverse drive
  TgtAng3 = 1.00;         // Gear 3 target setpoint angle for forward/reverse drive
  Turning = false;        // == true when Yaw gyro detects turning
  TurnMaxVd = 0.250;      // max acceleration used in turning
  TurnMinVd = 0.060;      // min acceleration used in turning
  YawGyrAng = 0.000;      // turn angle
  YawGyrAngLast = 0.000;  // previous turn angle
}

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

void synchLoopTimers() {
  // reset main loop timers
  while (millis() < 40){}     // we must wait until millis() is at least 40 from reset
  DispBatWarn = millis();     // 30 sec timer used to prevent battery warnings being too frequent
  DispOverun = millis();      // timer used to prevent the display function being over-run
  LED20ms = millis() + 20;    // set LED 20ms timer
  LED40ms = false;            // a 40ms timer derived from the LED20ms event
  next10ms = millis() - 3;    // set loop timer
  next20ms = millis() - 6;    // set loop timer
  next40ms = millis() - 9;    // set loop timer
  PIDus = micros();           // set PID loop timer
  print40ms  = millis() - 12; // reset 40ms printer timer with 6ms phase shift
  //  Serial.print("synchLoopTimers()");
}

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

