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

void AvgAccelAll() {
  // average the raw accelerometer values over a given number of values
  AcXSumL += AcX; AcYSumL += AcY; AcZSumL += AcZ;
  if (AcAvgDiv < AcAvgDivMax) {
    // increase the scope of the averaging factor if necessary
    // note at 20ms sample, AvgDiv = 50 == 1 second rolling average
    AcAvgDiv++;
  } else {
    // once the accumulator has the correct number of samples roll out the average
    AcXSumL -= AcXAvgL; AcYSumL -= AcYAvgL; AcZSumL -= AcZAvgL;
  }
  AcXAvgL = AcXSumL / AcAvgDiv;
  AcYAvgL = AcYSumL / AcAvgDiv;
  AcZAvgL = AcZSumL / AcAvgDiv;
}

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

void AvgGyrAll() {
  // called as a 20ms subtask, keeps a rolling average of the gyroscopes
  GyXSumL += GyX; GyYSumL += GyY; GyZSumL += GyZ;
  if (GyAvgDiv < GyAvgDivMax) {
    // increase the scope of the averaging factor if necessary
    // note at 20ms sample, AvgDiv = 50 == 1 second rolling average
    GyAvgDiv++;
  } else {
    // once the accumulator has the correct number of samples roll out the average
    GyXSumL -= GyXAvgL; GyYSumL -= GyYAvgL; GyZSumL -= GyZAvgL;
  }

  GyXAvgL = GyXSumL/GyAvgDiv;
  GyYAvgL = GyYSumL/GyAvgDiv;
  GyZAvgL = GyZSumL/GyAvgDiv;
}

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

void battRead() {
  // read the analogue voltage connected to the battery every 40ms
  // this frunction is no longer called due to the limitations of ADC2 under Wi-Fi
  BatVol = analogRead(BatPin);
  BatSum = BatSum + BatVol - BatAvg;
  BatAvg = BatSum/50;             // ADC is averaged over 50 readings (2 sec) to remove noise

  // force BattAvg high for debug. This line is normally commented out
  // BatAvg = BatHigh;

  if (TEST) {Serial.println("Battery " + String(BatVol) + "\t" + String(float(BatAvg)/BatCal) + "v\t" + String(AnyLong) + "%");}
  if ((BatAvg > BatCritical) && (USB)) {
    // power has returned
    USB = false;
    if (!motorInit) {
      motorInitA(1.0);
      motorInitB(1.0);
      motorInitC(1.0);
      motorInit = true;
      delay(500);
      // 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
      synchLoopTimers();
    }
  } 
  if (USB) {return;}

  BatV = getBatV(BatAvg); BatVfp = (float)BatV/10.0;

  // report WARNING state
  if (BatPc < 20) {Display_BatWarning();}

  // detect critical battery condition
  if (BatAvg <= BatCritical){
    // critical battery voltage reached
    digitalWrite(MotorAEn,LOW);             // motorA PWM is OFF
    digitalWrite(MotorBEn,LOW);             // motorB PWM is OFF
    digitalWrite(MotorCEn,LOW);             // motorC PWM is OFF
    set_LED_Task(-1);                       // turn OFF LEDs
    while (true) {yield();}                 // infinite loop, everything stops in Core 1
  }
}

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

void CalcAng() {
  // calculates the pitch, roll and turn angles of the Uni-Bot
  // based on Pitch: AcY and GyZ, and Roll: AcZ and GyY
  // calculate accelerometer angle
  PtchAccAng = asin((float)AcY/8192.0)* 57.296;
  RollAccAng = asin((float)AcZ/8192.0)* 57.296;
  YawAccAng  = asin((float)AcX/8192.0)* 57.296;

  // calculate gyro angles, which will be used as the true angles
  float zDiv = (float)GyTD/131068000.0;                 // nominally 0.0007629 at 10ms
  PtchGyrAng += (float)GyZ * zDiv;                      // Calculate the angle travelled during this 10ms loop and add this to the angle_gyro variable
  if (PtchGyrAng > 180.0) {PtchGyrAng = 180.0;}         // limit +ve gyro swing
  else if (PtchGyrAng < -180.0) {PtchGyrAng = -180.0;}  // limit -ve gyro swing

  RollGyrAng -= (float)GyY * zDiv;                      // Calculate the angle travelled during this 10ms loop and add this to the angle_gyro variable
  if (RollGyrAng > 180.0) {RollGyrAng = 180.0;}         // limit +ve gyro swing
  else if (RollGyrAng < -180.0) {RollGyrAng = -180.0;}  // limit -ve gyro swing

  float zYawAng = YawGyrAng;                            // note previous value
  YawGyrAng += (float)GyX * zDiv;                       // Calculate the angle travelled during this 10ms loop and add this to the angle_gyro variable
  if (YawGyrAng > 180.0) {YawGyrAng = 180.0;}           // limit +ve gyro swing
  else if (YawGyrAng < -180.0) {YawGyrAng = -180.0;}    // limit -ve gyro swing

  // eliminate drift using a small amount of AccAng
  PtchGyrAng = (PtchGyrAng * 0.99) + (PtchAccAng * 0.01);

  RollGyrAng = (RollGyrAng * 0.99) + (RollAccAng * 0.01);

  // this does not apply to YawGyrAng, as the angle cannot be corrected with gravity

  // check for corrupt angles
  if (isnan(PtchGyrAng)) {PtchGyrAng = PtchAccAng;}
  if (isnan(RollGyrAng)) {RollGyrAng = RollAccAng;}
  if (isnan(YawGyrAng)) {YawGyrAng = zYawAng;}
}

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

void ChangeMainMode() {
  // called from clicking on Control display to change mode
  // values declared in setDefaultsA() need to be preserved when switching
  if (MainMode == 0) {
    // in 'Demo' mode so switch to 'Test' mode
    gotoTestMode(true);
  } else {
    // in 'Test' mode so switch to 'Demo' mode
    gotoDemoMode();
  }
}

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

void CheckOrientation() {
  // called during runPOST() or after soft reset to determine MainMode
  // take 20 readings from the MPU, in 100ms
  for (int16_t zR = 0;zR < 21; zR++) {
    readGyrosAll(); readAccelAll(); AvgAccelAll(); AvgGyrAll();
    // Serial.println("AcY=" + String(AcY));
    delay(5);
  }
  // test robots orientation
  if (AcY < -7000) {
    // the MPU6050 Y-axis accelerometer ia looking at the ground
    // so assume Uni-Bot is resting on its front reaction wheel
    gotoDemoMode();
  } else {
    // the default mode is Test
    gotoTestMode(true);
  }
}

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

void decodeKey(int zkeyVal) {
  // decodes zkeyVal and excutes commands on '.'
  // every time we receive a character set response to 8ms later, just in case
  // there is another packet behind it coming from the controller
  print40ms = millis() + 6;
  if (zkeyVal == 10) {return;}
  if (zkeyVal == 13) {return;}
  keyChar = char(zkeyVal);
  switch (keyChar) {
    case '.': doCmd(); return;                        // command terminator
    case '~': // connected to OLED Mirror app either via USB or WiFi
      if (!DispMon) {DispMon = true; DispNext = 0; Display_Mirrored();} // display mirrored message
      DispTx = 75; return;  // apply a 3 second time-out
    case '-': cmdSgn = -1; return;    // numeric sign
    case '*': cmdMode = ' '; cmdType = ' '; cmdVal = 0; cmdSgn = 1; return; // ESC abort command
    case '0': extendCmdVal(0); return;
    case '1': extendCmdVal(1); return;
    case '2': extendCmdVal(2); return;
    case '3': extendCmdVal(3); return;
    case '4': extendCmdVal(4); return;
    case '5': extendCmdVal(5); return;
    case '6': extendCmdVal(6); return;
    case '7': extendCmdVal(7); return;
    case '8': extendCmdVal(8); return;
    case '9': extendCmdVal(9); return;
  }
  if (cmdMode == ' ') {
    // test for new Command Mode char?
    // only accept certain characters as valid commands, or ignore them
    // delay battery update by 10 seconds whilst receiving commands
    cmdMode = keyChar;
    switch (keyChar) {
      // check for valid mode and convert upper-case
      case 'd': cmdMode = 'D'; break;
      case 'g': cmdMode = 'G'; break;
      case 'i': cmdMode = 'I'; break;
      // case 'j': cmdMode = 'J'; break;
      // case 'm': cmdMode = 'M'; break;
      case 'p': cmdMode = 'P'; break;
      // case 'r': cmdMode = 'R'; break;
      // case 's': cmdMode = 'S'; break;
      // case 't': cmdMode = 'T'; break;
      // case 'y': cmdMode = 'Y'; break;
    } cmdType = ' '; cmdVal = 0;
  } else {
    // test for Command Type char?
    cmdType = keyChar;
    switch (keyChar) {
      // check for lower-case and convert to upper-case
      // case 'b': cmdType = 'B'; break;
      // case 'd': cmdType = 'D'; break;
      case 'e': cmdType = 'E'; break;
      // case 'g': cmdType = 'G'; break;
      // case 'i': cmdType = 'I'; break;
      case 'm': cmdType = 'M'; break;
      case 'p': cmdType = 'P'; break;
      // case 's': cmdType = 'S'; break;
      // case 't': cmdType = 'T'; break;
      // case 'u': cmdType = 'U'; break;
      case 'v': cmdType = 'V'; break;
      // case 'x': cmdType = 'X'; break;
      // case 'y': cmdType = 'Y'; break;
    }
  }
}

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

