
/* SSD1306Wire.h functions, see https://github.com/ThingPulse/esp8266-oled-ssd1306

  disply.clear()        - clear the local pixel buffer
  disply.display()      - write the buffer to the display
  disply.displayOff()   - turn the display OFF
  disply.displayOn()    - turn the display ON
  display.drawCircle(int16_t x, int16_t y, int16_t radius)
  display.drawFastImage(int16_t x, int16_t y, int16_t width, int16_t height, const uint8_t *image)
  display.drawHorizontalLine(int16_t x, int16_t y, int16_t length)
  display.drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1)  - draw from x0k,y0 to x1,y1
  display.drawProgressBar(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint8_t progress)
  display.drawRect(int16_t x, int16_t y, int16_t width, int16_t height)
  display.drawString(int16_t x, int16_t y, String text)
  display.drawStringMaxWidth(int16_t x, int16_t y, int16_t maxLineWidth, String text)
  display.drawVerticalLine(int16_t x, int16_t y, int16_t length)
  display.drawXbm(int16_t x, int16_t y, int16_t width, int16_t height, const char* xbm)
  display.fillCircle(int16_t x, int16_t y, int16_t radius)
  display.fillRect(int16_t x, int16_t y, int16_t width, int16_t height)
  display.flipScreenVertically()   - flip the current screen upside down
  disply.init()         - initialise the display
  disply.invertDisplay()- inverted display mode
  disply.end()          - free memory used by the display
  disply.displayOn()    - turn the display ON
  display.mirrorScreen() - draw the screen mirrored
  disply.normalDisplay()- normal display mode
  disply.reconnect()    - connect again through I2C
  display.setBrightness(uint8_t)
  display.setColor(OLEDDISPLAY_COLOR color) - BLACK, WHITE, INVERSE
  display.setContrast(uint8_t contrast, uint8_t precharge = 241, uint8_t comdetect = 64)
  display.setFont(const uint8_t* fontData)
  display.setPixel(int16_t x, int16_t y)  - set a pixel at x,y
  display.setTextAlignment(OLEDDISPLAY_TEXT_ALIGNMENT textAlignment)  - TEXT_ALIGN_LEFT, TEXT_ALIGN_CENTER, TEXT_ALIGN_RIGHT, TEXT_ALIGN_CENTER_BOTH

  W = display.getStringWidth(const char* text, uint16_t length)
  W = display.getStringWidth(String text)
  
  Display size is 128 x 64 (w x h) pixels
  Default fonts are ArialMT at 10, 16 & 24 pixels high
  Text options include:
    2 x 24, + 1 x 10 pnt lines
    1 x 24, + 2 x 16 pnt lines
    1 x 24, + 3 x 10 pnt lines
    4 x 16 pnt lines, no line spaces, decending characters will merge
    3 x 16, + 1 x 10 pnt lines
    6 x 10 pnt lines, no line spaces, decending characters will merge
*/

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

void Display_40ms() {
  // called every 40ms from the main loop if DispMon == true
  // if DispDel > 0 then we wait for DispDel cycles to complete before update
  // if (DispNOP) {return;}  // this function is aborted, no I2C activity
  
  if (DispDel > 0) {DispDel--; return;} // don't update display if DispDel > 0
  if (DispClr) {Display_clear(); DispClr = false; return;}
  if (DispDisp) {Display_display(); DispDisp = false; DispClr = true; return;}

  // we refresh individual displays based on a counter, DispCnt
  // set DispCnt -ve to prevent the display update process
  
  if (DispCnt > 0) {DispCnt--;}
  else {
    // normal screens, display depends on MainMOde values
    if (DispLock) {DispMode = DispOveride;} // force locked mode
    if (MainMode) {
      // display the following in Test mode
      switch(DispMode) {
        case -1: Display_Control(); break;
        case  0: Display_Battery(); break;
        case  1: Display_MPU_Levels(); break;
        case  2: Display_MPU_Acc(); break;
        case  3: Display_MPU_Gyro(); break;
        case  4: Display_MPU_Ang(); break;
        case  5: Display_MPU_AccRamps(); break;
        case  6: Display_PID_Ang(); break;
        case  7: Display_PtchSafeMode(); break;
        case  8: Display_PtchPID(); break;
        case  9: Display_RollSafeMode(); break;
        case 10: Display_RollPID(); break;
        case 11: Display_TurnCtrl(); break;
        case 12: Display_Velocity(); break;
        case 13: Display_Commutate(); break;
        case 14: Display_T0_T1(); break;
        case 15: Display_Joysticks(); break;
        case 16: Display_Wii_Ctrl(); break;
        default:  // catch-all
          DispMode = 0; break;
      }
      DM_Min = -1; DM_Max = 16;
    } else {
      // display the following in Demo mode
      switch(DispMode) {
        case -1: Display_Control(); break;
        case  0: Display_Battery(); break;
        case  1: Display_MPU_Levels(); break;
        case  2: Display_PtchSafeMode(); break;
        case  3: Display_PtchPID(); break;
        case  4: Display_RollSafeMode(); break;
        case  5: Display_RollPID(); break;
        case  6: Display_Filters(); break;
        case  7: Display_TurnCtrl(); break;
        case  8: Display_Brake(); break;
        case  9: Display_Gears(); break;
        case 10: Display_MPU_Acc(); break;
        case 11: Display_MPU_Gyro(); break;
        case 12: Display_MPU_Ang(); break;
        case 13: Display_T0_T1(); break;
        default:  // catch-all
          DispMode = 0; break;
      }
      DM_Min = -1; DM_Max = 13;
    }
    // the following line puts a rectangle around every display, showing the boundry region
  //  Border = true;
  //  if ((Border) || TEST) {Display_drawRect(0,0,128,64);}

    // Add a global frame delay offset, if set
    DispDel += DispDelGbl;
  }
}

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

void Display_Battery() {
  // normal screen to be displayed
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //  1111111111111111    2222222222222222        3333333333333333
  //                           4444444444444444
  Display_setFont(16); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 1, "Battery");
  if (BatAvg > BatCritical) {
    // battery powered mode
    Display_setTextAlignment(TEXT_ALIGN_LEFT);
    Display_drawString( 1,26, String(BatAvg));
    Display_setTextAlignment(TEXT_ALIGN_CENTER);
    if (BatVfp > 9.99) {Display_drawString(64, 21, String(BatVfp,1) + "v");}
    else {Display_drawString(64,21, String(BatVfp,2) + "v");}
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    Display_drawString(126,26, String(BatPc) + "%");
    // display the time
    Display_setTextAlignment(TEXT_ALIGN_LEFT);
    Display_drawString( 1,45, getHmsTime(millis()));
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    Display_drawString(126,45, BatTime$);
  } else {
    // USB powered mode
    Display_setTextAlignment(TEXT_ALIGN_CENTER);
    Display_drawString(64,24, "USB Pwr");
    // display the time
    Display_drawString(64,45, getHmsTime(millis()));
  }
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 25; // update every 1s
}

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

void Display_Battery_Low() {
  // battery critical warning
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //               1111111111111111     2222222222222222
  Display_setFont(16); Display_setColor(RED);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64,14, "Battery Critical 0%");
  Display_drawString(64,35, "Shutting down");
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
}

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

void Display_BatWarning() {
  // battery <= 20% warning
  // we need a timer here to prevent this being called too often
  if ((millis() - DispBatWarn) < 30000)  {return;} // only action after 30 sec

  DispBatWarn = millis();  // reset the 30 sec timer
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //                1111111111111111    2222222222222222
  Display_setFont(16); Display_setColor(RED);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64,15, "Battery Low");
  Display_drawString(64,35, "< 20%");
  Display_drawRect(0,0,128,64);   // draw box outline

  Display_setColor(WHITE);
  Display_display();
  DispDel = 50; // hold off display update for 2sec
}

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

void Display_Brake() {
  // test motor commutation with changing acceleration
  DM_Brake = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //    1111111111        2222222222   3333333333
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 3, "Brake");

  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(62,21, "MotCVel");
  Display_drawString(62,34, "Force");

  Display_setColor(BLUE);
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(66,21, String(Brake_Vel,2));
  Display_drawString(66,34, String(Brake_Force,3));

  Display_setColor(WHITE);
  Display_display();
  DispDel = 5; // update every 200ms
}

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

void Display_Commutate() {
  // test motor commutation with changing acceleration
  DM_Commutate = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111   2222222222  3333333333  4444444444  5555555555
  Display_setFont(10);
  if (!USB) {Display_setColor(BLUE);} else {Display_setColor(RED);}
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  String zT$ = "-";
  if (ComTest) {
    switch (motorPnt) {
      case 0: zT$ = "A"; break;
      case 1: zT$ = "B"; break;
      case 2: zT$ = "C"; break;
    }
  } Display_drawString(64, 2, "Commutate " + zT$);

  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_setColor(WHITE);
  Display_drawString(60,15, "Motor Acc");
  Display_drawString(60,27, "Freq");
  Display_drawString(60,39, "Voltage");
  Display_drawString(60,51, "Mode");

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (ComTest) {
    Display_setColor(BLUE);
    switch (motorPnt) {
      case 0:
        if (motorA_Vel < 0.0) {Display_drawString(65,15, "-");} else {Display_drawString(63,15, "+");}; break;
      case 1:
        if (motorB_Vel < 0.0) {Display_drawString(65,15, "-");} else {Display_drawString(63,15, "+");} break;
      case 2:
        if (motorC_Vel < 0.0) {Display_drawString(65,15, "-");} else {Display_drawString(63,15, "+");} break;
    }
    switch (motorPnt) {
      case -1:
        Display_drawString(69,15, "-.---"); break;
      case 0:
        Display_drawString(69,15, String(fabs(motorA_Vd),3)); break;
      case 1:
        Display_drawString(69,15, String(fabs(motorB_Vd),3)); break;
      case 2:
        Display_drawString(69,15, String(fabs(motorC_Vd),3)); break;
    }
    if (Motor_Freq == 999) {Display_drawString(64, 27, "Max");}
    else {Display_drawString(64, 27, String(Motor_Freq) + "Hz");}
    switch (motorPnt) {
      case -1: Display_drawString(64,39, "--.--v"); break;
      case  0: Display_drawString(64,39, String(motorAVol) + "v"); break;
      case  1: Display_drawString(64,39, String(motorBVol) + "v"); break;
      case  2: Display_drawString(64,39, String(motorCVol) + "v"); break;
    }
    if (ComType == 0) {Display_drawString(64,51, "Sawtooth");} else {Display_drawString(64,51, "StartStop");}
  } else {
    // ComTest is not active
    Display_drawString(69,15, "-.---");
    if (Motor_Freq == 999) {Display_drawString(64,27, "Max");}
    else {Display_drawString(64,27, String(Motor_Freq) + "Hz");}
    Display_drawString(64,39, "--.--v");
    if (ComType == 0) {Display_drawString(64,51, "Sawtooth");} else {Display_drawString(64,51, "StartStop");}
  }

  Display_setColor(WHITE);
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 3; // update every 120ms
}

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