void displayLft() {
  // change the DispMode pointer to the previous display
  DispMode--; if (DispMode < DM_Min) {DispMode = DM_Max;}
  DispDel = 0; DispCnt = 0;
}

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

void displayRht() {
  // change the DispMode pointer to the next display
  DispMode++; if (DispMode > DM_Max) {DispMode = DM_Min;}
  DispDel = 0; DispCnt = 0;
}

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

void DM_Acc_Ramps() {
  //##################################
  // Acc Ramps display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (DmX < 50) {displayLft();}
  else if ((DmX >= 52) && (DmX <= 61)) {
    if ((DmY >= 33) && (DmY <= 44)) {AccRampInc(0);}
    if ((DmY >= 57) && (DmY <= 68)) {AccRampInc(1);}
    if ((DmY >= 82) && (DmY <= 93)) {AccRampInc(2);}
  } else {displayRht();}
}

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

void DM_Brake_() {
  //##################################
  // Brake display
  //##################################
  // user has clicked on a blue field in Monitor+ display
       if (DmX < 50) {displayLft();}
  else if ((DmX >= 51) && (DmX <= 65) && (DmY >= 36) && (DmY <= 47)) {
    if ((DmX >= 51) && (DmX <= 55)) {BrakeVelInc(1.00);}
    if ((DmX >= 58) && (DmX <= 61)) {BrakeVelInc(0.10);}
    if ((DmX >= 62) && (DmX <= 65)) {BrakeVelInc(0.01);}
  } else if ((DmX >= 51) && (DmX <= 70) && (DmY >= 56) && (DmY <= 67)) {
    if ((DmX >= 51) && (DmX <= 55)) {BrakeFrcInc(1.0);}
    if ((DmX >= 58) && (DmX <= 61)) {BrakeFrcInc(0.100);}
    if ((DmX >= 62) && (DmX <= 65)) {BrakeFrcInc(0.010);}
    if ((DmX >= 66) && (DmX <= 70)) {BrakeFrcInc(0.001);}
  } else {displayRht();}
}

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

void DM_Commutate_() {
  //##################################
  // Commutate display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (!USB) {
    if ((DmX >= 26) && (DmX <= 65) && (DmY >= 5) && (DmY <= 18)) {
      if (ComTest) {
        // already in commutate test so cancel it
        TestDel = 0; TestMode = 14;
      } else {TestDel = 0; TestMode = 10;}
    } else if (DmX < 50) {
      // left side click
      displayLft();
    } else {
      // right side click
      if (ComTest) {
        if ((DmX >= 69) && (DmX <= 72) && (DmY >= 7) && (DmY <= 17)) {
          // change motor pointer
          motorChPnt();
        }
        else if ((DmX < 75) && (DmY > 27) && (DmY < 37)) {
          // user has clicked on comutate numbers field
                if ((DmX >= 54) && (DmX <= 57)) {commInc(1.0);}
          else if ((DmX >= 60) && (DmX <= 63)) {commInc(0.1);}
          else if ((DmX >= 65) && (DmX <= 68)) {commInc(0.01);}
          else if ((DmX >= 69) && (DmX <= 72)) {commInc(0.001);}
        }
        else if ((DmX >= 50) && (DmX <= 67) && (DmY >= 45) && (DmY <= 56)) {motorFreq();}
        else if ((DmX >= 50) && (DmX <= 64) && (DmY >= 63) && (DmY <= 74)) {
          // user has clicked on voltage numbers field
                if ((DmX >= 50) && (DmX <= 53)) {voltageInc(1.0);}
          else if ((DmX >= 56) && (DmX <= 59)) {voltageInc(0.1);}
          else if ((DmX >= 61) && (DmX <= 64)) {voltageInc(0.01);}
        }
        else if ((DmX >= 50) && (DmX <= 81) && (DmY >= 82) && (DmY <= 92)) {commType();}
        else {displayRht();}
      } else {displayRht();}
    }
  } else {
    if (DmX < 50) {displayLft();} else {displayRht();}
  }
}

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

void DM_Control_() {
  //##################################
  // Control display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (DmX <= 50) {displayLft();}
  else {
    // has user clicked on a blue field
          if ((DmX >= 51) && (DmX <= 64) && (DmY >= 27) && (DmY <= 37)) {ChangeMainMode();}
    else if ((DmX >= 51) && (DmX <= 71) && (DmY >= 45) && (DmY <= 56)) {ResetAllNow();}
    else if ((DmX >= 51) && (DmX <= 66) && (DmY >= 63) && (DmY <= 74)) {toggleMotorCool();}
    else if ((DmY >= 82) && (DmY <= 93)) {
      if ((DmX >= 51) && (DmX <= 55)) {AvFtrInc(10);}
      if ((DmX >= 56) && (DmX <= 59)) {AvFtrInc(1);}
    }
    else {displayRht();}
  }
}

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

void DM_Filters_() {
  //##################################
  // Filters display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if ((DmX >= 51) && (DmX <= 65)) {
    if ((DmY >= 32) && (DmY <= 42)) {
      if ((DmX >= 51) && (DmX <= 55)) {FilterAInc(1.00);}
      if ((DmX >= 58) && (DmX <= 61)) {FilterAInc(0.10);}
      if ((DmX >= 62) && (DmX <= 65)) {FilterAInc(0.01);}
    } else if ((DmY >= 52) && (DmY <= 62)) {
      if ((DmX >= 51) && (DmX <= 55)) {FilterBInc(1.00);}
      if ((DmX >= 58) && (DmX <= 61)) {FilterBInc(0.10);}
      if ((DmX >= 62) && (DmX <= 65)) {FilterBInc(0.01);}
    } else if ((DmY >= 72) && (DmY <= 82)) {
      if ((DmX >= 51) && (DmX <= 55)) {FilterCInc(1.00);}
      if ((DmX >= 58) && (DmX <= 61)) {FilterCInc(0.10);}
      if ((DmX >= 62) && (DmX <= 65)) {FilterCInc(0.01);}
    }
  } else if (DmX < 50) {displayLft();} else {displayRht();}
}

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

void DM_Gears_() {
  //##################################
  // Gears display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  int zY = 1 + (((DmY - 31) * 3)/51);
  if ((DmX >= 27) && (DmX <= 41) && (DmY >= 22) && (DmY <= 94)) {
    // in angle column
         if ((DmX >= 27) && (DmX <= 30)) {GrsAngInc(zY,1.00);}
    else if ((DmX >= 33) && (DmX <= 36)) {GrsAngInc(zY,0.10);}
    else if ((DmX >= 37) && (DmX <= 41)) {GrsAngInc(zY,0.01);}
  } else if ((DmX >= 76) && (DmX <= 86) && (DmY >= 22) && (DmY <= 94)) {
    // in speed column
        if ((DmX >= 76) && (DmX <= 80)) {GrsSpdInc(zY,1.00);}
    else if ((DmX >= 83) && (DmX <= 86)) {GrsSpdInc(zY,0.10);}
  } else if (DmX < 50) {displayLft();} else {displayRht();}
}

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

void DM_MPU_Acc_() {
  //##################################
  // MPU Acc display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (DmX < 50) {displayLft();}
  else {
    // has user clicked on a blue field
    if ((DmX >= 67) && (DmX <= 72) && (DmY >= 9) && (DmY <= 19)) {AccOffEn = !AccOffEn;}
    else {displayRht();}
  }
}

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

void DM_MPU_Gyr_() {
  //##################################
  // MPU Gyros display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (DmX < 50) {displayLft();}
  else {
    // has user clicked on a blue field
    if ((DmX >= 70) && (DmX <= 74) && (DmY >= 9) && (DmY <= 19)) {GyrOffEn = !GyrOffEn;}
    else {displayRht();}
  }
}

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

void DM_PtchPID_() {
  //##################################
  // Pitch PID display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (DmX < 50) {displayLft();}
  else if ((DmX >= 62) && (DmX <= 76) && (DmY >= 7) && (DmY <= 17)) {
    // user has clicked on SafeMode value
    togglePtchSM();
  } else if (PtchGo && (DmX >= 93) && (DmX <= 96) && (DmY >= 7) && (DmY <= 17)) {
    // user has clicked on the Pitch step 'S' field
    PtchStep();
  } else if ((DmX < 69) && (DmY >= 25) && (DmY <= 36)) {
    // user has clicked on P-gain field
          if ((DmX >= 50) && (DmX <= 53)) {PtchPinc(10.0);}
    else if ((DmX >= 54) && (DmX <= 57)) {PtchPinc(1.0);}
    else if ((DmX >= 61) && (DmX <= 64)) {PtchPinc(0.1);}
    else if ((DmX >= 65) && (DmX <= 68)) {PtchPinc(0.01);}
  } else if ((DmX < 69) && (DmY >= 44) && (DmY <= 54)) {
    // user has clicked on I-gain field
          if ((DmX >= 50) && (DmX <= 53)) {PtchIinc(10.0);}
    else if ((DmX >= 54) && (DmX <= 57)) {PtchIinc(1.0);}
    else if ((DmX >= 61) && (DmX <= 64)) {PtchIinc(0.1);}
    else if ((DmX >= 65) && (DmX <= 68)) {PtchIinc(0.01);}
  } else if ((DmX >= 78) && (DmX <= 96) && (DmY >= 44) && (DmY <= 54)) {
    // user has clicked on I-max field
          if ((DmX >= 78) && (DmX <= 81)) {PtchIMinc(10.0);}
    else if ((DmX >= 82) && (DmX <= 85)) {PtchIMinc(1.0);}
    else if ((DmX >= 88) && (DmX <= 92)) {PtchIMinc(0.1);}
    else if ((DmX >= 93) && (DmX <= 96)) {PtchIMinc(0.01);}
  } else if ((DmX < 69) && (DmY >= 62) && (DmY <= 73)) {
    // user has clicked on D-gain field
          if ((DmX >= 50) && (DmX <= 53)) {PtchDinc(10.0);}
    else if ((DmX >= 54) && (DmX <= 57)) {PtchDinc(1.0);}
    else if ((DmX >= 61) && (DmX <= 64)) {PtchDinc(0.1);}
    else if ((DmX >= 65) && (DmX <= 68)) {PtchDinc(0.01);}
  } else if ((DmX < 69) && (DmY >= 79) && (DmY <= 91)) {
    // user has clicked on StartAng field
         if ((DmX >= 54) && (DmX <= 57)) {PtchSinc(1.0);}
    else if ((DmX >= 61) && (DmX <= 64)) {PtchSinc(0.1);}
    else if ((DmX >= 65) && (DmX <= 68)) {PtchSinc(0.01);}
  } else {displayRht();}
}

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

void DM_PtchSM_() {
  //##################################
  // Pitch SafeMode display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if ((DmX >= 65) && (DmX <= 78) && (DmY >= 7) && (DmY <= 17)) {
    // user has clicked on SafeMode value
    togglePtchSM();
  } else if ((DmX >= 75) && (DmX <= 93) && (DmY >= 44) && (DmY <= 54)) {
    // user has clicked on SbRollSp value field
         if ((DmX >= 75) && (DmX <= 78)) {PtchSbInc(1.000);}
    else if ((DmX >= 82) && (DmX <= 85)) {PtchSbInc(0.100);}
    else if ((DmX >= 86) && (DmX <= 89)) {PtchSbInc(0.010);}
    else if ((DmX >= 90) && (DmX <= 93)) {PtchSbInc(0.001);}
  } else if (DmX < 50) {displayLft();} else {displayRht();}
}

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

void DM_RollPID_() {
  //##################################
  // Roll PID display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (DmX < 50) {displayLft();}
  else if ((DmX >= 60) && (DmX <= 73) && (DmY >= 7) && (DmY <= 17)) {
    // user has clicked on SafeMode value
    toggleRollSM();
  } else if ((DmX < 69) && (DmY >= 25) && (DmY <= 36)) {
    // user has clicked on P-gain field
          if ((DmX >= 50) && (DmX <= 53)) {RollPinc(10.0);}
    else if ((DmX >= 54) && (DmX <= 57)) {RollPinc(1.0);}
    else if ((DmX >= 61) && (DmX <= 64)) {RollPinc(0.1);}
    else if ((DmX >= 65) && (DmX <= 68)) {RollPinc(0.01);}
  } else if ((DmX < 69) && (DmY >= 44) && (DmY <= 54)) {
    // user has clicked on I-gain field
          if ((DmX >= 50) && (DmX <= 53)) {RollIinc(10.0);}
    else if ((DmX >= 54) && (DmX <= 57)) {RollIinc(1.0);}
    else if ((DmX >= 61) && (DmX <= 64)) {RollIinc(0.1);}
    else if ((DmX >= 65) && (DmX <= 68)) {RollIinc(0.01);}
  } else if ((DmX >= 78) && (DmX <= 96) && (DmY >= 44) && (DmY <= 54)) {
    // user has clicked on I-max field
          if ((DmX >= 78) && (DmX <= 81)) {RollIMinc(10.0);}
    else if ((DmX >= 82) && (DmX <= 85)) {RollIMinc(1.0);}
    else if ((DmX >= 88) && (DmX <= 92)) {RollIMinc(0.1);}
    else if ((DmX >= 93) && (DmX <= 96)) {RollIMinc(0.01);}
  } else if ((DmX < 69) && (DmY >= 62) && (DmY <= 73)) {
    // user has clicked on D-gain field
          if ((DmX >= 50) && (DmX <= 53)) {RollDinc(10.0);}
    else if ((DmX >= 54) && (DmX <= 57)) {RollDinc(1.0);}
    else if ((DmX >= 61) && (DmX <= 64)) {RollDinc(0.1);}
    else if ((DmX >= 65) && (DmX <= 68)) {RollDinc(0.01);}
  } else if ((DmX < 69) && (DmY >= 79) && (DmY <= 91)) {
    // user has clicked on StartAng field
         if ((DmX >= 54) && (DmX <= 57)) {RollSinc(1.0);}
    else if ((DmX >= 61) && (DmX <= 64)) {RollSinc(0.1);}
    else if ((DmX >= 65) && (DmX <= 68)) {RollSinc(0.01);}
  } else {displayRht();}
}

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

void DM_RollSM_() {
  //##################################
  // Roll SafeMode display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if ((DmX >= 65) && (DmX <= 78) && (DmY >= 7) && (DmY <= 17)) {
    // user has clicked on SafeMode value
    toggleRollSM();
  }
  else if ((DmX >= 75) && (DmX <= 93) && (DmY >= 44) && (DmY <= 54)) {
    // user has clicked on SbRollSp value field
         if ((DmX >= 75) && (DmX <= 78)) {RollSbInc(1.000);}
    else if ((DmX >= 82) && (DmX <= 85)) {RollSbInc(0.100);}
    else if ((DmX >= 86) && (DmX <= 89)) {RollSbInc(0.010);}
    else if ((DmX >= 90) && (DmX <= 93)) {RollSbInc(0.001);}
  } else if ((DmX >= 80) && (DmX < 94) && (DmY >= 80) && (DmY <= 91)) {
    // user has clicked on Vd limit field
          if (DmX <= 83) {RollVdInc(1.00);}
    else if ((DmX >= 86) && (DmX <= 90)) {RollVdInc(0.10);}
    else if (DmX > 90) {RollVdInc(0.01);}

  } else if (DmX < 50) {displayLft();} else {displayRht();}
}

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

void DM_TurnCtrl_() {
  //##################################
  // Turn Control display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (DmX < 50) {displayLft();}
  else if ((DmX >= 51) && (DmX <= 70) && (DmY >= 48) && (DmY <= 59)) {
         if ((DmX >= 51) && (DmX <= 55)) {TurnMinInc(1.000);}
    else if ((DmX >= 58) && (DmX <= 61)) {TurnMinInc(0.100);}
    else if ((DmX >= 62) && (DmX <= 65)) {TurnMinInc(0.010);}
    else if ((DmX >= 66) && (DmX <= 70)) {TurnMinInc(0.001);}
  } else if ((DmX >= 51) && (DmX <= 70) && (DmY >= 64) && (DmY <= 75)) {
         if ((DmX >= 51) && (DmX <= 55)) {TurnMaxInc(1.000);}
    else if ((DmX >= 58) && (DmX <= 61)) {TurnMaxInc(0.100);}
    else if ((DmX >= 62) && (DmX <= 65)) {TurnMaxInc(0.010);}
    else if ((DmX >= 66) && (DmX <= 70)) {TurnMaxInc(0.001);}
  } else {displayRht();}
}

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

void DM_Velocity_() {
  //##################################
  // Velocity display
  //##################################
  // user has clicked on a blue field in Monitor+ display
  if (!USB) {
    if ((DmX >= 33) && (DmX <= 60) && (DmY >= 4) && (DmY <= 19)) {
      if (VelTest) {
        // already in velocity test so cancel it
        TestDel = 0; TestMode = 4;
      } else {TestDel = 0; TestMode = 1;}
    } else if (DmX < 50) {
      // left side click
      displayLft();
    } else {
      // right side click
      if (VelTest) {
        if ((DmX >= 61) && (DmX <= 66) && (DmY >= 7) && (DmY <= 17)) {
          // change motor pointer
          motorChPnt();
        }
        else if ((DmX < 75) && (DmY > 29) && (DmY < 45)) {
          // user has clicked on velocity numbers field
                if ((DmX >= 49) && (DmX <= 53)) {motorChDir();}
          else if ((DmX >= 54) && (DmX <= 57)) {motorInc(10.0);}
          else if ((DmX >= 58) && (DmX <= 62)) {motorInc(1.0);}
          else if ((DmX >= 64) && (DmX <= 68)) {motorInc(0.1);}
          else if ((DmX >= 69) && (DmX <= 72)) {motorInc(0.01);}
        }
        else if ((DmX >= 50) && (DmX <= 67) && (DmY >= 49) && (DmY <= 60)) {motorFreq();}
        else if ((DmX >= 50) && (DmX <= 64) && (DmY >= 71) && (DmY <= 82)) {
          // user has clicked on voltage numbers field
                if ((DmX >= 50) && (DmX <= 53)) {voltageInc(1.0);}
          else if ((DmX >= 56) && (DmX <= 59)) {voltageInc(0.1);}
          else if ((DmX >= 61) && (DmX <= 64)) {voltageInc(0.01);}
        }
        else {displayRht();}
      } else {displayRht();}
    }
  } else {
    if (DmX < 50) {displayLft();} else {displayRht();}
  }
}

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