void Display_Control() {
  // display a control screen, with options
  DM_Control = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111   2222222222  3333333333  4444444444  5555555555
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 2, "Control");

  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(62,15, "MainMode");
  Display_drawString(62,27, "Reset All");
  Display_drawString(62,39, "Motor Cool");
  Display_drawString(62,51, "Av.Filter");

  Display_setColor(BLUE);
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (MainMode == 0) {Display_drawString(66,15, "Demo");}
  else {Display_drawString(66,15, "Test");}
  Display_drawString(66,27, ">Now");  // this is for Rest All
  Any$ = ""; if (AvCnt < 10) {Any$ = "0";}
  Display_drawString(66,51, Any$ + String(AvCnt));  // this is Avg filter count

  if (USB) {Display_setColor(RED);}

  if (MainMode == 0) {
    // in Demo mode, so cooling test is not available
    Display_setColor(WHITE); Display_drawString(66,39, "NA");
  } else {
    // in Test mode, so cooling test can be run
    if (Cooler) {Display_drawString(66,39, ">Off");}
    else {Display_drawString(66,39, ">On");}
  }

  Display_setColor(WHITE);
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void Display_Filters() {
  // display joystick data
  DM_Filters = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111      2222222222   3333333333   4444444444
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 2, "Filters");
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(62,18, "LpfA");
  Display_drawString(62,31, "LpfB");
  Display_drawString(62,44, "LpfC");

  Display_setTextAlignment(TEXT_ALIGN_LEFT); Display_setColor(BLUE);
  Display_drawString(66,18, String(LpfA,2));
  Display_drawString(66,31, String(LpfB,2));
  Display_drawString(66,44, String(LpfC,2));

  Display_display();
  DispDel = 10; // update every 200ms
}

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

void Display_Gears() {
  // display joystick data
  DM_Gears = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111      2222222222   3333333333   4444444444
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 2, "Gears");
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(30,18, "Ang1"); Display_drawString(94,18, "Spd1");
  Display_drawString(30,31, "Ang2"); Display_drawString(94,31, "Spd2");
  Display_drawString(30,44, "Ang3"); Display_drawString(94,44, "Spd3");

  Display_setTextAlignment(TEXT_ALIGN_LEFT); Display_setColor(BLUE);
  Display_drawString(34,18, String(TgtAng1,2)); Display_drawString(98,18, String(maxTgtSpd1,1));
  Display_drawString(34,31, String(TgtAng2,2)); Display_drawString(98,31, String(maxTgtSpd2,1));
  Display_drawString(34,44, String(TgtAng3,2)); Display_drawString(98,44, String(maxTgtSpd3,1));

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 10; // update every 200ms
}

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

void Display_Joysticks() {
  // display joystick data
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111   2222222222 3333333333     4444444444 5555555555
  Display_setFont(10);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 2, "Joysticks");
  if (!WiFiEn) {
    // problems with WiFi
    if (WiFiConnected) {
      Display_drawString(64,15, "WiFi Connected");
      Display_drawString(64,27, "NOT Enabled");

      Display_setTextAlignment(TEXT_ALIGN_LEFT);
      Display_drawString(28,41, "JX:" + String(JoyX));
      Any$ = "0"; if (CZ & 2) {Any$ = "1";}
      if (CZ & 1) {Any$ += "1";} else {Any$ += "0";}
      Display_drawString(28,52, "CZ:" + Any$);
      Display_drawString(73,41, "JY:" + String(JoyY));
      Display_drawString(73,52, " D: -");
    } else {
      Display_drawString(64,15, "No WiFi Link");
      Display_drawString(64,26, "Inaccessible");
    }
  } else {
    if (WiFiConnected) {
      Display_drawString(64,15, "WiFi Connected");
      Display_drawString(64,26, "Enabled");
    } else {
      Display_drawString(64,15, "WiFi Enabled");
      Display_drawString(64,26, "Not Connected");
    }
    Display_setTextAlignment(TEXT_ALIGN_LEFT);
    Display_drawString(28,41, "JX:" + String(JoyX));
    Any$ = "0"; if (CZ & 2) {Any$ = "1";}
    if (CZ & 1) {Any$ += "1";} else {Any$ += "0";}
    Display_drawString(28,52, "CZ:" + Any$);
    Display_drawString(73,41, "JY:" + String(JoyY));
    Display_drawString(73,52, " D: -");
  }

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 3; // update every 160ms
}

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

void Display_Mirrored() {
  // display robot type when DispMon is first connected
  // we clear current flags so that this is displayed immediately
  DispClr = false; DispDisp = false; DispCnt = 1;
  
  PrintTx+= "\n";       // terminate any half-sent command
  Display_clear();      // remove current display content
  if (Brightness != 255) {Display_setBrightness(255);}  // display may be in sleep mode
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  // 111111111111111111111111 2222222222222222           3333333333
  Display_setFont(24); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 0, "Uni-Bot");
  Display_setFont(10);
  Display_drawString(64,25, "vR1");
  Display_drawString(64,51, "Display Mirror");

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();    // display immediately
  DispDel = 50;         // wait 2s before displaying next count
  DispMode = DispNext;  // drop back to the default battery display
}

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

void Display_MPU_Acc() {
  // display MPU6050 accelerometer information
  DM_MPU_Acc = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //    1111111111      2222222222      3333333333      4444444444
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 3, "MPU Acc.");

  Display_setTextAlignment(TEXT_ALIGN_LEFT); Display_setColor(BLUE);
  if (AccOffEn) {Display_drawString(88, 3, "-");}
           else {Display_drawString(86, 3, "R");}

  if (AccOffEn) {Display_setColor(WHITE);} else {Display_setColor(YELLOW);}
  Display_drawString( 2,19, "AcX");
  if (AccOffEn) {Display_drawString(26,19, String(AcX));} else {Display_drawString(26,19, String(AcX + AccOffX));}
  if (MainMode == 1) {
    Display_drawString(70,19, "AvX");
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    if (AccOffEn) {Display_drawString(125,19, String(AcXAvgL));} else {Display_drawString(125,19, String(AcXAvgL + AccOffX));}
  }

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString( 2,35, "AcY");
  if (AccOffEn) {Display_drawString(26,35, String(AcY));} else {Display_drawString(26,35, String(AcY + AccOffY));}
  if (MainMode == 1) {
    Display_drawString(70,35, "AvY");
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    if (AccOffEn) {Display_drawString(125, 35, String(AcYAvgL));} else {Display_drawString(125, 35, String(AcYAvgL + AccOffY));}
  }

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString( 2,51, "AcZ");
  if (AccOffEn) {Display_drawString(26,51, String(AcZ));} else {Display_drawString(26,51, String(AcZ + AccOffZ));}
  if (MainMode == 1) {
    Display_drawString(70,51, "AvZ");
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    if (AccOffEn) {Display_drawString(125,51, String(AcZAvgL));} else {Display_drawString(125,51, String(AcZAvgL + AccOffZ));}
  }

    if (MainMode) {Display_setColor(WHITE); Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void Display_MPU_AccRamps() {
  // display MPU6050 accelerometer ramp rate limits
  DM_MPU_AccRamps = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //    1111111111      2222222222      3333333333      4444444444
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 3, "Acc Ramps");

  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(62,19, "AccRampX");
  Display_drawString(62,35, "AccRampY");
  Display_drawString(62,51, "AccRampZ");

  Display_setColor(BLUE);
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (AccRampX > 0) {Display_drawString(66,19, String(getNum00(AccRampX)));} else {Display_drawString(66,19, "Off");}
  if (AccRampY > 0) {Display_drawString(66,35, String(getNum00(AccRampY)));} else {Display_drawString(66,35, "Off");}
  if (AccRampZ > 0) {Display_drawString(66,51, String(getNum00(AccRampZ)));} else {Display_drawString(66,51, "Off");}

  if (MainMode) {Display_setColor(WHITE); Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void Display_MPU_Ang() {
  // displays the MPU angles
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //    1111111111      2222222222      3333333333      4444444444
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 3, "MPU Acc Ang.");

  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(62,19, "Pitch");
  Display_drawString(62,35, "Roll");
  Display_drawString(62,51, "Yaw");

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(66,19, String(PtchAccAng) + Deg);
  Display_drawString(66,35, String(RollAccAng) + Deg);
  Display_drawString(66,51, String(YawAccAng) + Deg);

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void Display_MPU_Gyro() {
  // display MPU6050 gyro information
  DM_MPU_Gyros = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //    1111111111      2222222222      3333333333      4444444444
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 3, "MPU Gyros");

  Display_setTextAlignment(TEXT_ALIGN_LEFT); Display_setColor(BLUE);
  if (GyrOffEn) {Display_drawString(92, 3, "-");}
           else {Display_drawString(90, 3, "R");}

  if (GyrOffEn) {Display_setColor(WHITE);} else {Display_setColor(YELLOW);}
  Display_drawString( 2,19, "GyX");
  if (GyrOffEn) {Display_drawString(26,19, String(GyX));} else {Display_drawString(26,19, String(GyX + GyrOffX));}
  if (MainMode == 1) {
    Display_drawString(70,19, "GvX");
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    if (GyrOffEn) {Display_drawString(125,19, String(GyXAvgL));} else {Display_drawString(125,19, String(GyXAvgL + GyrOffX));}
  }

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString( 2,35, "GyY");
  if (GyrOffEn) {Display_drawString(26,35, String(GyY));} else {Display_drawString(26,35, String(GyY + GyrOffY));}
  if (MainMode == 1) {
    Display_drawString(70,35, "GvY");
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    if (GyrOffEn) {Display_drawString(125,35, String(GyYAvgL));} else {Display_drawString(125,35, String(GyYAvgL + GyrOffY));}
  }

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString( 2,51, "GyZ");
  if (GyrOffEn) {Display_drawString(26,51, String(GyZ));} else {Display_drawString(26,51, String(GyZ + GyrOffZ));}
  if (MainMode == 1) {
    Display_drawString(70,51, "GvZ");
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    if (GyrOffEn) {Display_drawString(125,51, String(GyZAvgL));} else {Display_drawString(125,51, String(GyZAvgL + GyrOffZ));}
  }

  Display_setColor(WHITE);
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void Display_MPU_Levels() {
  // Display MPU6050 Pitch and Roll angles like spirit level
  // PItch is drawn in yellow, roll in red
  Display_clear();
  Display_setFont(10);

  if ((fabs(PtchGyrAng) <= 2.00) && (fabs(RollGyrAng) <= 2.00)) {
    // angles are <= 2.0°
    // Y64 vertical text spacing, from Y=0 to Y=63
    // 0123456789012345678901234567890123456789012345678901234567890123
    // 1111111111                 2222222222
    Display_setTextAlignment(TEXT_ALIGN_LEFT);
    Display_setColor(YELLOW);
    if (PtchGyrAng > 0.00) {
      Display_drawString(68, 0, String(PtchGyrAng,1) + Deg);
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(60, 0, "B");
    } else {
      Display_drawString(68,53, String(PtchGyrAng,1) + Deg);
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(60,53, "F");
    }
    Display_drawLine(64,0,64,63);     // vertical centre line
    DispCY = 30 - (16 * PtchGyrAng);  // Pitch bubble
    Display_drawRect(62,DispCY,4,4);  // vertical bubble
    Display_setTextAlignment(TEXT_ALIGN_LEFT);

    Display_setColor(RED);
    if (RollGyrAng > 0.00) {
      Display_drawString(100,26, String(RollGyrAng,1) + Deg);
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(28,26, "L");
    } else {
      Display_drawString(100,26, "R");
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(28,26, String(RollGyrAng,1) + Deg);
    }
    Display_drawLine(32,32,96,32);    // horizontal centre line
    DispCX = 62 + (16 * RollGyrAng);  // Roll bubble
    Display_drawRect(DispCX,30,4,4);  // horizontal bubble
  } else {
    // angles are >= 2.0°
    Display_setColor(YELLOW);
    if (PtchGyrAng >= 0.00) {
      // pitching backwards
      DispCX = 2 * fabs(PtchGyrAng); DispCY = 1; 
      Display_drawLine(64 - DispCX,DispCY,64 + DispCX,DispCY);  // vertical wedge up
      Display_drawLine(64,64,64 - DispCX,DispCY);
      Display_drawLine(64,64,64 + DispCX,DispCY);
      if (RollGyrAng < 0.00) {
        // rolling right
        Display_setTextAlignment(TEXT_ALIGN_LEFT);
        Display_drawString(70,53, String(PtchGyrAng,1) + Deg);
        Display_setTextAlignment(TEXT_ALIGN_RIGHT);
        Display_drawString(58,53, "B");
      } else {
        // rolling left
        Display_setTextAlignment(TEXT_ALIGN_RIGHT);
        Display_drawString(58,53, String(PtchGyrAng,1) + Deg);
        Display_setTextAlignment(TEXT_ALIGN_LEFT);
        Display_drawString(70,53, "B");
      }
    } else {
      // pitching forwards
      DispCX = 2 * fabs(PtchGyrAng); DispCY = 63; 
      Display_drawLine(64 - DispCX,DispCY,64 + DispCX,DispCY);  // vertical wedge down
      Display_drawLine(64,0,64 - DispCX,DispCY);
      Display_drawLine(64,0,64 + DispCX,DispCY);
      if (RollGyrAng < 0.00) {
        // rolling right
        Display_setTextAlignment(TEXT_ALIGN_LEFT);
        Display_drawString(70, 0, String(PtchGyrAng,1) + Deg);
        Display_setTextAlignment(TEXT_ALIGN_RIGHT);
        Display_drawString(58, 0, "F");
      } else {
        // rolling left
        Display_setTextAlignment(TEXT_ALIGN_RIGHT);
        Display_drawString(58, 0, String(PtchGyrAng,1) + Deg);
        Display_setTextAlignment(TEXT_ALIGN_LEFT);
        Display_drawString(70, 0, "F");
      }
    }
    Display_setColor(RED);
    if (RollGyrAng >= 0.00) {
      // rolling left
      DispCX = 96; DispCY = 2 * fabs(RollGyrAng);
      Display_drawLine(DispCX,32 - DispCY,DispCX,32 + DispCY);  // horizontal wedge right
      Display_drawLine(32,32,DispCX,32 - DispCY);
      Display_drawLine(32,32,DispCX,32 + DispCY);
      Display_setTextAlignment(TEXT_ALIGN_LEFT);
      Display_drawString(DispCX + 4,26, String(RollGyrAng,1) + Deg);
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(28,26, "L");
    } else {
      // rolling right
      DispCX = 32; DispCY = 2 * fabs(RollGyrAng);
      Display_drawLine(DispCX,32 - DispCY,DispCX,32 + DispCY);  // horizontal wedge left
      Display_drawLine(96,32,DispCX,32 - DispCY);
      Display_drawLine(96,32,DispCX,32 + DispCY);
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(DispCX - 4,26, String(RollGyrAng,1) + Deg);
      Display_setTextAlignment(TEXT_ALIGN_LEFT);
      Display_drawString(100,26, "R");
    }
  }
  Display_setColor(WHITE);
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void DisplayMouseClick() {
  // called whenever a mouse click is received from the display monitor app
  if (DispX >= 50) {DispMode++;} else {DispMode--;}
  if (DispMode < 0) {DispMode = 6;}
  if (DispMode > 6) {DispMode = 0;}
  DispClr = false; DispDisp = false; DispCnt = 1; DispDel = 1;
}

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

void Display_PID_Ang() {
  // displays the MPU angles
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //    1111111111      2222222222      3333333333      4444444444
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 3, "PID Ang.");

  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(30,19, "P.Acc");
  Display_drawString(30,35, "P.Gyr");
  Display_drawString(30,51, "Diff");
  Display_drawString(94,19, "R.Acc");
  Display_drawString(94,35, "R.Gyr");
  Display_drawString(94,51, "Diff");

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(34,19, String(PtchAccAng) + Deg);
  Display_drawString(34,35, String(PtchGyrAng) + Deg);
  Display_drawString(34,51, String(PtchGyrAng - PtchAccAng) + Deg);
  Display_drawString(98,19, String(RollAccAng) + Deg);
  Display_drawString(98,35, String(RollGyrAng) + Deg);
  Display_drawString(98,51, String(RollGyrAng - RollAccAng) + Deg);

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void Display_PIDoverun() {
  // called if the PID loop runs out of time, over-riding current display
  // we need a timer here to prevent this being called too often
  if ((millis() - DispOverun) < 5000)  {return;}

  DispOverun = millis();  // reset the 5 sec timer
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //                1111111111111111    2222222222222222
  Display_setFont(16); Display_setColor(RED);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64,15, "PID loop");
  Display_drawString(64,35, "Over-run");
  Display_drawRect(0,0,128,64);   // draw box outline

  Display_setColor(WHITE);
  Display_display();
  DispDel = 75; // hold off display update for 3sec
}

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

void Display_PtchPID() {
  // display Pitch PID variables
  DM_PtchPID = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111  2222222222  3333333333  4444444444  5555555555
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(78,2, "Pitch PID");
  Display_drawString(60,14, "P-gain");
  Display_drawString(60,26, "I-gain");
  Display_drawString(60,38, "D-gain");
  Display_drawString(60,50, "StartAng");

  if (PIDtask > 0) {Display_setColor(GREEN); Display_drawString(127,50, String(runSpaceRec));}

  if (PtchGo) {
    Display_setTextAlignment(TEXT_ALIGN_RIGHT); Display_setColor(BLUE);
    Display_drawString(125, 2, "S"); Display_setColor(WHITE);
  }

  if (!USB) {Display_setColor(BLUE);} else {Display_setColor(RED);}
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (PtchStartAng >= 0.0) {Display_drawString(64,50, "+");} else {Display_drawString(66,50, "-");}
  if (Ptchen) {Display_drawString(80, 2, String(PtchSafeMode));}
  else {Display_drawString(80, 2, "OFF");}

  Display_setColor(BLUE);
  Any$ = ""; if (Ptch_p_gain < 10.00) {Any$ = "0";}
  Display_drawString(64,14, Any$ + String(Ptch_p_gain,2));
  if (Ptch_i_En) {
    Any$ = ""; if (Ptch_i_gain < 10.00) {Any$ = "0";}
    Display_drawString(64,26, Any$ + String(Ptch_i_gain,2));
    Any$ = ""; if (Ptch_i_max < 10.0) {Any$ = "0";}
    Display_drawString(100,26, Any$ + String(Ptch_i_max,2));
  } else {Display_drawString(64,26, "---.---");}
  if (Ptch_d_En) {
    Any$ = ""; if (Ptch_d_gain < 10.00) {Any$ = "0";}
    Display_drawString(64,38, Any$ + String(Ptch_d_gain,2));}
  else {Display_drawString(64,38, "---.---");}
  Display_drawString(70,50, String(fabs(PtchStartAng),2) + Deg);

  Display_setColor(WHITE);
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 6; // update every 160ms
}

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

void Display_PtchSafeMode() {
  // display Pitch SafeMode + data
  DM_PtchSM = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111  2222222222  3333333333  4444444444  5555555555
  Display_setFont(10);
  if (!USB) {Display_setColor(BLUE);} else {Display_setColor(RED);}
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (Ptchen) {Display_drawString(83, 2, String(PtchSafeMode));}
  else {Display_drawString(83, 2, "OFF");}

  Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(81, 2, "Ptch SafeMode");
  Display_drawString(60,14, "PtchGyrAng");
  Display_drawString(60,26, "SbPtchSp");
  Display_drawString(60,38, "PtchErr");
  Display_drawString(60,50, "Ptch_Op");

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(64,14, String(PtchGyrAng) + Deg);
  Display_drawString(64,26, String(SbPtchSp) + Deg);
  Display_drawString(64,38, String(PtchErr));
  Display_drawString(64,50, String(Ptch_Op));
  Display_drawString(97,50, String(motorC_Vel));

  Display_setColor(BLUE);
    if (!PtchSb) {Display_drawString(97,26, "OFF");}
  else {Display_drawString(97,26, String(SbPtchSpInc,3));}


  Display_setColor(WHITE);
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void Display_RollPID() {
  // display Roll PID variables
  DM_RollPID = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111  2222222222  3333333333  4444444444  5555555555
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(75, 2, "Roll PID");
  Display_drawString(60,14, "P-gain");
  Display_drawString(60,26, "I-gain");
  Display_drawString(60,38, "D-gain");
  Display_drawString(60,50, "StartAng");
  if (PIDtask > 0) {Display_setColor(GREEN); Display_drawString(127,50, String(runSpaceRec));}

  if (!USB) {Display_setColor(BLUE);} else {Display_setColor(RED);}
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (RollStartAng >= 0.0) {Display_drawString(64,50, "+");} else {Display_drawString(66,50, "-");}
  if (Rollen) {Display_drawString(77, 2, String(RollSafeMode));}
  else {Display_drawString(77, 2, "OFF");}

  Display_setColor(BLUE);
  Any$ = ""; if (Roll_p_gain < 10.00) {Any$ = "0";}
  Display_drawString(64,14, Any$ + String(Roll_p_gain,2));
  if (Roll_i_En) {
    Any$ = ""; if (Roll_i_gain < 10.00) {Any$ = "0";}
    Display_drawString( 64,26, Any$ + String(Roll_i_gain,2));
    Any$ = ""; if (Roll_i_max < 10.0) {Any$ = "0";}
    Display_drawString(100,26, Any$ + String(Roll_i_max,2));
  } else {Display_drawString(64,26, "---.---");}
  if (Roll_d_En) {
    Any$ = ""; if (Roll_d_gain < 10.00) {Any$ = "0";}
    Display_drawString(64,38, Any$ + String(Roll_d_gain,2));}
  else {Display_drawString(64,38, "---.---");}
  Display_drawString(70,50, String(fabs(RollStartAng),2) + Deg);

  Display_setColor(WHITE);
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 6; // update every 160ms
}

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

void Display_RollSafeMode() {
  // display SafeMode + data
  DM_RollSM = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111  2222222222  3333333333  4444444444  5555555555
  Display_setFont(10);
  if (!USB) {Display_setColor(BLUE);} else {Display_setColor(RED);}
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (Rollen) {Display_drawString(83,2, String(RollSafeMode));}
  else {Display_drawString(83,2, "OFF");}

  if (USB) {Display_setColor(BLUE);}
  if (!RollSb) {Display_drawString(97,26, "OFF");}
  else {Display_drawString(97,26, String(SbRollSpInc,3));}
  Display_drawString(103,50, String(motorA_VdMax));

  Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(81, 2, "Roll SafeMode");
  Display_drawString(60,14, "RollGyrAng");
  Display_drawString(60,26, "SbRollSp");
  Display_drawString(60,38, "RollErr");
  Display_drawString(60,50, "motorVel");

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(64,14, String(RollGyrAng) + Deg);
  Display_drawString(64,26, String(SbRollSp) + Deg);
  Display_drawString(64,38, String(RollErr));
  Display_drawString(64,50, String(motorA_Vel));
  Display_drawString(90,50, "Vd");

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 160ms
}

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

void Display_T0_T1() {
  // display t1-t0 timings
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111    2222222222    3333333333    4444444444
  Display_setFont(10); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 2, "t0 - t1");

  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(72,16, "motor.move()");
  Display_drawString(72,30, "readAccAll()");

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(76,16, String(t1));
  Display_drawString(76,30, String(t3));

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 3; // update every 120ms
}

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

void Display_Text_2x16(String zT0$,String zT1$,int16_t zD) {
  // display T0$ and T1$ as two lines of text, for zD x 40ms
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //             1111111111111111    2222222222222222
  Display_setFont(16); Display_setColor(WHITE);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64,12, zT0$);
  Display_drawString(64,32, zT1$);

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = zD; // display for zD x 40ms
}

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

void Display_Topple() {
  // display turn demans and acceleration limits
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //           1111111111111111           2222222222
  Display_setFont(16); Display_setColor(RED);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  switch (random(9)) {
    case 0: Display_drawString(64,10, "Ouch!"); break;
    case 1: Display_drawString(64,10, "Oh no!"); break;
    case 2: Display_drawString(64,10, "Oops!"); break;
    case 3: Display_drawString(64,10, "Crash!"); break;
    case 4: Display_drawString(64,10, "Aah!"); break;
    case 5: Display_drawString(64,10, "Wham!"); break;
    case 6: Display_drawString(64,10, "Smash!"); break;
    case 7: Display_drawString(64,10, "Ow!"); break;
    case 8: Display_drawString(64,10, "Wallop!"); break;
  }
  Display_setFont(10);
  switch (random(10)) {
    case 0: Display_drawString(64,37, "That hurt"); break;
    case 1: Display_drawString(64,37, "Hurt my wheels"); break;
    case 2: Display_drawString(64,37, "Try again"); break;
    case 3: Display_drawString(64,37, "Steady on"); break;
    case 4: Display_drawString(64,37, "Call a mechanic"); break;
    case 5: Display_drawString(64,37, "Stop it"); break;
    case 6: Display_drawString(64,37, "So painful"); break;
    case 7: Display_drawString(64,37, "Take more care"); break;
    case 8: Display_drawString(64,37, "Who's driving?"); break;
    case 9: Display_drawString(64,37, "Pick me up"); break;
  }
  Display_display();
  DispDel = 75; // display for 3 seconds
}

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

void Display_TurnCtrl() {
  // display turn demans and acceleration limits
  DM_TurnCtrl = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //    1111111111     2222222222 3333333333 4444444444 5555555555
  Display_setFont(10);
  if (USB) {Display_setColor(RED);} else {Display_setColor(WHITE);}
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 3, "Turn Control");

  if (USB) {Display_setColor(WHITE);}
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(62,18, "JoyX");
  if (!USB && (JoyX != 128) && !(PtchGo && RollGo)) {Display_drawString(62,29, "Motor Vel");}
  else {Display_drawString(62,29, "Min Acc");}
  Display_drawString(62,40, "Max Acc");
  Display_drawString(62,51, "Turn");

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (RxClass || RxNunch) {Display_drawString(66,18, String(JoyX));}
  else {Display_drawString(66,18, "---");}
  Display_setColor(BLUE);
  Display_drawString(66,40, String(TurnMaxVd,3));
  if (USB || (JoyX == 128) || (PtchGo && RollGo)) {Display_drawString(66,29, String(TurnMinVd,3));}
  else {Display_setColor(WHITE); Display_drawString(66,29, String(motorB_Vel,2));}
  Display_setColor(WHITE);
  if (Turning) {Display_drawString(66,51, String(YawGyrAng,1) + Deg);}
  else {Display_drawString(66,51, "---");}

  if (PtchGo && RollGo && WiFiEn) {
    Display_setTextAlignment(TEXT_ALIGN_RIGHT); Display_setColor(GREEN);
    Display_drawString(124,51, String(TurnTask));
    Display_setColor(WHITE);
  }

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 5; // update every 200ms
}

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