void doCmd() {
  // a '.' has been received so execute command if valid
  // supported commands are:
  //
  // .        - report P,I,D gains
  // DDn.     - increase (1) or decrease (-1) DispDel global delay offset DispDelGbl
  // DLn.     - toggle display lock, n=0 OFF, n=1 ON
  // DMxxyy.  - display monitor left mouse click, xx = 0-99, yy = 0-99
  // DM-.     - display monitor mouse up
  // Dn.      - D-gain enable, 1 = ON, 0 = OFF
  // DVnn.    - D-gain value * 100
  // G.       - report Scope data for graphing app
  // GE.      - end of Scope graphing mode
  // GPnn.    - nn points to data to be sent
  // In.      - I-gain enable, 1 = ON, 0 = OFF
  // IVnn.    - I-gain value * 100
  // PVnn.    - P-gain value * 1000

  cmdVal*= cmdSgn;  // correct cmdVal with the sign value

  switch (cmdMode) {
    case ' ':
      PrintTx += "PID=" + String((int)(Roll_p_gain * 1000.0)) + ",";
      if (Roll_i_En) {PrintTx += String((int)(Roll_i_gain * 100.0)) + ",";}
      else {PrintTx += "--.--,";}
      if (Roll_d_En) {PrintTx += String((int)(Roll_d_gain * 10.0)) + "\n";}
      else {PrintTx += "--.--\n";}
      break;
    case 'D':
      switch (cmdType) {
        case ' ': // toggle d-gain
          if (cmdVal == 1) {Roll_d_En = true;} else {Roll_d_En = false;}
          break;
        case 'D': // DDn. command, adjust global delay offset
          if (cmdVal > 0) {DispDelGbl ++;} else {DispDelGbl --;}
          if (DispDelGbl < 0) {DispDelGbl = 0;}
          // display this in the Rx field of Monitor+
          PrintTx+= "$DispDelGbl: " + String(DispDelGbl) +"\n";
          break;
        case 'L': // DLn. command, toggle display lock
          if (cmdVal != 0) {DispLock = true; DispOveride = DispMode;}
          else {DispLock = false;}
          break;
        case 'M':
          if (cmdSgn < 0) {}  // mouse click released
          else {
            if (cmdVal >= 10000) {
              // this is a Display Monitor mouse right-click
              // determine whether it is a left or right button click
              cmdVal /= 10000; cmdVal --; DmB = 1;
              DmY = cmdVal % 100; DmX = cmdVal/100;
            } else {
              // this is a Display Monitor mouse left-click
              DmY = cmdVal % 100; DmX = cmdVal/100; DmB = 0;
            }
                 if (DispMode == DM_Brake) {DM_Brake_();}
            else if (DispMode == DM_Commutate) {DM_Commutate_();}
            else if (DispMode == DM_Control) {DM_Control_();}
            else if (DispMode == DM_Filters) {DM_Filters_();}
            else if (DispMode == DM_Gears) {DM_Gears_();}
            else if (DispMode == DM_MPU_Acc) {DM_MPU_Acc_();}
            else if (DispMode == DM_MPU_AccRamps) {DM_Acc_Ramps();}
            else if (DispMode == DM_MPU_Gyros) {DM_MPU_Gyr_();}
            else if (DispMode == DM_PtchPID) {DM_PtchPID_();}
            else if (DispMode == DM_PtchSM) {DM_PtchSM_();}
            else if (DispMode == DM_RollPID) {DM_RollPID_();}
            else if (DispMode == DM_RollSM) {DM_RollSM_();}
            else if (DispMode == DM_TurnCtrl) {DM_TurnCtrl_();}
            else if (DispMode == DM_Velocity) {DM_Velocity_();}
            else {
              // this is not a special 'clickable' display, so simply change it
              if (DmX < 50) {displayLft();  // left side click
              } else {displayRht(); }       // right side click
            }
          }
          break;
        case 'V': // set D-gain variable
          Roll_d_gain = cmdVal; Roll_d_gain /= 10.0;
          PrintTx += "DV" + String(cmdVal) + "\n";
          break;
      } break;
    case 'G':
      // Scope graphing app report commands
      switch (cmdType) {
        case ' ': 
          // report the same set of values twice as WiFi data can easily be
          // corrupted. Therefore two matching consequitives messages must be
          // received for the graphing function to work
          // if in Display Monitor mode then switch that off
          if (DispMon) {DispMon = false; PrintTx = ""; DispTx = -1;}
          // RPms = millis() - RPt0; RPt0 = millis();
          ScopeReport(GPnn); ScopeReport(GPnn);
          break;
        case 'E': // GE. enter monitor mode when dropping out of graphing mode
          // return to Display Monitor mode if it was previously active
          if (DispTx < 0) {DispMon = true; PrintTx = ""; DispTx = 75; DispDel = 0; DispCnt = 0;}
          break;
        case 'P': // GPnn. sets the reporting mode and triggers title reporting
          if (DispMon) {DispMon = false; PrintTx = ""; DispTx = -1;}
          GPnn = cmdVal; 
          if (G_once) {G_once = false; GPnn = 4;}
          GP_Title = true;
          PrintTx += "GP" + String(GPnn) + "\n";
          break;
      } break;
    case 'I':
      switch (cmdType) {
        case ' ': // toggle i-gain
          if (cmdVal == 1) {Roll_i_En = true;} else {Roll_i_En = false;}
          break;
        case 'V': // set I-gain variable
          Roll_i_gain = cmdVal; Roll_i_gain /= 100.0;
          PrintTx += "IV" + String(cmdVal) + "\n";
          break;
      } break;
    case 'P':
      switch (cmdType) {
        case 'V': // set P-gain variable
          Roll_p_gain = cmdVal; Roll_p_gain /= 1000.0;
          PrintTx += "PV" + String(cmdVal) + "\n";
          break;
      } break;
  }
  // now reset the variables
  cmdMode = ' '; cmdType = ' '; cmdVal = 0; cmdSgn = 1;
}

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

void extendCmdVal(int zVal) {
  // adds a new digit to the right-hand end of cmdVal
  cmdVal = (cmdVal * 10) + zVal;  
}

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

String getBAhex() {
  // Rerturns the current Broardcast Address MAC, ie: 50:02:91:68:F7:3F
  String zRet$ = ""; uint8_t zV;
  for (int zP = 0;zP < 6;zP++) {
    zV = broadcastAddress[zP]/16;
    if (zV < 10) {zRet$ += char(zV + 48);} else {zRet$ += char(zV + 55);}
    zV = broadcastAddress[zP] - (zV *16);
    if (zV < 10) {zRet$ += char(zV + 48);} else {zRet$ += char(zV + 55);}
    if (zP < 5) {zRet$ += ":";}
  }
  return zRet$;
}

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

int16_t getBatV(int32_t zAvg) {
  // Determine the battery voltage, and percentage, by mapping the ADC readings onto threshold Voltages
  // These threshold voltages were determined using a multimeter, to calibrate the ESP32's ADC
  // if the voltage exceeds BatMax, the estimated voltage is displayed, but battery capacity will be limited to 100%
       if (zAvg > BatMax)  {BatV = map(zAvg,3402,3841,114,123); BatPc = 100;} // >= 100%
  else if (zAvg > BatHigh) {BatV = map(zAvg,3402,3841,114,123); BatPc = map(zAvg,3402,3841,80,100);}  // >= 80%
  else if (zAvg > BatWarn) {BatV = map(zAvg,3175,3402,105,114); BatPc = map(zAvg,3175,3402,20, 80);}  // >= 20%
  else {BatV = map(zAvg,2895,3175,100,105); BatPc = map(zAvg,2895,3175,0,20);}  // < 20%
  return BatV;
}

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

String GetBIN(byte zVal) {
  // returns an 8 character binary representation of zVal
  String zBIN$ = "";
  // convert zVal into binary
  if (zVal & 0B10000000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B01000000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00100000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00010000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00001000) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00000100) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00000010) {zBIN$ += "1";} else {zBIN$ += "0";}
  if (zVal & 0B00000001) {zBIN$ += "1";} else {zBIN$ += "0";}
  return zBIN$;
}

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

String getHmsTime(unsigned long zMillis) {
  // returns a 'time' string in Hrs:Min:sec base on zMillis
  String zTime$ = "";
  long zU = zMillis/3600000; zMillis = zMillis%3600000;
  if (zU > 0) {zTime$ = String(zU) + ":";}
  zU = zMillis/60000; zMillis = zMillis%60000;
  if (zU < 10) {zTime$ += "0";}
  zTime$ += String(zU);
  zU = zMillis/1000;
  if (zU < 10) {zTime$ += ":0";} else {zTime$ += ":";}
  zTime$ += String(zU);
  return zTime$;
}

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

String getNum00(int16_t zV) {
  // returns a two character number 00 - 99
  if (zV < 10) {return "0" + String(zV);}
  else {return String(zV);}
}

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

void GetRndCol(byte zB) {
  // return a random colour of strength zB
  if (zB < 2) {zB = 2;}
  switch(random(6)) {
    case 0: Col_R =   zB; Col_G =    0; Col_B =    0; break;  // red
    case 1: Col_R = zB/2; Col_G = zB/2; Col_B =    0; break;  // yellow
    case 2: Col_R =    0; Col_G =   zB; Col_B =    0; break;  // green
    case 3: Col_R =    0; Col_G = zB/2; Col_B = zB/2; break;  // cyan
    case 4: Col_R =    0; Col_G =    0; Col_B =   zB; break;  // blue
    case 5: Col_R = zB/2; Col_G =    0; Col_B = zB/2; break;  // magenta
  }
}

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