void Display_Velocity() {
  // display the motor velocity and allow user to change it with mouse clicks
  DM_Velocity = DispMode; // note Display Mode assignment
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111    2222222222    3333333333    4444444444
  Display_setFont(10); 
  if (!USB) {Display_setColor(BLUE);} else {Display_setColor(RED);}
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  String zT$ = "-";
  if (VelTest) {
    switch (motorPnt) {
      case 0: zT$ = "A"; break;
      case 1: zT$ = "B"; break;
      case 2: zT$ = "C"; break;
    }
  }
  Display_drawString(64, 2, "Velocity " + zT$);

  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_setColor(WHITE);
  Display_drawString(60,16, "Motor Vel");
  Display_drawString(60,30, "Freq");
  Display_drawString(60,44, "Voltage");

  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  if (VelTest) {
    Display_setColor(BLUE);
    switch (motorPnt) {
      case -1:
        Display_drawString(69,16, " --.--"); break;
      case 0:
        if (motorA_Vel < 0.0) {Display_drawString(65, 16, "-");} else {Display_drawString(63, 16, "+");}
        zT$ = "0"; if (fabs(motorA_Vel) > 9.99) {zT$ = "";}
        Display_drawString(69,16, zT$ + String(fabs(motorA_Vel))); break;
      case 1:
        if (motorB_Vel < 0.0) {Display_drawString(65, 16, "-");} else {Display_drawString(63, 16, "+");}
        zT$ = "0"; if (fabs(motorB_Vel) > 9.99) {zT$ = "";}
        Display_drawString(69,16, zT$ + String(fabs(motorB_Vel))); break;
      case 2:
        if (motorC_Vel < 0.0) {Display_drawString(65, 16, "-");} else {Display_drawString(63, 16, "+");}
        zT$ = "0"; if (fabs(motorC_Vel) > 9.99) {zT$ = "";}
        Display_drawString(69,16, zT$ + String(fabs(motorC_Vel))); break;
    }
    if (Motor_Freq == 999) {Display_drawString(64, 30, "Max");}
    else {Display_drawString(64, 30, String(Motor_Freq) + "Hz");}
    switch (motorPnt) {
      case -1: Display_drawString(64,44, "--.--v"); break;
      case  0: Display_drawString(64,44, String(motorAVol) + "v"); break;
      case  1: Display_drawString(64,44, String(motorBVol) + "v"); break;
      case  2: Display_drawString(64,44, String(motorCVol) + "v"); break;
    }
  } else {
    Display_drawString(69,16, " --.--");
    if (Motor_Freq == 999) {Display_drawString(64, 30, "Max");}
    else {Display_drawString(64, 30, String(Motor_Freq) + "Hz");}
    Display_drawString(64,44, "--.--v");
  }
  Display_setColor(WHITE);
  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 3; // update every 120ms
}

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