float getVolNow(float zVol) {
  // Returns a voltage which is corrected for battery discharge over time
  // BatVfpIni = supply voltage when SimpleFOC was initialised
  // BatVfp    = current battery voltage
  return (zVol * BatVfpIni/BatVfp);
}

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

void gotoDemoMode() {
  // initialises conditions for Demo Mode
  if (DEMO) {return;}

  DEMO = true;                  // set mode flag to prevent re-entry whilst face down
  // Serial.println(F("Demo Mode"));
  MainMode = 0;   // set main loop flag for Demo mode
  resetDMs();     // reset the displaymode references
  setDefaultsB(); // reset some variables
  // if (I2C_MPU) {MPU_6050_Initialise();} // ensure recent crash hasn't affected the MPU micro
  if (!USB) {
    // on 12v battery power
    Gear = 0;                   // speed control flag, default = 0
    PIDtask = 0;                // start with PID calculation sequence OFF
    Ptchen = true;              // enable Pitch PID calculations & safe modes
    PtchGo = false;             // ensure Pitch go flag is reset
    PtchSafeMode = -1;          // initialise Pitch safe mode
    Rollen = true;              // enable Roll PID calculations & safe modes
    RollGo = false;             // ensure Roll go flag is reset
    RollSafeMode = -1;          // initialise Roll safe mode
    SbPtchSpInc = SbPtchSpIncI; // set initial auto-adjust increment
    SbRollSpInc = SbRollSpIncI; // set initial auto-adjust increment
    TurnTask = 0;               // resaet Turn state machine
    set_LED_Task(1);            // Demo Safe Mode LEDs
    AutoOFF = 100;              // set auto remove motor power after 2 seconds
    changeGear(Gear);           // set speeds
    Display_Text_2x16("DEMO","Mode",50);
  } else {
    // on USB power
    PIDtask = 0;                // turn off PID calculation sequence
    Ptchen = false;             // disable Pitch safe mode & PID calculations
    Rollen = false;             // disable Roll safe mode & PID calculations
    set_LED_Task(4);            // green blink
    Display_Text_2x16("DEMO","USB Pwr",50);
  }
  synchLoopTimers();
}

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

void gotoSafeMode() {
  // Uni-Bot pointing upwards towards the sky
  // here we ensure everything is powered down, before going into Test mode
  if (SAFE) {return;}

  SAFE = true;                // set flag to prevent re-entry whilst pointing upwards
  PIDtask = 0;                // turn off PID calculation sequence
  Ptchen = false;             // disable Pitch safe mode
  PtchGo = false;             // ensure Pitch go flag is reset
  Rollen = false;             // disable Roll safe mode
  RollGo = false;             // ensure Roll go flag is reset
  motorA.voltage_limit = 3.0; // set voltage sufficient to stop motor
  motorA_Vel = 0.0;           // stop motor A
  motorA.move(motorA_Vel);
  motorB.voltage_limit = 3.0; // set voltage sufficient to stop motor
  motorB_Vel = 0.0;           // stop motor B
  motorB.move(motorB_Vel);
  motorC.voltage_limit = 3.0; // set voltage sufficient to stop motor
  motorC_Vel = 0.0;           // stop motor C
  motorC.move(motorC_Vel);
  AutoOFF = 100;              // set auto remove motor power after 2 seconds
  Display_Text_2x16("SAFE","Mode",50);
  gotoTestMode(false);
 }

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

void gotoTestMode(bool zD) {
  // initialises conditions for Test Mode
  if (DEMO) {resetDMs(); setDefaultsB();}
  // Serial.println(F("Test Mode"));
  AutoOFF = 100;              // set auto remove motor power after 2 seconds
  DEMO = false;               // clear flag
  PIDtask = 0;                // turn off PID calculation sequence
  Ptchen = false;             // disable Pitch safe mode
  PtchGo = false;             // ensure Pitch go flag is reset
  Rollen = false;             // disable Roll safe mode
  RollGo = false;             // ensure Roll go flag is reset
  MainMode = 1;               // set main loop flag for Demo mode
  TestMode = -1;              // start in default state
  set_LED_Task(0);            // set LEDs to Test mode
  if (zD) {Display_Text_2x16("TEST","Mode",50);}
  synchLoopTimers();
}

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

void initBatteryRead() {
  // called during runPOST() or on a soft reset
  // take 100 battery readings over a 100ms period to get an average
  for (int zI = 0;zI < 100;zI++) {BatVol+= analogRead(BatPin); delay(1);}
  BatVol/= 100;

  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."));}
  // Convert BatAvg to a mapped voltage and floating point equivalent
  BatV = getBatV(BatAvg); BatVfp = (float)BatV/10.0;
}

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

void Init_ESP_NOW() {
  // called to initiate an ESEP-NOW link or connect to another trannsceiver
  // each time this function is called it will attempt to connect to a different
  // boradcast address, cycling through them, over and over
  //  Serial.println("Init_ESP_NOW()");

  // if we have previously tried a connection then we need to de-initialise it
  if (ESP_NOW_Init) {
    // already attempted to initialise ESP-NOW so strip everything back
    Serial.println("Closing ESP-NOW");
    esp_now_del_peer(broadcastAddress);
    // WiFi.disconnect();
  }
  
  // Init ESP-NOW and returns its status
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  ESP_NOW_Init = true;
  
  // change the broadcast address pointer each time
  ESP_NOW_BA++;
  // Serial.println("ESP_NOW_BA = " + String(ESP_NOW_BA));
  
  switch (ESP_NOW_BA) {
    case 0: // Wii D1 Mk1 Transciever unique MAC: 50:02:91:68:F7:3F
      broadcastAddress[0] = 0x50;
      broadcastAddress[1] = 0x02;
      broadcastAddress[2] = 0x91;
      broadcastAddress[3] = 0x68;
      broadcastAddress[4] = 0xF7;
      broadcastAddress[5] = 0x3F;
      break;
    case 1: // Wii D1 Mk1 Transciever unique MAC: 50:02:91:68:F0:D2
      broadcastAddress[0] = 0x50;
      broadcastAddress[1] = 0x02;
      broadcastAddress[2] = 0x91;
      broadcastAddress[3] = 0x68;
      broadcastAddress[4] = 0xF0;
      broadcastAddress[5] = 0xD2;
      break;
    case 2: // Wii D1 Mk1 Transciever unique MAC: 58:BF:25:DB:27:A2
      broadcastAddress[0] = 0x58;
      broadcastAddress[1] = 0xBF;
      broadcastAddress[2] = 0x25;
      broadcastAddress[3] = 0xDB;
      broadcastAddress[4] = 0x27;
      broadcastAddress[5] = 0xA2;
      break;
    case 3: // Wii ESP32-C3 Zero Transciever unique MAC: EC:DA:3B:BD:4B:AC
      broadcastAddress[0] = 0xEC;
      broadcastAddress[1] = 0xDA;
      broadcastAddress[2] = 0x3B;
      broadcastAddress[3] = 0xBD;
      broadcastAddress[4] = 0x4B;
      broadcastAddress[5] = 0xAC;

      // End of list reached, so extend pause time and reset cycle
      // Turn off serial port reporting after first pass, unless connection fails
      ESP_NOW_BA = -1;      // 1st case will be 0
      WiFiConCnt = 75;      // 3 sec delay between trying cycles
      WiFiTryOnce = false;  // don't report trying after one cycle
      break;
  }

  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Trasnmitted packet
  esp_now_register_send_cb(OnDataSent);

  // Register peer
  esp_now_peer_info_t peerInfo; //initialize and assign the peer information as a pointer to an addres
  memset(&peerInfo, 0, sizeof(peerInfo));
  memcpy(peerInfo.peer_addr, broadcastAddress, 6); //copy the value of  broadcastAddress with 6 bytes to peerInfo.peer_addr
  peerInfo.channel = 0;     //channel at which the esp talk. 0 means undefined and data will be sent on the current channel. 1-14 are valid channels which is the same with the local device 
  peerInfo.encrypt = false; //not encrypted

  //Add the device to the paired device list 
  if (esp_now_add_peer(&peerInfo) != ESP_OK){ 
    Serial.println("Failed to add peer");
    return;
  }

  // Register for a callback function that will be called when data is received
  esp_now_register_recv_cb(OnDataRecv);
}

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

void LED_Tasks() {
  // called from the main loop() every 20ms (50Hz) to perform a range of LED tasks
  // LED_Task functions:
  //  0 - Test mode, blue battery voltage pattern
  //  1 - demo mode, colours represent the safe mode state of each controller
  //  2 - bright orange blink during SimpleFOC motor initialisation
  //  3 - blink red
  //  4 - blink green
  // 10 - bright random colours during Wii control connect/disconnect
  if (LED_Flash) {
    // bright white flash
    LED_Del = LED_Flash; LED_Flash = 0;
    LED[0].setRGB(128,128,128);  LED[1].setRGB(128,128,128);
    FastLED.show();
  }
  if (LED_Del > 0) {LED_Del--; return;}

  // perform specified task
  switch(LED_Task) {
    case -1: FastLED.clear(); FastLED.show(); LED_Del = 500; break;
    case  0: LED_Task_Batt(); break;
    case  1: LED_Task_DemoMode(); break;
    case  2: LED_Task_BlinkOrg(); break;
    case  3: LED_Task_BlinkRed(); break;
    case  4: LED_Task_BlinkGrn(); break;
    case 10: // random LED colours for Wii connect/disconnect
      GetRndCol(32); LED[0].setRGB(Col_R,Col_G,Col_B);
      GetRndCol(32); LED[1].setRGB(Col_R,Col_G,Col_B);
      FastLED.show(); LED_Del = 6; break;
  }
}

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

void LED_Task_Batt() {
  // cycle LED colour to represent battery voltage in Test Mode
  switch (LED_SubTsk) {
    case 0:
      LED_Cnt+= LED_Inc; if (LED_Cnt > 64) {LED_Inc = -4; LED_SubTsk ++;}
      LED[0].setRGB(0,0,LED_Cnt); LED[1].setRGB(0,0,LED_Cnt);
      LED_Del = 2; LEDshow = true;
      break;
    case 1:
      LED_Cnt = map(BatPc,20,100,1,5); LED_SubTsk ++; break;
    case 2:
      LED[0].setRGB(0,0,64); LED[1].setRGB(0,0,64); LEDshow = true;
      LED_Del = 4; LED_SubTsk ++; break;
    case 3:
      LED[0].setRGB(0,0,0); LED[1].setRGB(0,0,0); LEDshow = true;
      LED_Cnt--; if (LED_Cnt > 0){LED_SubTsk = 2; LED_Del = 8;}
      else {LED_SubTsk ++; LED_Del = 2; LED_Cnt = 64;}
      break;
    case 4:
      LED_Del = 2; LED_Cnt+= LED_Inc; if (LED_Cnt < 1) {LED_Inc = 1; LED_SubTsk = 0; LED_Del = 8;}
      LED[0].setRGB(0,0,LED_Cnt); LED[1].setRGB(0,0,LED_Cnt);
      LEDshow = true;
      break;
  }
}

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

void LED_Task_BlinkGrn() {
  // slowly blink green LED reflecting SAFE mode
  if (LED_Cnt < 1) {
    LED[0].setRGB(0,32,0); LED[1].setRGB(0,32,0);
    LED_Cnt = 1; LED_Del = 2;
  } else {
    LED[0].setRGB(0,0,0); LED[1].setRGB(0,0,0);
    LED_Cnt = 0; LED_Del = 8;
  }
  LEDshow = true;
}

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

void LED_Task_BlinkOrg() {
  // flash bright orange LED during motor initialisation period
  if (LED_Cnt < 1) {
    LED[0].setRGB(64,32,0); LED[1].setRGB(64,32,0);
    LED_Cnt = 1; LED_Del = 2;
  } else {
    LED[0].setRGB(0,0,0); LED[1].setRGB(0,0,0);
    LED_Cnt = 0; LED_Del = 8;}
  LEDshow = true;
}

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

void LED_Task_BlinkRed() {
  // flash red LED during motor tests
  if (LED_Cnt < 1) {
    LED[0].setRGB(32,0,0); LED[1].setRGB(32,0,0);
    LED_Cnt = 1; LED_Del = 2;
  } else {
    LED[0].setRGB(0,0,0); LED[1].setRGB(0,0,0);
    LED_Cnt = 0; LED_Del = 8;
  }
  LEDshow = true;
}

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

void LED_Task_DemoMode() {
  // default LED task for Demo mode
  LED_Flg = !LED_Flg; // toggle phase flag
  LED_Del = 5;        // default timer
  if (AcY < -7000) {
    // Uni-Bot is pointing face down, so display bright green LEDs
    LED[0].setRGB(0,64,0); LED[1].setRGB(0,64,0);
  } else if (PtchGo && RollGo) {
    // both Pitch and Roll are in safe mode 4, so flash alternating LEDs
    // LEDs are Red initially, turning to purple when self-balance is achieved
    // LED[0] is right Roll LED, LED[1] is left Pitch LED
    if (LED_Flg) {
      if (SbRollSpInc > 0.004) {LED[0].setRGB(48,0,0);} else {LED[0].setRGB(32,0,16);}
      LED[1].setRGB(0,0,0);
    } else {
      LED[0].setRGB(0,0,0);
      if (SbPtchSpInc > 0.000) {LED[1].setRGB(48,0,0);} else {LED[1].setRGB(32,0,16);}
    } LED_Del = 10;
  } else {
    // not yet in full balance mode, so handle each controller separately
    if (!Ptchen) {
      // Pitch is not enabled so set LED to green
      LED[1].setRGB(0,32,0);
    } else if (PtchSafeMode < 4) {
      if (PtchSafeMode <= 0) {
        LED[1].setRGB(0,32,0);
      } else if (LED_Flg) {
          if (PtchSafeMode <= 2) {LED[1].setRGB(24,24,0);}
          else {LED[1].setRGB(32,16,0);}
      } else {LED[1].setRGB(0,0,0);}
    }

    if (!Rollen) {
      // Pitch is not enabled so set LED to green
      LED[0].setRGB(0,32,0);
    } else if (RollSafeMode < 4) {
      if (RollSafeMode <= 0) {
        LED[0].setRGB(0,32,0);
      }else if (LED_Flg) {
        if (RollSafeMode <= 2) {LED[0].setRGB(24,24,0);}
        else {LED[0].setRGB(32,16,0);}
      } else {LED[0].setRGB(0,0,0);}
    }
  }
  LEDshow = true;
}

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

float mapF(float zX,float zin_min,float zin_max,float zout_min,float zout_max) {
  // floating point map() function
  return (zX - zin_min) * (zout_max - zout_min) / (zin_max - zin_min) + zout_min;
}

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

void MPU_6050_Initialise() {
  // set up gyro hardware
  // By default the MPU-6050 sleeps. So we have to wake it up.
  Wire.beginTransmission(MPU_address);  // Start communication with the address found during search.
  Wire.write(0x6B);                     // We want to write to the PWR_MGMT_1 register (6B hex)
  Wire.write(0x00);                     // Set the register bits as 00000000 to activate the gyro
  I2CErr = Wire.endTransmission();      // End the transmission with the MPU.
  if (I2CErr != 0) {return;}
  delayMicroseconds(10);                // Allow MPU time to respond

  //Set the full scale of the gyro to +/- 250 degrees per second
  Wire.beginTransmission(MPU_address);  // Start communication with the address found during search.
  Wire.write(0x1B);                     // We want to write to the GYRO_CONFIG register (1B hex)
  Wire.write(0x00);                     // Set the register bits as 00000000 (250dps full scale)
  I2CErr = Wire.endTransmission();      // End the transmission with the MPU.
  if (I2CErr != 0) {return;}
  delayMicroseconds(10);                // Allow MPU time to respond

  //Set the full scale of the accelerometer to +/- 4g.
  Wire.beginTransmission(MPU_address);  // Start communication with the address found during search.
  Wire.write(0x1C);                     // We want to write to the ACCEL_CONFIG register (1A hex)
  //  Wire.write(0x00);                 // Set the register bits as 00000000 (+/- 2g full scale range)
  Wire.write(0x08);                     // Set the register bits as 00001000 (+/- 4g full scale range)
  I2CErr = Wire.endTransmission();      // End the transmission with the MPU.
  if (I2CErr != 0) {return;}
  delayMicroseconds(10);                // Allow MPU time to respond

  //Set some filtering to improve the raw data.
  Wire.beginTransmission(MPU_address);  // Start communication with the address found during search
  Wire.write(0x1A);                     // We want to write to the CONFIG register (1A hex)
  Wire.write(0x03);                     // Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
  I2CErr = Wire.endTransmission();      // End the transmission with the MPU.
  if (I2CErr != 0) {return;}
  delayMicroseconds(10);                // Allow MPU time to respond
}

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