void Display_Warning() {
  // display overspeed warning
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //           1111111111111111           2222222222
  Display_setFont(16); Display_setColor(RED);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64,10, "WARNING!");
  Display_setFont(10);
  Display_drawString(64,37, "Overspeed");
  Display_display();
  DispDel = 75; // update every 200ms

}

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

void Display_Wii_Ctrl() {
  // display 6 bytes received from Wii over ESP-NOW
  // display I2C values as decimal 0 - 255
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111    2222222222    3333333333    4444444444
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_setFont(10); DispCX = 58;
  Display_drawString(DispCX,  0, String(RxWiFi[0]));
  Display_drawString(DispCX, 10, String(RxWiFi[1]));
  Display_drawString(DispCX, 20, String(RxWiFi[2]));
  Display_drawString(DispCX, 30, String(RxWiFi[3]));
  Display_drawString(DispCX, 40, String(RxWiFi[4]));
  Display_drawString(DispCX, 50, String(RxWiFi[5]));
  // display I2C values as binary 00000000 - 11111111
  Display_setTextAlignment(TEXT_ALIGN_RIGHT); DispCX = 126;
  Display_drawString(DispCX,  0, GetBIN(RxWiFi[0]));
  Display_drawString(DispCX, 10, GetBIN(RxWiFi[1]));
  Display_drawString(DispCX, 20, GetBIN(RxWiFi[2]));
  Display_drawString(DispCX, 30, GetBIN(RxWiFi[3]));
  Display_drawString(DispCX, 40, GetBIN(RxWiFi[4]));
  Display_drawString(DispCX, 50, GetBIN(RxWiFi[5]));
  // display 'I2C' in top left corner
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(2, 0, "Wii");
       if (WiiType == 'C') {Display_drawString(2, 10, "Classic");}
  else if (WiiType == 'N') {Display_drawString(2, 10, "Nunchuk");}

  if (MainMode) {Display_drawRect(0,0,128,64);} // draw 'Test' box outline
  Display_display();
  DispDel = 4; // update every 120ms
}

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