void readAccelAll() {
  // read the X,Y,Z-axis acceleration values from the MPU
  // in this robot:
  // AcX = yaw, +ve tip right, -ve tip left
  // AcY = up/down, +ve upside-down, -ve right way up
  // AcZ = pitch, +ve tip forward, -ve tip backward
  t2 = micros();                        // we are timing this I2C function
  AcXL = AcX; AcYL = AcY; AcZL = AcZ;   // track previous values
  Wire.beginTransmission(MPU_address);
  Wire.write(0x3B);                     // starting with register 0x3B (ACCEL_XOUT_H)
  I2CErr = Wire.endTransmission();      // End the transmission with the MPU.
  if (I2CErr != 0) {return;}            // abort on errors
  delayMicroseconds(I2Cdel);                // Allow MPU time to respond

  Wire.requestFrom(MPU_address,6);      // request 6 register read
  AcX = Wire.read()<<8|Wire.read();     // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read()<<8|Wire.read();     // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read()<<8|Wire.read();     // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

  // Serial.print(AcX); Serial.print("\t");
  // Serial.print(AcY); Serial.print("\t");
  // Serial.println(AcZ);
  
  // apply calibration offsets to these raw values
  AcX -= AccOffX;  // note AcXOff should be zero initially in order to determin its value
  AcY -= AccOffY;  // note AcYOff should be zero initially in order to determin its value
  AcZ -= AccOffZ;  // note AcZOff should be zero initially in order to determin its value

  // test for rapid Pitch accelerometer overshoot, due to vibration
  // setting AccRampY too low will impact on angle calculations
  if (AccRampX > 0) {
    AccDif = AcX - AcXL; if (fabs(AccDif) > AccRampX) {if (AcX > AcXL) {AcX = AcXL + AccRampX;} else {AcX = AcXL - AccRampX;}}
  }
  if (AccRampY > 0) {
    AccDif = AcY - AcYL; if (fabs(AccDif) > AccRampY) {if (AcY > AcYL) {AcY = AcYL + AccRampY;} else {AcY = AcYL - AccRampY;}}
  }
  if (AccRampZ > 0) {
    AccDif = AcZ - AcZL; if (fabs(AccDif) > AccRampZ) {if (AcZ > AcZL) {AcZ = AcZL + AccRampZ;} else {AcZ = AcZL - AccRampZ;}}
  }
  
  // // // test for rapid Roll accelerometer overshoot, due to vibration
  // AccDif = AcZ - AcZL;
  // if (fabs(AccDif) > AccRamp) {
  //   // rate of change of AcZ is too great, so limit to AccRamp
  //   if (AcZ > AcZL) {AcZ = AcZL + AccRamp;} else {AcZ = AcZL - AccRamp;}
  // }

  //##############################################
  // Averaging filters to reduce MPU random noise
  //##############################################
  // set AvCnt > 1 to increase averaging effect, default AvCnt = 1
  // note this form of filtering introduces a phase delay, which may lead to instability
  if (AvCnt > 1) {
    AcXSum = AcXSum - AcXAvg + AcX; AcXAvg = AcXSum /AvCnt; AcX = AcXAvg;
    AcYSum = AcYSum - AcYAvg + AcY; AcYAvg = AcYSum /AvCnt; AcY = AcYAvg;
    AcZSum = AcZSum - AcZAvg + AcZ; AcZAvg = AcZSum /AvCnt; AcZ = AcZAvg;
  }

  // limit values to avoid trig function problems
  if (AcX > 8192) {AcX = 8192;} else if (AcX < -8192) {AcX = -8192;}
  if (AcY > 8192) {AcY = 8192;} else if (AcY < -8192) {AcY = -8192;}
  if (AcZ > 8192) {AcZ = 8192;} else if (AcZ < -8192) {AcZ = -8192;}
  t3 = micros() - t2;                   // capture time spent in t3, 980us
}

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

void readGyrosAll() {
  // read the X,Y,Z-axis gyroscope values from the MPU
  // note gyro values are in degrees/sec so we use time between measurements to 
  // determine the average displaced angle
  //  t0 = micros();  // t1-t0 average = 315µs, longer with I2C bus contention
  GyXL = GyX; GyYL = GyY; GyZL = GyZ;   // track previous values
  Wire.beginTransmission(MPU_address);
  Wire.write(0x43);                     // starting with register 0x43 (GYRO_XOUT_H)
  I2CErr = Wire.endTransmission();      // End the transmission with the MPU.
  if (I2CErr != 0) {return;}            // abort on errors
  delayMicroseconds(I2Cdel);            // Allow MPU time to respond

  Wire.requestFrom(MPU_address,6);      // request 6 register reads
  GyX = Wire.read()<<8|Wire.read();     // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read()<<8|Wire.read();     // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read()<<8|Wire.read();     // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  // Serial.print(GyX); Serial.print("\t");
  // Serial.print(GyY); Serial.print("\t");
  // Serial.println(GyZ);

  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 so set nominal 5ms
    GyT = micros();
    GyTD = 5000; GyXL = GyX; GyYL = GyY; GyZL = GyZ;
  }

  // apply calibration offsets to these raw values
  GyX -= GyrOffX;  // note GyrOffX should be zero initially in order to determin its value
  GyY -= GyrOffY;  // note GyrOffY should be zero initially in order to determin its value
  GyZ -= GyrOffZ;  // note GyrOffZ should be zero initially in order to determin its value

  // average over two readings to reduce random noise
  // GyX = (GyX + GyXL)/2; GyY = (GyY + GyYL)/2; GyZ = (GyZ + GyZL)/2;

  //##############################################
  // Averaging filters to reduce MPU random noise
  //##############################################
  GyXSum = GyXSum - GyXAvg + GyX; GyXAvg = GyXSum /AvCnt; GyX = GyXAvg;
  GyYSum = GyYSum - GyYAvg + GyY; GyYAvg = GyYSum /AvCnt; GyY = GyYAvg;
  GyZSum = GyZSum - GyZAvg + GyZ; GyZAvg = GyZSum /AvCnt; GyZ = GyZAvg;
  //  t1 = micros();
}

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

void readSerial() {
  // reads characters from the serial port and responds to commands
  // set PrintTgt to indicate source as serial port for responses
  keyVal = Serial.read();
  if (keyVal != -1) {SerialRx = true; PrintTgt = 0; decodeKey(keyVal);}
}

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

void readWiFiRx() {
  // called from the main loop every cycle to check for ESP-NOW data  
  // a state machine is used to separate the data blocks by header type
  switch(Rx_Task) {
    case 0: // determine the type of data to handle
      // B  - binary data block
      // C  - Wii Classic controller
      // N  - Wii Nunchuk controller
      // P  - Wll Classic Pro controller
      // -  - unknown type, transceiver did not recognise
           if (Rx_Buff.ESPdata[Rx_Pnt] == 'B') {Rx_Task = 3; break;}                // binary data block
      else if (Rx_Buff.ESPdata[Rx_Pnt] == 'C') {WiiType = 'C'; Rx_Task = 1; break;} // Classic controller
      else if (Rx_Buff.ESPdata[Rx_Pnt] == 'N') {WiiType = 'N'; Rx_Task = 1; break;} // Nunchuk controller
      else if (Rx_Buff.ESPdata[Rx_Pnt] == 'P') {WiiType = 'P'; Rx_Task = 1; break;} // Classic Pro controller
      else if (Rx_Buff.ESPdata[Rx_Pnt] == '-') {WiiType = '-'; Rx_Task = 1; break;} // unknown type
      else if (Rx_Buff.ESPdata[Rx_Pnt] == '$') {Rx_Task = 2; break;}
      // oh dear! block header not recognised so discard the block and flag error
      else {WiFiRxErr++; WiFiRx = false;}
      break;
      
    case 1: // load Wii I2C data
      // after the header byte there are 6 data bytes + checksum
      // perform XOR on 6 bytes to check for errors
      RxClass = false;  // clear the 'Classic' flag
      RxNunch = false;  // clear the 'Nunchuk' flag
      if (Rx_Buff.ESPdata[7] == (Rx_Buff.ESPdata[1] ^ Rx_Buff.ESPdata[2] ^ Rx_Buff.ESPdata[3] ^ Rx_Buff.ESPdata[4] ^ Rx_Buff.ESPdata[5] ^ Rx_Buff.ESPdata[6])) {
        // data has passed the checksum test
        RxWiFi[0] = Rx_Buff.ESPdata[1];
        RxWiFi[1] = Rx_Buff.ESPdata[2];
        RxWiFi[2] = Rx_Buff.ESPdata[3];
        RxWiFi[3] = Rx_Buff.ESPdata[4];
        RxWiFi[4] = Rx_Buff.ESPdata[5];
        RxWiFi[5] = Rx_Buff.ESPdata[6];

        // if Wii data strip off the JX,JY and CZ values
        RxRec = true; // indicate a valid frame
        if (WiiType == 'N') {
          JoyX = RxWiFi[0];             // joystick X 0-255
          JoyY = RxWiFi[1];             // joystick Y 0-255
          CZ = RxWiFi[5] & 3;           // CZ buttons, C = 2/0, Z = 1/0, active low
        } else if (WiiType == 'C') {    // Wii Classic
          JoyX = WiiRightStickX() * 8;  // joystick X 0-255
          JoyY = WiiRightStickY() * 8;  // joystick Y 0-255
          CZ = ((RxWiFi[4] & B100000)>>5) + (RxWiFi[4] & B10); // read LT and RT buttons as C,Z
        } else if (WiiType == 'P') {    // Wii Classic Pro
          JoyX = WiiRightStickX() * 8;  // joystick X 0-255
          JoyY = WiiRightStickY() * 8;  // joystick Y 0-255
          CZ = (RxWiFi[4] & 0b10) + ((RxWiFi[5] & 0b100)>2); // read RT and ZR buttons as C,Z
        } else {
          // not Nunchuk so set defaults
          JoyX = 128; JoyY = 128; CZ = 3;
        }
      } else {
        // received data failed the checksum test
        WiFiRxErr++; RxRec = false;
      }
      Rx_Task = 0;  // reset the task pointer to lok at another block if needed
      Rx_Pnt += 8;  // increase the data pointer beyond this block
      if (Rx_Pnt >= Rx_len) {WiFiRx = false; Rx_len = 0;} // reached end of buffer
      break;

    case 2: // load text data as if from a serial port
      // read characters up to the end of the buffer
      Rx_Pnt++;
      if (Rx_Pnt >= Rx_len) {WiFiRx = false; Rx_len = 0; break;} // reached end of buffer
      keyVal = Rx_Buff.ESPdata[Rx_Pnt]; PrintTgt = 1; decodeKey(keyVal);
  //      Serial.write(keyVal);
      break;

    case 3: // load binary data block
      // data length is specified in second byte
      Rx_Pnt++;
      // at the moment just jump over it, binary transfer is not needed
      Rx_Pnt += Rx_Buff.ESPdata[Rx_Pnt] + 1;
      if (Rx_Pnt >= Rx_len) {WiFiRx = false; break;} // reached end of buffer
      break;
  }
}

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

void readWii() {
  // called every 20ms, but wireless transceiver only transmits every 40ms
  // considers data received from the Wii Nunchuk controller over WiFi and 
  // manages all movements demanded by the controller
  if (RxRec) {
    //##############################################################################
    //
    //  WiFi Nunchuk data received
    //
    //##############################################################################
    // Wii data received but we may not have enabled WiFi mode yet
    RxRec = false; RTxTimeout = 25; // set timeout to approx 500ms
    if (!WiFiEn) {
      //##############################################################################
      //
      //  WiFi is currently disabled, listening
      //
      //##############################################################################
      // we have not received enough 'C' buttons to make the system active
      // the 'C' button alone must be held in to activate the Wii WiFi link
      if (CZ == 1) {
        // C button only is being pressed, normally for WiFi connection or a Gear
        // change if already connected
        if (WiFiCntC < 1) {Display_Text_2x16("Connecting","WiFi",50);}
        WiFiCntC += 2;
      // Serial.println("WiFiCnt = " + String(WiFiCntC));
        if (WiFiCntC > 60) {
          // going active, user has held in 'C' button for sufficient time
        //  Serial.println("WiFiEn = true");
          WiFiEn = true; C_Dn = 8; WiFiCntC = 64;
          Display_Text_2x16("WiFi","Connected",25);
          // set defaults here
          Gear = 1; changeGear(Gear);
          if (MainMode == 0) {set_LED_Task(1);}   // LED tasks set to Demo mode
          else {set_LED_Task(0);}                 // LED tasks set to Test mode
        } else {
          // still counting C buttons
          set_LED_Task(10);
        }
      } else {
        // C button not pressed or released early so check for a inadequate count
        if (WiFiCntC > 0) {
          // reduce the C-button counter, which has to be sustained to enable WiFi mode
          WiFiCntC--;
          if (WiFiCntC < 1) {
            if (MainMode == 0) {set_LED_Task(1);}   // LED tasks set to Demo mode
            else {set_LED_Task(0);}                 // LED tasks set to Test mode
            Display_Text_2x16("WiFi","Not Connected",25);
          } else {set_LED_Task(10);}
        }
      }
    } else {
      
      //##############################################################################
      //
      //  WiFi is currently enabled
      //
      //##############################################################################
      // WiFi enabled so check for disconnection
      if (CZ == 0) {
        // both C and Z buttons are being pressed so move towards WiFi disconnection
        if (WiFiCntC > 0) {
          if (WiFiCntC == 64) {Display_Text_2x16("Disconnecting","WiFi Mode",25);}
          WiFiCntC -= 2;
        //  Serial.println("WiFiCnt = " + String(WiFiCntC));
          if (WiFiCntC <= 0) {
            // disconnection condition satisfied
          //  Serial.println("WiFi mode disabled!");
            WiFiCntC = 0; WiFiEn = false;
            Display_Text_2x16("WiFi Mode","Disconnected",25);
          } else {
            // still counting down CZ buttons
            set_LED_Task(10);
          }
        }
      } else {
        // Check CZ buttons to see if a speed change is wanted
        if ((CZ & 2) == 0) {
          // C button is pressed, assume a speed increase is needed
          C_Dn++;
          if (C_Dn == 2) {
            C_Dn = 8;
            if (Gear < 3) {Gear++; changeGear(Gear);}
            Display_Text_2x16("Gear",String(Gear),25);
          }
        //  Serial.println("WalkSpeed =" + String(WalkSpeed));
        } else {C_Dn = 0;} // C button released
        // Check Z button. It has two functions. Short press change speed, but if
        // held in it changes the mode of the joysticks
        if ((CZ & 1) == 0) {
          // Z button is pressed
          Z_Dn++; if (Z_Dn > 12) {Z_Dn = 50;} // Z_Dn capped at 50, held for >= 2 seconds
        //  Serial.print("Z_Dn "); Serial.println(Z_Dn);
        } else {
          // Z button is not pressed
          if ((Z_Dn > 1) && (Z_Dn <= 10)) {
            // held down for less than 400 ms @ 40ms Rx cycle
            if (Gear > 1) {Gear--; changeGear(Gear);}
            Display_Text_2x16("Gear",String(Gear),25);
          } Z_Dn = 0; // Z button released so clear down counter
        }
      //  Serial.print("Z_Dn "); Serial.println(Z_Dn);

        if (WiFiCntC < 64) {
          // C-button counter is not at full strength, assume WiFi off released prematurely
          WiFiCntC++;
          if (WiFiCntC == 64) {LED_Task = -1;} else {LED_Task = -10;}
        }
      }
    }
  }
}

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

void ResetAllNow() {
  // called from clicking on Control display
  // all values will be reset to their defaults
  setPIDgains();        // set the default PID gains
  setDefaultsA();       // set initial conditions for primary variables
  setDefaultsB();       // set initial conditions for variables

  // now, like runPOST, we set the MainMode depending on orientation
  initBatteryRead();    // initialise battery readings
  CheckOrientation();   // determine mode based on orientation
  set_LED_Task(0);      // set LED task to blue pulse
  synchLoopTimers();
}

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

void RxGetChecksum(int zlen) {
  // performs an XOR checksum calc on the Rx buffer of length zlen
  Rx_Chk = 0x55;  // define checksum seed as 01010101
  for (int zP = 0;zP < zlen;zP++) {
    Rx_Chk = Rx_Chk ^ Rx_Buff.ESPdata[zP];
  }
}

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

void set_LED_Task(int16_t zT) {
  // starts a new LED task
  if (LED_Task == zT) {return;}

  LED_Del = 0;          // LED Task delay, adds pauses into the processes
  LED_Cnt = 0;          // general counter used in LED tasks
  LED_Flg = false;      // LED flag used in simple flashing mode
  LED_Inc = 1;          // LED Task increment variable
  LED_SubTsk = 0;       // LED sub-task pointer
  LED_Task = zT;        // LED task pointer
}

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

int WiiRightStickX() {
  // returns a Wii Classic RX value 0/16/31
  return ((RxWiFi[0] & (byte)0xc0) >> 3) + ((RxWiFi[1] & (byte)0xc0) >> 5) +  ((RxWiFi[2] & (byte)0x80) >> 7);
}

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

int WiiRightStickY() {
  // returns a Wii Classic RY value 0/16/31
  return RxWiFi[2] & (byte)0x1f;    
}

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



  //###############################################################################
  //
  //  WiFi Code
  //
  //###############################################################################

void WiFiClearRxBuff(int zNum) {
  // clears the contents of the receive buffer
  for (int zP = 0; zP < zNum;zP++) {Rx_Buff.ESPdata[zP] = 0;}
}

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

void WiFiClearTxBuff(int zNum) {
  // clears the contents of the transmit buffer
  for (int zP = 0; zP < zNum;zP++) {Tx_Buff.ESPdata[zP] = 0;}
}

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

void WiFiDisable() {
  // when either a switch is pressed or WiFi disable is required
  WiFiEn = false; WiFiCntC = 0; WiFiRestCnt = 0; C_Dn = 8; Z_Dn = 8;
}

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

void WiFiDisconnect() {
  // enter the WiFi disconnected state
  WiFiConnected = false;
  WiFiClearRxBuff(WiFiTx_len);
  WiFiClearTxBuff(WiFiTx_len);
  WiFiTryCnt = 0;     // reset the connection retry counter
  WiFiTx_CB = 1;      // set ready to send flag
  WiFiRxRec = false;  // clear the data received flag
  Serial.println("WiFiDisconnected!");
}

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

void WiFiTryToConnect() {
  // try to connect with the Wii Transceiver by sending devices name
  // initialise ESP-NOW link first
  Init_ESP_NOW();

  Tx_Buff.ESPdata[ 0] = 'U';
  Tx_Buff.ESPdata[ 1] = 'n';
  Tx_Buff.ESPdata[ 2] = 'i';
  Tx_Buff.ESPdata[ 3] = '-';
  Tx_Buff.ESPdata[ 4] = 'B';
  Tx_Buff.ESPdata[ 5] = 'o';
  Tx_Buff.ESPdata[ 6] = 't';
  Tx_Buff.ESPdata[ 7] = ' ';
  Tx_Buff.ESPdata[ 8] = 'R';
  Tx_Buff.ESPdata[ 9] = '0';
  WiFiTx_len = 11;
  //  t0 = micros();
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &Tx_Buff, WiFiTx_len);
  if (result == ESP_OK) {
  //    Serial.println("Sent with success");
  } else {
  //    Serial.println("Error sending the data");
  } 

  WiFiTryNum++; // count the total number of attempts
  //  Serial.println("WiFiTryToConnect...");
}

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

void WiFiTxGetChecksum(int zlen) {
  // performs an XOR checksum calc on the Tx buffer of length zlen
  WiFiTx_Chk = 0x55;  // define checksum seed as 01010101
  for (int zP = 0;zP < zlen;zP++) {
    WiFiTx_Chk = WiFiTx_Chk ^ Tx_Buff.ESPdata[zP];
  }
}

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

