//^includes
  #include <LiquidCrystal_I2C.h>
  #include <ButtonDebounce.h>
  //#define ENCODER_OPTIMIZE_INTERRUPTS  // this breaks attach interrupt for limit and emerg stop
  #include <Encoder.h>
  #include <FastAccelStepper.h>
//^defines
  #define LCD_SCL_Pin 21
  #define LCD_SDA_Pin 20

  #define EncoderL_CLK_Pin 18
  #define EncoderL_DT_Pin 31
  #define EncoderL_SW_Pin 32
  #define EncoderR_CLK_Pin 19
  #define EncoderR_DT_Pin 46
  #define EncoderR_SW_Pin 47

  #define SlidePotA_Pin A11
  #define SlidePotB_Pin A12
  #define RotaryPotA_Pin A13

  #define MotDrv_Disable_Pin 9
  #define MotDrv_Step_Pin 8
  #define MotDrv_Direction_Pin 7

  #define AnalogHoriz_Pin A15
  #define AnalogVert_Pin A14
  #define AnalogSW_Pin 41
  #define Nunchuck_Btn1_Pin 35
  #define Nunchuck_Btn2_Pin 40

  #define DIP1_Pin 22
  #define DIP2_Pin 23
  #define DIP3_Pin 24
  #define DIP4_Pin 25
  #define DIP5_Pin 26
  #define DIP6_Pin 27
  #define DIP7_Pin 28
  #define DIP8_Pin 29

  #define RunBtn_Pin 42
  #define RunBtnLED_Pin 43

  #define MotorEnableLED_Pin 45
  #define MotorDirectionLED_Pin 44
  #define RunningLED_Pin 34
  #define FaultLED_Pin 33
  #define Limit1_LED_Pin 11
  #define Limit2_LED_Pin 10

  #define Limit1_Pin 4
  #define Limit2_Pin 5
  #define Limit_Int_Pin 3
  #define EmergStop_Pin 2

  #define I2C_ADDR 0x27
  #define LCD_COLUMNS 20
  #define LCD_LINES 4


  LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);
//^Global Vars
  //~ Encoder Vars
    int EncoderL_Mapped;
    int EncoderR_Mapped;
    int RightEnc100;
    unsigned int EncoderL_Val;
    bool LeftEncoderBtn_State = false;
    unsigned int EncoderR_Val;
    bool RightEncoderBtn_State = false;

  //~ Analog Vars
    int SlidePotA_Val = 0;
    int SlidePotA_Map;
    int SlidePotB_Val = 0;
    int SlidePotB_Map;
    int RotaryPot_Val = 0;
    int RotaryPot_Map;

    int AnalogHoriz_Val = 0;  //Nunchuck
    int AnalogHoriz_Map;  //Nunchuck
    int AnalogVert_Val = 0;  //Nunchuck
    int AnalogVert_Map;  //Nunchuck


  //~Buttons and Switches
    bool AnalogSW_State = false;  //Nunchuck
    bool NunchuckBtn1_State = false;  //Nunchuck
    bool NunchuckBtn2_State = false;  //Nunchuck
    bool RunBtn_State = false;
    bool DIP_States[8] = {false,false,false,false,false,false,false,false};
    volatile bool Limit1_State = false;
    volatile bool Limit2_State = false;
    volatile bool EmergStop_State = false;

  //~LED States
    volatile bool RunBtnLED_State = false;
    volatile bool MotorEnableLED_State = false;
    volatile bool MotorDirectionLED_State = false;
    volatile bool RunningLED_State = false;
    volatile bool FaultLED_State = false;
    volatile bool Limit1LED_State = false;
    volatile bool Limit2LED_State = false;

  //~Params
    int FwdRunSpeed = 50;
    int RevRunSpeed = 50; 
    int FwdAccel = 50;
    int FwdDecel = 50;
    int RevAccel = 50;
    int RevDecel = 50;
    int FwdDwell = 50;
    int RevDwell = 50;
    int FwdRunLength = 0;
    int RevRunLength = 0;
    int FwdLimit = 0;
    int RevLimit = 0;
    //int Center = 0;
    int CenterOffset = 0;
    int ManualFwd = 0;
    int ManualRev = 0;
    int ManualLeft = 0;
    int ManualRight = 0;

  //~Setup and Build
    bool UseAutoOff = false;   //should be false
    int AutoOffTarget = 0;  //should be 0
    bool UsePresets = true;  //should be true
    bool AutoIncPresets = false;  //  should be false
    int SelectedPreset = 0;  // should be 0
    int AutoIncTarget = 50;  
    bool SetRuntimeParams = false;  //should fe false

    int MenuLevel = 1;  //should be 0, 1 to skip splash
    int MenuStep = 0;  //should be 0
    int Selector = 1; //should be 1
    int RunScreen = 1;  //should be 1
    int GlobalFlag = 1;  //should be 0 to start. 0 is Homing, 1 is in the setup menu, 2 is Manual Runtime, 3 is Params Runtime, 4 is Presets Runtime, 5 is Run Complete.

  //~Runtime Stuff
    int RealCycleCount = 0;  //should be 0
    int RealCycleCountOld;
    int ManualPositionOld;
    int ManualThreshold;
    unsigned long CurrentMillis = millis();
    const long RunDispInterval = 3000; 
    unsigned long PreviousMillis = 0; 
    unsigned long PrevDwellMillis = 0;

    bool MotDrvDisable_State = true;
    volatile bool ErrorState = false;
    long CurrentPosition;
    int RuntimeFlag = 0;
    int RunDirection = 0;
//^Buttons
  ButtonDebounce NextBtn(EncoderR_SW_Pin, 100);
  ButtonDebounce BackBtn(EncoderL_SW_Pin, 100);
  ButtonDebounce RunBtn(RunBtn_Pin, 100);
  //ButtonDebounce StopBtn(EmergStop_Pin, 100);
  ButtonDebounce NunBtn1(Nunchuck_Btn1_Pin, 100);
  ButtonDebounce NunBtn2(Nunchuck_Btn2_Pin, 100);
  ButtonDebounce NunAnaBtn(AnalogSW_Pin, 100);
//^Encoders
  Encoder EncRight(19, 46);
  Encoder EncLeft(18, 31);
//^Stepper
  FastAccelStepperEngine engine = FastAccelStepperEngine();
  FastAccelStepper *Motor = NULL;

//! ~~~~NOTES~~~~
  //todo  Make encoders loop to min at max+1 
  //todo  Rewire emerg stop and limit switches to have isolated grounds
  //todo  Find out How to change LCD Text to Progmem!
  //todo  Write Preset Chooser in setup menu
  //todo  Write Preset Display
  //todo  Write Preset Runtime
  //todo  Revisit reset function
  //~     Remember to set global menu var values to normal
  //#                 To ADD?
  //# Pause Runtime?

  /*   Some Motor Commands hide here
  isRunning();
  getCurrentPosition();
  getSpeedInUs()
  getSpeedInTicks()
  getSpeedInMilliHz()
  setAcceleration()
  getAcceleration()
  getCurrentAcceleration()
  stopMove();
  runForward();
  */
//! ~~~~NOTES~~~~  

void setup(){
  lcd.init();
  lcd.backlight();
  Serial.begin(38400);
  //~Interrupts
    attachInterrupt(digitalPinToInterrupt(Limit_Int_Pin), CriticalStopISR, FALLING);
    attachInterrupt(digitalPinToInterrupt(EmergStop_Pin), CriticalStopISR,FALLING);
  //~ Stepper Setup
    engine.init();
    Motor = engine.stepperConnectToPin(MotDrv_Step_Pin);
    if (Motor) {
      Motor->setDirectionPin(MotDrv_Direction_Pin);
      //Motor->setEnablePin(MotDrv_Enable_Pin);
      Motor->setAutoEnable(true);
    }
    // 3000 steps = 15 rotations, timer output reports 1087 millis to complete == 900 RPM
    // with accel == 10000 and speed in hz 20000
  //~ Pin Setup
    pinMode(DIP1_Pin, INPUT_PULLUP);
    pinMode(DIP2_Pin, INPUT_PULLUP);
    pinMode(DIP3_Pin, INPUT_PULLUP);
    pinMode(DIP4_Pin, INPUT_PULLUP);
    pinMode(DIP5_Pin, INPUT_PULLUP);
    pinMode(DIP6_Pin, INPUT_PULLUP);
    pinMode(DIP7_Pin, INPUT_PULLUP);
    pinMode(DIP8_Pin, INPUT_PULLUP);

    pinMode(RunBtnLED_Pin, OUTPUT);
    pinMode(MotorEnableLED_Pin, OUTPUT);
    pinMode(MotorDirectionLED_Pin, OUTPUT);
    pinMode(RunningLED_Pin, OUTPUT);
    pinMode(FaultLED_Pin, OUTPUT);
    pinMode(Limit1_LED_Pin, OUTPUT);
    pinMode(Limit2_LED_Pin, OUTPUT);

    pinMode(Limit1_Pin, INPUT_PULLUP);
    pinMode(Limit2_Pin, INPUT_PULLUP);
    pinMode(EmergStop_Pin, INPUT_PULLUP);
    pinMode(Limit_Int_Pin, INPUT_PULLUP);

    pinMode(SlidePotA_Pin, INPUT);
    pinMode(SlidePotB_Pin, INPUT);
    pinMode(RotaryPotA_Pin, INPUT);

    pinMode(AnalogHoriz_Pin, INPUT);
    pinMode(AnalogVert_Pin, INPUT);
  digitalWrite(MotDrv_Disable_Pin, MotDrvDisable_State);
  ReadDip();
  DipDebug();
  //# for testing!!
    //UseAutoOff = false;   //should be false
    //AutoOffTarget = 0;  //should be 0
    //UsePresets = false;  //should be true
    //AutoIncPresets = false;  //  should be false
    //SelectedPreset = 0;  // should be 0
    //AutoIncTarget = 50;  
    //SetRuntimeParams = true;  //should fe false

    //MenuLevel = 100;  //should be 0, 1 to skip splash
    //MenuStep = 0;  //should be 0
    //Selector = 1; //should be 1
    //RunScreen = 20;  //should be 1
    //GlobalFlag = 3;
    //0 is Homing, 1 is in the setup menu, 2 is Manual Runtime, 
    //3 is Params Runtime, 4 is Presets Runtime, 5 and 6 are Run Complete.

    //MotDrvDisable_State = false;
    //digitalWrite(MotDrv_Disable_Pin, MotDrvDisable_State);

    FwdLimit = 1000;//! set by homing, this val is for testing
    RevLimit = 1000;//! set by homing, this val is for testing
} //@  Close Setup

void loop(){
  if(ErrorState == true){FaultHandler();}
  
  UpdateLED();
  //ReadButtons();
  if(GlobalFlag == 0){Homing();}
  if(GlobalFlag == 1){SetupMenu();}
  if(GlobalFlag == 2){ManualRuntime();}
  if(GlobalFlag == 3){ParamsRuntime();}
  if(GlobalFlag == 4){PresetRuntime();}
  if(GlobalFlag == 5 || GlobalFlag == 6){RunComplete();}
  if(GlobalFlag == 999){ThePlayground();}
} //@ close Loop

void SetupMenu(){ //!  Preset Chooser not done
  int EncLOld = EncoderL_Mapped;
  int EncROld = EncoderR_Mapped;
  int EncR100Old = EncROld;
  int EncL100Old = EncLOld;
  int SelectorOld = Selector;
  if(MenuLevel == 0){
    lcd.clear();
    lcd.setCursor(6, 0);
    lcd.print("WELCOME,");
    lcd.setCursor(7, 1);
    lcd.print("to the");
    lcd.setCursor(5, 3);
    lcd.print("F-BOMBER!!");
    delay(500);
    MenuLevel = 1;
  }// Close MenuLevel 0

  if(MenuLevel == 1){ //Auto Off Counter and Use Presets? Yes Presets->10. No Presets->20
    if(MenuStep == 0){//Draw auto off counter 
      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("Auto Off Count");
      lcd.setCursor(4, 1);
      lcd.print(AutoOffTarget);
      lcd.setCursor(8, 1);
      lcd.print(" (0=off)");
      MenuStep = 1;
    } //Close MenuLevel 1 MenuStep 0

    if(MenuStep == 1){//Auto off counter Logic 
      ReadButtons();
      ReadEncoders(false);
      if(RightEncoderBtn_State == true){
        if(AutoOffTarget != 0){UseAutoOff = true;}else{UseAutoOff = false;}
        lcd.setCursor(4, 2);
        lcd.print("Use Presets?");
        lcd.setCursor(8, 3);
        lcd.print("Yes");
        MenuStep = 2;
        delay(50);
      }
      if(AutoOffTarget != EncoderR_Mapped){
        AutoOffTarget = EncoderR_Mapped;
        lcd.setCursor(4, 1);
        lcd.print("     ");
        lcd.setCursor(4, 1);
        lcd.print(AutoOffTarget);
      }
    } //Close MenuLevel 1 MenuStep 1 auto off counter logic

    if(MenuStep == 2){  //Use Presets Y/N logic
      ReadButtons();
      ReadEncoders(false);
      if(EncoderR_Mapped > EncROld){ // Yes
        lcd.setCursor(0, 3);
        lcd.print("                    ");
        lcd.setCursor(8, 3);
        lcd.print("Yes");
        UsePresets = true;
      }

      if(EncoderR_Mapped < EncROld){ // No
        lcd.setCursor(0, 3);
        lcd.print("                    ");
        lcd.setCursor(8, 3);
        lcd.print("No");
        UsePresets = false;
      }

      if(RightEncoderBtn_State == true){
        MenuStep = 0;
        if(UsePresets == true){MenuLevel = 10;}
        if(UsePresets == false){MenuLevel = 20;}
        ReadEncoders(true);
        delay(50);
      }
    } //Close MenuLevel 1 MenuStep 2
  } //Close MenuLevel 1

  if(MenuLevel == 10){  //Yes Presets Init Dest.  Incriment Presets? Y/N
        
    if(MenuStep == 0){// Draw Inc Presets Screen
      lcd.clear();
      lcd.setCursor(1, 0);
      lcd.print("Auto Inc Count");
      lcd.setCursor(8, 3);
      lcd.print("(0=off)");
      MenuStep = 1;
    }  //Close MenuLevel 10 MenuStep 0

    if(MenuStep == 1){ // Auto Inc Counter Logic
      ReadButtons();
      ReadEncoders(false);
      if(RightEncoderBtn_State == true){
        if(AutoIncTarget != 0){AutoIncPresets = true;}else{AutoIncPresets = false;}
        MenuStep = 0;
        MenuLevel = 11;
      }

      if(AutoIncTarget != EncoderR_Mapped){
        AutoIncTarget = EncoderR_Mapped;
        lcd.setCursor(4, 3);
        lcd.print("     ");
        lcd.setCursor(4, 3);
        lcd.print(AutoIncTarget);
      }
    }  //Close MenuLevel 10 MenuStep 1
  }//Close MenuLevel 10

  if(MenuLevel == 11){ //!Preset Chooser Screen 1
    
    if(MenuStep == 0){
      lcd.clear();
      MenuStep = 1;
      }  // Close MenuLevel 11 MenuStep 0

    if(MenuStep == 1){   //Draw Presets to screen
      lcd.setCursor(1, 0);
      lcd.print("Preset 1");
      lcd.setCursor(0, 0);
      lcd.print(">");
      lcd.setCursor(1, 1);
      lcd.print("Preset 2");
      lcd.setCursor(1, 2);
      lcd.print("Preset 3");
      lcd.setCursor(1, 3);
      lcd.print("Preset 4");
      MenuStep = 2;
    }  // Close MenuLevel 11 MenuStep 1

    if(MenuStep == 2){  //Preset Selector Logic
      ReadButtons();
      ReadEncoders(false);
      if(RightEncoderBtn_State == true){
        SelectedPreset = Selector;
        MenuStep = 0;
        MenuLevel = 19;
      }
      if(EncR100Old > RightEnc100 || EncR100Old < RightEnc100){
        if(EncR100Old < RightEnc100){Selector = Selector+1;}
        if(EncR100Old > RightEnc100){Selector = Selector-1;}
        if(Selector <= 0){Selector = 1;}
        if(Selector != SelectorOld){
          switch(Selector){
            case 1:
              lcd.setCursor(0, 0);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(" ");
              lcd.setCursor(0, 0);
              lcd.print(">");
            break;

            case 2:
              lcd.setCursor(0, 0);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(">");
            break;

            case 3:
              lcd.setCursor(0, 0);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(">");
            break;

            case 4:
              lcd.setCursor(0, 0);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(">");
            break;

            case 5:
              MenuStep = 0;
              MenuLevel = 12;
            break;
          }  //close selector switch
        }// SelectorOld!=Selector IF
      } //close encoder vs encoder olf if
    }  // Close MenuLevel 11 MenuStep 2
  }// Close MenuLevel 11

  if(MenuLevel == 12){ //!Preset Chooser Screen 2
    ReadButtons();
    ReadEncoders(false);

    if(MenuStep == 0){
      lcd.clear();
      MenuStep = 1;
    }  //  Close MenuLevel 12 Menustep 0

    if(MenuStep == 1){
      lcd.setCursor(1, 0);
      lcd.print("Preset 5");
      lcd.setCursor(0, 0);
      lcd.print(">");
      lcd.setCursor(1, 1);
      lcd.print("Preset 6");
      lcd.setCursor(1, 2);
      lcd.print("Preset 7");
      lcd.setCursor(1, 3);
      lcd.print("Preset 8");
      MenuStep = 2;
    }   //  Close MenuLevel 12 menustep 1

    if(MenuStep == 2){
      ReadButtons();
      ReadEncoders(false);
      if(RightEncoderBtn_State == true){
        SelectedPreset = Selector;
        MenuStep = 0;
        MenuLevel = 19;
      }
      if(EncR100Old > RightEnc100 || EncR100Old < RightEnc100){
        if(EncR100Old < RightEnc100){Selector = Selector+1;}
        if(EncR100Old > RightEnc100){Selector = Selector-1;}
        if(Selector < 5){
          MenuLevel = 11;
          MenuStep = 0;
          Selector = 1;
          }

        if(Selector != SelectorOld){
          switch(Selector){
            case 5:
              lcd.setCursor(0, 0);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(" ");
              lcd.setCursor(0, 0);
              lcd.print(">");
            break;

            case 6:
              lcd.setCursor(0, 0);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(">");
            break;

            case 7:
              lcd.setCursor(0, 0);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(">");
            break;

            case 8:
              lcd.setCursor(0, 0);
              lcd.print(" ");
              lcd.setCursor(0, 1);
              lcd.print(" ");
              lcd.setCursor(0, 2);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(" ");
              lcd.setCursor(0, 3);
              lcd.print(">");
            break;

            case 9:
              Selector = 1;
              MenuStep = 0;
              MenuLevel = 11;
            break;
          }  //close selector switch
        }// SelectorOld!=Selector IF
      }//close encoder vs encoder olf if
    }  //  Close MenuLevel 12 menustep 2
  }  //  Close MenuLevel 12

  if(MenuLevel == 19){ // Presets Summary Screen
    //Serial.println("Enter case 19");
    if(MenuStep == 0){
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Chosen Preset");
      lcd.setCursor(0, 1);
      switch(SelectedPreset){
        case 1:
          lcd.print("Preset 1");
        break;

        case 2:
          lcd.print("Preset 2");
        break;

        case 3:
          lcd.print("Preset 3");
        break;

        case 4:
          lcd.print("Preset 4");
        break;

        case 5:
          lcd.print("Preset 5");
        break;

        case 6:
          lcd.print("Preset 6");
        break;

        case 7:
          lcd.print("Preset 7");
        break;

        case 8:
          lcd.print("Preset 8");
        break;
      }
      
      if(AutoIncPresets == true){
        lcd.setCursor(0, 2);
        lcd.print("Incriment Every ");
        lcd.setCursor(16, 2);
        lcd.print(AutoIncTarget);
      }else{
        lcd.setCursor(0, 2);
        lcd.print("Auto Inc is Off");
      }
      if(UseAutoOff == true){
        lcd.setCursor(0, 3);
        lcd.print("Auto Off at ");
        lcd.setCursor(12, 3);
        lcd.print(AutoOffTarget);
      }else{
        lcd.setCursor(0, 3);
        lcd.print("Auto Off Disabled");
      }
      MenuStep = 1;
    }
    if(MenuStep == 1){
      ReadButtons();
    //delay(2000);
    if(RightEncoderBtn_State == true){
      MenuStep = 0;
      MenuLevel = 99;
      }
    //Serial.println("end case 19");
    }
  }  // Close MenuLevel 19

  if(MenuLevel == 20){ // No Presets Init Dest. Set Runtime Params? Y/N  <> Yes Set Run->30 No Set run->40
    if(MenuStep == 0){  //draw set runtime scrn
      lcd.clear();
      lcd.setCursor(1, 0);
      lcd.print("Set Run Parameters");
      lcd.setCursor(8, 3);
      lcd.print("Yes");
      SetRuntimeParams = true;
      MenuStep = 1;
    }  // Close MenuLevel 20 MenuStep 0

    if(MenuStep == 1){ 
      ReadButtons();
      ReadEncoders(false);
      if(EncoderR_Mapped > EncROld){ // Yes
        lcd.setCursor(0, 3);
        lcd.print("                    ");
        lcd.setCursor(8, 3);
        lcd.print("Yes");
        SetRuntimeParams = true;
      }

      if(EncoderR_Mapped < EncROld){ // No
        lcd.setCursor(0, 3);
        lcd.print("                    ");
        lcd.setCursor(8, 3);
        lcd.print("No");
        SetRuntimeParams = false;
      }

      if(RightEncoderBtn_State == true){
        MenuStep = 0;
        if(SetRuntimeParams == true){MenuLevel = 30;} //runtime params
        if(SetRuntimeParams == false){MenuLevel = 40;}  // full manual
      }
    }  // Close MenuLevel 20 MenuStep 1
  }  // Close MenuLevel 20 MenuStep 

  if(MenuLevel == 30){ //Set Runtime Params Init Dest  and all forward settings
    
    if(MenuStep == 0){  // draw fwd run params screen
      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("Fwd Speed= ");
      lcd.setCursor(14, 0);
      lcd.print(FwdRunSpeed);

      lcd.setCursor(3, 1);
      lcd.print("Fwd Accel= ");
      lcd.setCursor(14, 1);
      lcd.print(FwdAccel);

      lcd.setCursor(3, 2);
      lcd.print("Fwd Decel= ");
      lcd.setCursor(14, 2);
      lcd.print(FwdDecel);

      lcd.setCursor(3, 3);
      lcd.print("Fwd Dwell= ");
      lcd.setCursor(14, 3);
      lcd.print(FwdDwell);
      ReadEncoders(true);
      MenuStep = 1;
    }  //  Close MenuLevel 30 MenuStep 0

    if(MenuStep == 1){
      ReadButtons();
      ReadEncoders(false);

      if(FwdRunSpeed != RightEnc100){
        FwdRunSpeed = RightEnc100;
        lcd.setCursor(14, 0);
        lcd.print("     ");
        lcd.setCursor(14, 0);
        lcd.print(FwdRunSpeed);
      }

      if(RightEncoderBtn_State == true){
        ReadEncoders(true);
        MenuStep = 2;
        delay(250);
        }
    }  //  Close MenuLevel 30 MenuStep 1

    if(MenuStep == 2){
      ReadButtons();
      ReadEncoders(false);

      if(FwdAccel != RightEnc100){
        FwdAccel = RightEnc100;
        lcd.setCursor(14, 1);
        lcd.print("     ");
        lcd.setCursor(14, 1);
        lcd.print(FwdAccel);
      }

      if(RightEncoderBtn_State == true){
        ReadEncoders(true);
        MenuStep = 3;
        delay(250);
        }
    }  //  Close MenuLevel 30 MenuStep 2

    if(MenuStep == 3){
      ReadButtons();
      ReadEncoders(false);

      if(FwdDecel != RightEnc100){
        FwdDecel = RightEnc100;
        lcd.setCursor(14, 2);
        lcd.print("     ");
        lcd.setCursor(14, 2);
        lcd.print(FwdDecel);
      }

      if(RightEncoderBtn_State == true){
        ReadEncoders(true);
        MenuStep = 4;
        delay(250);
        }
    }  //  Close MenuLevel 30 MenuStep 3

    if(MenuStep == 4){
      ReadButtons();
      ReadEncoders(false);

      if(FwdDwell != RightEnc100){
        FwdDwell = RightEnc100;
        lcd.setCursor(14, 3);
        lcd.print("     ");
        lcd.setCursor(14, 3);
        lcd.print(FwdDwell);
        delay(250);
      }

      if(RightEncoderBtn_State == true){
        ReadEncoders(true);
        MenuStep = 5;
        delay(250);
        }
    }  //  Close MenuLevel 30 MenuStep 4

    if(MenuStep == 5){
      ReadEncoders(true);
      MenuStep = 0;
      MenuLevel = 31;
    }  //  Close MenuLevel 30 MenuStep 5
  }  //  Close MenuLevel 30

  if(MenuLevel == 31){  // Set Runtime Params all reverse settings

    if(MenuStep == 0){  // draw Rev run params screen
      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("Rev Speed= ");
      lcd.setCursor(14, 0);
      lcd.print(RevRunSpeed);

      lcd.setCursor(3, 1);
      lcd.print("Rev Accel= ");
      lcd.setCursor(14, 1);
      lcd.print(RevAccel);

      lcd.setCursor(3, 2);
      lcd.print("Rev Decel= ");
      lcd.setCursor(14, 2);
      lcd.print(RevDecel);

      lcd.setCursor(3, 3);
      lcd.print("Rev Dwell= ");
      lcd.setCursor(14, 3);
      lcd.print(RevDwell);
      MenuStep = 1;
    }  //  Close MenuLevel 31 MenuStep 0

    if(MenuStep == 1){
      ReadButtons();
      ReadEncoders(false);

      if(RevRunSpeed != RightEnc100){
        RevRunSpeed = RightEnc100;
        lcd.setCursor(14, 0);
        lcd.print("     ");
        lcd.setCursor(14, 0);
        lcd.print(RevRunSpeed);
      }

      if(RightEncoderBtn_State == true){
        ReadEncoders(true);
        MenuStep = 2;
        delay(250);
        }
    }  //  Close MenuLevel 31 MenuStep 1

    if(MenuStep == 2){
      ReadButtons();
      ReadEncoders(false);

      if(RevAccel != RightEnc100){
        RevAccel = RightEnc100;
        lcd.setCursor(14, 1);
        lcd.print("     ");
        lcd.setCursor(14, 1);
        lcd.print(RevAccel);
      }

      if(RightEncoderBtn_State == true){
        ReadEncoders(true);
        MenuStep = 3;
        delay(250);
        }
    }  //  Close MenuLevel 31 MenuStep 2

    if(MenuStep == 3){
      ReadButtons();
      ReadEncoders(false);

      if(RevDecel != RightEnc100){
        RevDecel = RightEnc100;
        lcd.setCursor(14, 2);
        lcd.print("     ");
        lcd.setCursor(14, 2);
        lcd.print(RevDecel);
      }

      if(RightEncoderBtn_State == true){
        ReadEncoders(true);
        MenuStep = 4;
        delay(250);
        }
    }  //  Close MenuLevel 31 MenuStep 3

    if(MenuStep == 4){
      ReadButtons();
      ReadEncoders(false);

      if(RevDwell != RightEnc100){
        RevDwell = RightEnc100;
        lcd.setCursor(14, 3);
        lcd.print("     ");
        lcd.setCursor(14, 3);
        lcd.print(RevDwell);
        delay(250);
      }

      if(RightEncoderBtn_State == true){
        ReadEncoders(true);
        MenuStep = 5;
        delay(250);
        }
    }  //  Close MenuLevel 31 MenuStep 4

    if(MenuStep == 5){
      MenuStep = 0;
      MenuLevel = 39;
    }  //  Close MenuLevel 31 MenuStep 5 
  } //Close MenuLevel 31

  if(MenuLevel == 39){ //  Runtime Params Summary Screen
    ReadButtons();
    if(MenuStep == 0){
    lcd.clear();
    MenuStep = 1;
    }
    
    if(MenuStep == 1){
      lcd.setCursor(3, 0);
      lcd.print("Forward Info");
      lcd.setCursor(0, 1);
      lcd.print("Run Speed = ");
      lcd.print(FwdRunSpeed);
      lcd.setCursor(0, 2);
      lcd.print("Acc = ");
      lcd.print(FwdAccel);
      lcd.setCursor(11, 2);
      lcd.print("Dec = ");
      lcd.print(FwdDecel);
      lcd.setCursor(0, 3);
      lcd.print("Dwell = ");
      lcd.print(FwdDwell);
      //delay(100);
      MenuStep = 2;
    }

    if(MenuStep == 2){
      if(RightEncoderBtn_State == true){
        MenuStep = 3;
      }
    }

    if(MenuStep == 3){
      lcd.clear();
      lcd.setCursor(4, 0);
      lcd.print("Reverse Info");
      lcd.setCursor(0, 1);
      lcd.print("Run Speed = ");
      lcd.print(RevRunSpeed);
      lcd.setCursor(0, 2);
      lcd.print("Acc = ");
      lcd.print(RevAccel);
      lcd.setCursor(11, 2);
      lcd.print("Dec = ");
      lcd.print(RevDecel);
      lcd.setCursor(0, 3);
      lcd.print("Dwell = ");
      lcd.print(RevDwell);
      delay(2000);
      MenuStep = 4;
    }

    if(MenuStep == 4){
      //delay(2000);
      Serial.println(MenuStep);
      if(RightEncoderBtn_State == true){
        MenuStep = 0;
        MenuLevel = 99;
        }
    }
  }// Close MenuLevel 39

  if(MenuLevel == 40){ //  Full Manual Init Dest / Draw Summary Screen
    lcd.clear();
    lcd.setCursor(2, 0);
    lcd.print("Full Manual Mode");
    lcd.setCursor(0, 1);
    lcd.print("Control With Sliders");
    lcd.setCursor(1, 2);
    lcd.print("Knobs and Nunchuck");
    lcd.setCursor(5, 3);
    lcd.print("Press Next");
    MenuLevel = 41;
  } //Close MenuLevel 40

  if(MenuLevel == 41){

    ReadButtons();
    if(RightEncoderBtn_State == true){MenuLevel = 99;}
  } //Close MenuLevel 41

  if(MenuLevel == 99){  // PreRun Screen
  //Serial.println("Enter case 99");
    if(MenuStep == 0){
      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("!Ready to Run!");

      lcd.setCursor(1, 2);
      lcd.print("Press Run To Start");

      lcd.setCursor(2, 3);
      lcd.print("Or Stop to Reset");
      MenuStep = 1;

      //SerialDebug();
    } // Close menulevel 99 menustep 0

    if(MenuStep == 1){
      ReadButtons();
      if(RunBtn_State == true){
        //RunBtnLED_State = true;
        if(UsePresets == false && SetRuntimeParams == false){
          RunScreen = 10;
          GlobalFlag = 2;
          }
        if(SetRuntimeParams == true){
          GlobalFlag = 3;
          RunScreen = 20;
        }
        if(UsePresets == true){
          GlobalFlag = 4;
          RunScreen = 30;
        }
        MenuLevel = 100;
        GlobalFlag = 2;
        }
      if(EmergStop_State == true){
        ResetMenu();
      }

      //Flash Run Button LED
      CurrentMillis = millis();
      if (CurrentMillis - PreviousMillis >= 500 && RunBtnLED_State == false) {
        RunBtnLED_State = true;
        PreviousMillis = CurrentMillis;
      }
      if (CurrentMillis - PreviousMillis >= 500 && RunBtnLED_State == true) {
        RunBtnLED_State = false;
        PreviousMillis = CurrentMillis;
      }
      digitalWrite(RunBtnLED_Pin,RunBtnLED_State);
      //UpdateLED();
    } // Close menulevel 99 menustep 1
    //Serial.println("exit case 99");
  } //Close MenuLevel 99

} //@ Close Setup Menu Function

void PresetRuntime(){  //!Work here
  if(RuntimeFlag == 0){  //enable motor
      MotDrvDisable_State = false;
      digitalWrite(MotDrv_Disable_Pin, MotDrvDisable_State);
      RuntimeFlag = 1;
  }
  //~ Runtime Display
  CurrentMillis = millis();
  if (CurrentMillis - PreviousMillis >= RunDispInterval) {  //Runtime Display Cycle
      RuntimeDisplay();
      PreviousMillis = CurrentMillis;
  }

  RealCycleCountOld = RealCycleCount;  //here for debugging, actual runtime will need to set old val before updating real val
  RealCycleCount = RealCycleCount+1;  // here for debugging

}  //@  Close Preset Runtime

void ParamsRuntime(){ //@ Mostly Done, need to make decel work and will need to fine tune
  int SlidePotAMapOld = SlidePotA_Map;
  int SlidePotBMapOld = SlidePotB_Map;
  int RotaryPotMapOld = RotaryPot_Map;
  int RunSpd;
  long FwdRunTarget;
  long RevRunTarget;
  int RunAccel;
  int RunDecel;
  unsigned long RunDwell;
  int RunOffset = RotaryPot_Val;
  int ParamsFwdRunLength;
  int ParamsRevRunLength;
  bool CycleCalcDone = false;

  ReadAnalog();
  UpdateLED();
  ReadButtons();

  //~These are here to tweak testing values
  //FwdRunSpeed = 50;
  //RevRunSpeed = 50; 
  //FwdAccel = 50;
  //FwdDecel = 50;
  //RevAccel = 50;
  //RevDecel = 50;
  //FwdDwell = 500;
  //RevDwell = 50;
  //FwdRunLength = 0;
  //RevRunLength = 0;
  //FwdLimit = 0;
  //RevLimit = 0;
  //~Those were there to tweak testing values

  CurrentMillis = millis();

  if(RuntimeFlag == 0){ //enable motor
    MotDrvDisable_State = false;
    digitalWrite(MotDrv_Disable_Pin, MotDrvDisable_State);
    Motor->setAcceleration(50000);
    RuntimeDisplay();
    RuntimeFlag = 1;
  }

  //~ Runtime Display
  if (CurrentMillis - PreviousMillis >= RunDispInterval) { 
    RuntimeDisplay();
    RuntimeFlag = RuntimeFlag+1;
    if(RuntimeFlag > 3){RuntimeFlag = 1;}
    PreviousMillis = CurrentMillis;
  }
  if(RuntimeFlag == 1 && SlidePotAMapOld != SlidePotA_Map){
    lcd.setCursor(11,3);
    lcd.print("   ");
    lcd.setCursor(11,3);
    lcd.print(FwdRunLength);
  }
  if(RuntimeFlag == 2 && SlidePotBMapOld != SlidePotB_Map){
    lcd.setCursor(11,3);
    lcd.print("   ");
    lcd.setCursor(11,3);
    lcd.print(RevRunLength);
  }
  if(RuntimeFlag == 3 && RotaryPotMapOld != RotaryPot_Map){
    lcd.setCursor(8,3);
    lcd.print("   ");
    lcd.setCursor(8,3);
    lcd.print(CenterOffset);

    if(UseAutoOff == true){
        lcd.setCursor(3,0);
        lcd.print("Cycle=");
        lcd.print(RealCycleCount);
        lcd.print("/");
        lcd.print(AutoOffTarget);
      }else{
        lcd.setCursor(3,0);
        lcd.print("Cycle Counter");
        lcd.setCursor(3,1);
        lcd.print(RealCycleCount);
      }
      lcd.setCursor(3,2);
      lcd.print("Center Offset");
      lcd.setCursor(8,3);
      lcd.print(CenterOffset);
  }

  if(UseAutoOff == true && RealCycleCount >= AutoOffTarget){GlobalFlag = 5;}

  //Map Center Offset
  if(RunOffset < 512){RunOffset = map(RunOffset,513,1023,0,500);}  
  if(RunOffset > 512){RunOffset = map(RunOffset,0,511,-500,0);}
  if(RunOffset == 512){RunOffset = 0;}
  
  if(RunDirection == 0){ //Forward

    if(CycleCalcDone == false){
      // Map speeds to hz value
      RunSpd = map(FwdRunSpeed, 0, 100, 0, 20000);

      //integrate teh sliders
      ParamsFwdRunLength = map(SlidePotB_Val, 0, 1023, 0, FwdLimit);

      //calculate fwd run
      FwdRunTarget = ParamsFwdRunLength+RunOffset;

      // calculete accel
      RunAccel = map(FwdAccel, 0, 100, 0, 30000); //! set to reasonable val

      // calculate decel
      RunDecel = map(FwdDecel, 0, 100, 0, 30000);//! set to reasonable val

      // Calculete Dwell
      RunDwell = map(FwdDwell, 0, 100, 0, 500); //! set to reasonable val
      CycleCalcDone = true;
    }

    // run forward
    CurrentPosition = Motor->getCurrentPosition();
    if(CurrentPosition < FwdRunTarget){
    Motor->setSpeedInHz(RunSpd);
    Motor->moveTo(FwdRunTarget, false);
    }
    
    //dwell
    if(CurrentMillis - PrevDwellMillis >= RunDwell && CurrentPosition >= FwdRunTarget){
        RunDirection = 1;
        CycleCalcDone = false;
        PrevDwellMillis = CurrentMillis;
    }
  } //close fwd run

  if(RunDirection == 1){ //Reverse

    if(CycleCalcDone == false){
      //Map Speed to hz val
      RunSpd = map(RevRunSpeed, 0, 100, 0, 20000);

      //integrate teh sliders
      ParamsRevRunLength = map(SlidePotA_Val, 0, 1023, 0, RevLimit);

      //Calculate rev run
      RevRunTarget = ParamsRevRunLength-RunOffset;

      // calculete accel
      RunAccel = map(RevAccel, 0, 100, 0, 30000); //! set to reasonable val

      // calculate decel
      RunDecel = map(RevDecel, 0, 100, 0, 30000);//! set to reasonable val

      // Calculete Dwell
      RunDwell = map(RevDwell, 0, 100, 0, 500); //! set to reasonable val
      CycleCalcDone = true;
    }

    //run reverse
    CurrentPosition = Motor->getCurrentPosition();
    if(CurrentPosition > -RevRunTarget){
    Motor->setSpeedInHz(RunSpd);
    Motor->moveTo(-RevRunTarget, false);
    }

    //dwell
    if(CurrentMillis - PrevDwellMillis >= RunDwell && CurrentPosition <= RevRunTarget){
      RunDirection = 0;
      CycleCalcDone = false;
      PrevDwellMillis = CurrentMillis;
    }
  }// close rev run
  RealCycleCount = RealCycleCount+1;
}  //@  Close Params Runtime

void ManualRuntime(){  //@ some testing stuff and assumptions to remove , decide what nun btns do
  int AnalogMotorControl = AnalogVert_Val;
  int ManualCenterOffset = RotaryPot_Val;
  int ManualRunSpeed;
  int ManualFwdRunLength;
  int ManualRevRunLength;

  UpdateLED();
  ReadButtons();

  if(RuntimeFlag == 0){ //enable motor
    MotDrvDisable_State = false;
    digitalWrite(MotDrv_Disable_Pin, MotDrvDisable_State);
    Motor->setAcceleration(50000);
    RuntimeDisplay();
    RuntimeFlag = 1;
  }
  //~ Runtime Display

  CurrentMillis = millis();
  if (CurrentMillis - PreviousMillis >= RunDispInterval) {  //Runtime Display Cycle
      RuntimeDisplay();
      if(RuntimeFlag == 1){RuntimeFlag = 2;}else{RuntimeFlag = 1;}
      PreviousMillis = CurrentMillis;
    }
  if(RuntimeFlag == 1){
    
    int SlidePotAMapOld = SlidePotA_Map;
    int SlidePotBMapOld = SlidePotB_Map;
    int RotaryPotMapOld = RotaryPot_Map;
    ReadAnalog();
    if(SlidePotAMapOld != SlidePotA_Map){
      //FwdRunLength = SlidePotA_Map;
      lcd.setCursor(9,0);
      lcd.print("   ");
      lcd.setCursor(9,0);
      lcd.print(FwdRunLength);
    }

    if(SlidePotBMapOld != SlidePotB_Map){
      //RevRunLength = SlidePotB_Map;
      lcd.setCursor(9,1);
      lcd.print("   ");
      lcd.setCursor(9,1);
      lcd.print(RevRunLength);
    }

    if(RotaryPotMapOld != RotaryPot_Map){
      //CenterOffset = RotaryPot_Map;
      lcd.setCursor(8,2);
      lcd.print("   ");
      lcd.setCursor(8,2);
      lcd.print(CenterOffset);
    }

    if(RealCycleCount != RealCycleCountOld && UseAutoOff == true){
      lcd.setCursor(6,3);
      lcd.print("       ");
      lcd.setCursor(6,3);
      lcd.print(RealCycleCount);
      lcd.print("/");
      lcd.print(AutoOffTarget);
    }
  }
  if(RuntimeFlag == 2){
    
    int ManualFwdOld = ManualFwd;
    int ManualRevOld = ManualRev;
    int ManualRightOld = ManualRight;
    int ManualLeftOld = ManualLeft;

    ReadAnalog();
    if(ManualRightOld != ManualRight || ManualLeftOld != ManualLeft){
      lcd.setCursor(6,1);
      lcd.print("   ");
      lcd.setCursor(6,1);
      lcd.print(ManualLeft);

      lcd.setCursor(17,1);
      lcd.print("   ");
      lcd.setCursor(17,1);
      lcd.print(ManualRight);
    }

    if(ManualFwdOld != ManualFwd || ManualRevOld != ManualRev){
      lcd.setCursor(5,0);
      lcd.print("   ");
      lcd.setCursor(5,0);
      lcd.print(ManualFwd);

      lcd.setCursor(15,0);
      lcd.print("   ");
      lcd.setCursor(15,0);
      lcd.print(ManualRev);
    }
  }
  if(RealCycleCount != RealCycleCountOld && UseAutoOff == true){
    lcd.setCursor(6,3);
    lcd.print("       ");
    lcd.setCursor(6,3);
    lcd.print(RealCycleCount);
    lcd.print("/");
    lcd.print(AutoOffTarget);
    RealCycleCountOld = RealCycleCount;
  }
  
  if(UseAutoOff == true && RealCycleCount >= AutoOffTarget){GlobalFlag = 5;}
  //if(NunchuckBtn1_State == true){}
  //if(NunchuckBtn2_State == true){}
  //if(AnalogSW_State == true){}

  //# Map the center offset and scale it
  //!the map values (500) need to be set reasonable
  if(ManualCenterOffset < 512){ManualCenterOffset = map(ManualCenterOffset,513,1023,0,500);}  
  if(ManualCenterOffset > 512){ManualCenterOffset = map(ManualCenterOffset,0,511,-500,0);}
  if(ManualCenterOffset == 512){ManualCenterOffset = 0;}

  //# Integrate the center offset with the analog motor control
  AnalogMotorControl = AnalogMotorControl+ManualCenterOffset;

  //# Map the sloders to a useful var for manual usage, from 0 to whatever the limit is
  ManualFwdRunLength = map(SlidePotB_Val,0,1023,0,FwdLimit);//! will need to set to something reasonable
  ManualRevRunLength = map(SlidePotA_Val,0,1023,0,RevLimit);//! will need to set to something reasonable

  //# Map the analog stick full back to full fwd ---> full back = reverse run length <> full fwd = fwd run length
  if(AnalogMotorControl < 512){AnalogMotorControl = map(AnalogMotorControl,512,1023,0,ManualFwdRunLength);}
  if(AnalogMotorControl > 512){AnalogMotorControl = map(AnalogMotorControl,512,0,0,-ManualRevRunLength);}
  if(AnalogMotorControl == 512){AnalogMotorControl = 0;}

  //# Set Manual Run Speed to 0-100 weather or not its in reverse or forward
  if(ManualFwd > 0){ManualRunSpeed = ManualFwd;}
  if(ManualRev > 0){ManualRunSpeed = ManualRev;}

  //# Map Manual Runspeed to something accelstepper will take
  ManualRunSpeed = map(ManualRunSpeed,0,100,0,20000);

  //# Make sure what were asking the motor to do will not overrun the limit switches
  AnalogMotorControl = constrain(AnalogMotorControl,-RevLimit,FwdLimit);

    //# Tell the Motor to actually do the thing!
  Motor->setSpeedInHz(ManualRunSpeed);
  Motor->moveTo(AnalogMotorControl, false);

  //# Cycle counter
  CurrentPosition = Motor->getCurrentPosition();
  if(CurrentPosition < ManualPositionOld && ManualThreshold == 0){
    RealCycleCount = RealCycleCount+1;
    ManualThreshold = 1;
  }
  if(CurrentPosition > ManualPositionOld && ManualThreshold == 1){ManualThreshold = 0;}
  ManualPositionOld = CurrentPosition;
}  //@  Close Manual Runtime

void RunComplete(){  //# Done
  if(GlobalFlag == 5){ // ~~~Draw Run Over Display~~~
    Motor->moveTo(0, true);
    Motor->stopMove();
    MotDrvDisable_State = true;
    digitalWrite(MotDrv_Disable_Pin, MotDrvDisable_State);
    RunBtnLED_State = false;

    lcd.clear();

    lcd.setCursor(3,0);
    lcd.print("!Run Complete!");
      
    lcd.setCursor(1,1);
    lcd.print("Actual Cycle Count");
    lcd.setCursor(0,2);
    lcd.print(RealCycleCount);

    lcd.setCursor(2,3);
    lcd.print("Any Key To Reset");

    GlobalFlag = 6;
  }
  if(GlobalFlag == 6){
    ReadButtons();
    if(RightEncoderBtn_State == true || LeftEncoderBtn_State == true || RunBtn_State == true || NunchuckBtn1_State == true || NunchuckBtn2_State == true || AnalogSW_State == true){
      ResetMenu();
    }
  }
}  //@  Close RunComplete

void RuntimeDisplay(){  //! NEED TO WRITE PRESET RUNTIME DISPLAY, perhaps delete much of the manual runtime screens
  // Runscreen 10-19 = Manual Mode <> 20-29 = Params Mode <> 30-39 = Presets

  if(UsePresets == false && SetRuntimeParams == false){ //~~~Manual Display Block~~~
    if(RunScreen == 10){  // Draw Manual Mode, RunScreen 1
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Fwd Stop=");
      lcd.setCursor(9,0);
      lcd.print(FwdRunLength);

      lcd.setCursor(0,1);
      lcd.print("Rev Stop=");
      lcd.setCursor(9,1);
      lcd.print(RevRunLength);

      lcd.setCursor(0,2);
      lcd.print("Center=");
      lcd.setCursor(8,2);
      lcd.print(CenterOffset);

      if(UseAutoOff == true){
        lcd.setCursor(0,3);
        lcd.print("Cycle=");
        lcd.print(RealCycleCount);
        lcd.print("/");
        lcd.print(AutoOffTarget);
      }else{
        lcd.setCursor(3,3);
        lcd.print("Manual Shutoff");
      }

      RunScreen = 11;
    } //close runscreen 10

    if(RunScreen == 11){  //  Update Manual Mode RunScreen 1
      RunScreen = 12;
      return;
    } // close runscreen 11

    if(RunScreen == 12){  //  Draw Manual Mode Screen 2
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Fwd=");
      lcd.setCursor(5,0);
      lcd.print(ManualFwd);
      lcd.setCursor(10,0);
      lcd.print("Rev=");
      lcd.setCursor(15,0);
      lcd.print(ManualRev);

      lcd.setCursor(0,1);
      lcd.print("Left=");
      lcd.setCursor(6,1);
      lcd.print(ManualLeft);
      lcd.setCursor(11,1);
      lcd.print("Right=");
      lcd.setCursor(17,1);
      lcd.print(ManualRight);

      if(UseAutoOff == true){
        lcd.setCursor(0,3);
        lcd.print("Cycle=");
        lcd.print(RealCycleCount);
        lcd.print("/");
        lcd.print(AutoOffTarget);
      }else{
        lcd.setCursor(3,3);
        lcd.print("Manual Shutoff");
      }
      RunScreen = 13;
    } //close runscreen 12

    if(RunScreen == 13){  //  Update Manual Mode RunScreen 1
      RunScreen = 10;
      return;
    }// close runscreen 13
  } // Close Manual Display Block

  if(SetRuntimeParams == true){//~~~Params Display Block~~~
    if(RunScreen == 20){  //  Draw Params Mode, Screen 1
      lcd.clear();

      lcd.setCursor(2,0);
      lcd.print("Forward Settings");
      
      lcd.setCursor(0,1);
      lcd.print("Spd=");
      lcd.setCursor(4,1);
      lcd.print(FwdRunSpeed);

      lcd.setCursor(9,1);
      lcd.print("Dwell=");
      lcd.setCursor(15,1);
      lcd.print(FwdDwell);

      lcd.setCursor(0,2);
      lcd.print("Acc=");
      lcd.setCursor(4,2);
      lcd.print(FwdAccel);

      lcd.setCursor(9,2);
      lcd.print("Dec=");
      lcd.setCursor(13,2);
      lcd.print(FwdDecel);

      lcd.setCursor(0,3);
      lcd.print("Fwd Length=");
      lcd.setCursor(11,3);
      lcd.print(FwdRunLength);

      RunScreen = 21;
      return;
    } //close runscreen 20

    if(RunScreen == 21){  // Draw Params Mode Screen 2
      lcd.clear();

      lcd.setCursor(2,0);
      lcd.print("Reverse Settings");

      lcd.setCursor(0,1);
      lcd.print("Spd=");
      lcd.setCursor(4,1);
      lcd.print(RevRunSpeed);

      lcd.setCursor(9,1);
      lcd.print("Dwell=");
      lcd.setCursor(15,1);
      lcd.print(RevDwell);

      lcd.setCursor(0,2);
      lcd.print("Acc=");
      lcd.setCursor(4,2);
      lcd.print(RevAccel);

      lcd.setCursor(9,2);
      lcd.print("Dec=");
      lcd.setCursor(13,2);
      lcd.print(RevDecel);

      lcd.setCursor(0,3);
      lcd.print("Rev Length=");
      lcd.setCursor(11,3);
      lcd.print(RevRunLength);

      RunScreen = 22;
      return;
    }// close runscreen 21

    if(RunScreen == 22){  // Draw Params Mode Screen 3
      lcd.clear();
      if(UseAutoOff == true){
        lcd.setCursor(3,0);
        lcd.print("Cycle=");
        lcd.print(RealCycleCount);
        lcd.print("/");
        lcd.print(AutoOffTarget);
      }else{
        lcd.setCursor(3,0);
        lcd.print("Cycle Counter");
        lcd.setCursor(3,1);
        lcd.print(RealCycleCount);
      }
      lcd.setCursor(3,2);
      lcd.print("Center Offset");
      lcd.setCursor(8,3);
      lcd.print(CenterOffset);

      RunScreen = 20;
      return;
    }// close runscreen 22
  }// Close Params Display Block

  if(UsePresets == true){//! ~~~Presets Display Block~~~
    if(RunScreen == 30){
      
    }  //  Close Runscreen 30

    if(RunScreen == 31){
      
    }  //  Close Runscreen 31

    if(RunScreen == 32){
      
    }  //  Close Runscreen 32

    if(RunScreen == 33){
      
    }  //  Close Runscreen 33

    if(RunScreen == 34){
      
    }  //  Close Runscreen 34

    if(RunScreen == 35){
    
  }  //  Close Runscreen 35

  } // Close Presets display block
  
}  //@ Close Runtime Display

void Homing(){ //# Done
  //~ Skip Homing Routine with DIP Switch 8
  DIP_States[7] = digitalRead(DIP8_Pin);
  if(DIP_States[7] == false){DIP_States[7] = true;}
  else{DIP_States[7] = false;}
  if(DIP_States[7]){
    FwdLimit = 100;
    RevLimit = -100;
    CenterOffset = 0;
    GlobalFlag = 1;
    return;
  }

  detachInterrupt(digitalPinToInterrupt(Limit_Int_Pin));

  int Center = 0;
  MotDrvDisable_State = false;
  digitalWrite(MotDrv_Disable_Pin, MotDrvDisable_State);
  UpdateLED();

  lcd.clear();
  lcd.setCursor(3,0);
  lcd.print("Homing Routine");
  bool HomingComplete = false;
  bool FwdLimitComplete = false;
  bool RevLimitComplete = false;
  int HomingStep = 1;  //1 is fwd, 2 is rev, 
  bool MoveGo = true;
  long HomingIncriment = 5;
  Motor->setSpeedInHz(1000);
  Motor->setAcceleration(500);
  Motor->moveTo(HomingIncriment, false);

  while(HomingComplete == false){
    Limit1_State = digitalRead(Limit1_Pin);
    Limit2_State = digitalRead(Limit2_Pin);

    if(HomingStep == 1){HomingIncriment = HomingIncriment+5;}
    if(HomingStep == 2){HomingIncriment = HomingIncriment-5;}

    Motor->moveTo(HomingIncriment, false);

    CurrentPosition = Motor->getCurrentPosition();
    if(FwdLimitComplete == false && HomingStep == 1){
      lcd.setCursor(0,1);
      lcd.print("Find Fwd Limit ");
      lcd.print(CurrentPosition);
      }
    if(FwdLimitComplete == true && HomingStep == 2){
      lcd.setCursor(0,2);
      lcd.print("Find Rev Limit ");
      lcd.print(CurrentPosition);
    }
    
    if(Limit2_State == true && FwdLimitComplete == false){  //Fwd Limit Hit
      Motor->stopMove();
      Motor->setSpeedInHz(1);
      Motor->setAcceleration(1);
      while(FwdLimitComplete == false){
        Limit2_State = digitalRead(Limit2_Pin);
        if(Limit2_State == true){
          Motor->backwardStep();
          CurrentPosition = Motor->getCurrentPosition();
          lcd.setCursor(15,1);
          lcd.print("     ");
          lcd.setCursor(15,1);
          lcd.print(CurrentPosition);
          delay(100);
          }
        
        if(Limit2_State == false){
          Motor->stopMove();
          CurrentPosition = Motor->getCurrentPosition();
          FwdLimit = CurrentPosition;
          lcd.setCursor(0,1);
          lcd.print("                    ");
          lcd.setCursor(0,1);
          lcd.print("Fwd Limit at ");
          lcd.print(FwdLimit);
          Motor->setSpeedInHz(1000);
          Motor->setAcceleration(500);
          Motor->moveTo(0, true);
          HomingIncriment = 0;
          HomingStep = 2;
          FwdLimitComplete = true;
        }
      }
    }

    if(Limit1_State == true && RevLimitComplete == false){  //Rev Limit Hit
      Motor->stopMove();
      Motor->setSpeedInHz(1);
      Motor->setAcceleration(1);
      while(RevLimitComplete == false){
        Limit1_State = digitalRead(Limit1_Pin);
        if(Limit1_State == true){
          Motor->forwardStep();
          CurrentPosition = Motor->getCurrentPosition();
          lcd.setCursor(15,2);
          lcd.print("     ");
          lcd.setCursor(15,2);
          lcd.print(CurrentPosition);
          delay(100);
          }
        
        if(Limit1_State == false){
          Motor->stopMove();
          CurrentPosition = Motor->getCurrentPosition();
          RevLimit = CurrentPosition;
          lcd.setCursor(0,2);
          lcd.print("                    ");
          lcd.setCursor(0,2);
          lcd.print("Rev Limit at ");
          lcd.print(RevLimit);
          HomingStep = 3;
          RevLimitComplete = true;
          HomingComplete = true;
        }
      }
    }
    UpdateLED();
  } //close homing complete while loop
  float LimitDelta = RevLimit+FwdLimit;
  Center = LimitDelta/2;
  Motor->setSpeedInHz(1000);
  Motor->setAcceleration(500);
  Motor->moveTo(Center, true);
  Motor->stopMove();
  Motor->setCurrentPosition(0);

  attachInterrupt(digitalPinToInterrupt(Limit_Int_Pin), CriticalStopISR, FALLING);
  MotDrvDisable_State = true;
  digitalWrite(MotDrv_Disable_Pin, MotDrvDisable_State);
  UpdateLED();
  GlobalFlag = 1;
}  //@  Close Homing

void CriticalStopISR(){ //# Done
  Limit1_State = digitalRead(Limit1_Pin);
  Limit2_State = digitalRead(Limit2_Pin);
  EmergStop_State = digitalRead(EmergStop_Pin);
  ErrorState = true;
}  //@  Close CriticalStopISR

void FaultHandler(){ //# Done
  //Motor->stopMove();
  Motor->forceStop();
  FaultLED_State = true;
  RunningLED_State = false;
  MotorEnableLED_State =false;
  lcd.clear();

  if(Limit1_State == true){
    Limit2LED_State=true;
    lcd.setCursor(3,0);
    lcd.print("Critial Fault!");
    lcd.setCursor(3,1);
    lcd.print("Reverse Limit");
    lcd.setCursor(1,2);
    lcd.print("Threshold Exeeded!");
    lcd.setCursor(4,3);
    lcd.print("Halting Run!");
  }

  if(Limit2_State == true){
    Limit1LED_State=true;
    lcd.setCursor(3,0);
    lcd.print("Critial Fault!");
    lcd.setCursor(3,1);
    lcd.print("Forward Limit");
    lcd.setCursor(1,2);
    lcd.print("Threshold Exeeded!");
    lcd.setCursor(4,3);
    lcd.print("Halting Run!");
  }

  if(EmergStop_State == true){
    lcd.setCursor(2,0);
    lcd.print("Kill Switch Hit!");
    lcd.setCursor(1,1);
    lcd.print("Return to Rev Stop");
    //lcd.setCursor(1,2);
    //lcd.print("");
    lcd.setCursor(4,3);
    lcd.print("Halting Run!");
  }
  UpdateLED();
  while(ErrorState == true){delay(10000);}
}  //@  Close Fault Handler

void ResetMenu(){  //@ Likely missing vars that would need to be reset
  FwdRunSpeed = 50;
  RevRunSpeed = 50; 
  FwdAccel = 50;
  FwdDecel = 50;
  RevAccel = 50;
  RevDecel = 50;
  FwdDwell = 50;
  RevDwell = 50;
  FwdLimit = 0;
  RevLimit = 0;
  CenterOffset = 0;
  
  EncoderL_Mapped = 0;
  EncoderR_Mapped = 0;
  RightEnc100 = 0;
  EncoderL_Val = 0;
  EncoderR_Val = 0;
  SlidePotA_Val = 0;
  SlidePotA_Map = 0;
  SlidePotB_Val = 0;
  SlidePotB_Map = 0;
  RotaryPot_Val = 0;
  RotaryPot_Map = 0;
  AnalogHoriz_Val = 0;
  AnalogHoriz_Map = 0;
  AnalogVert_Val = 0;
  AnalogVert_Map = 0;

  UseAutoOff = false;
  AutoOffTarget = 0;
  UsePresets = true;
  AutoIncPresets = false;
  SelectedPreset = 0;
  AutoIncTarget = 50;
  SetRuntimeParams = true;

  RealCycleCount = 0; 
  RealCycleCountOld = 0;

  PreviousMillis = 0; 

  MenuLevel = 0;
  MenuStep = 0;
  Selector = 1;
  RunScreen = 1;
  GlobalFlag = 0;
  lcd.clear();
  ReadEncoders(true);
}  //@  Close Reset Menu

void ReadButtons(){ //# Done
  RightEncoderBtn_State = false;
  LeftEncoderBtn_State = false;
  RunBtn_State = false;
  EmergStop_State = false;
  NunchuckBtn1_State = false;
  NunchuckBtn2_State = false;
  AnalogSW_State = false;

  NextBtn.update();
  if(NextBtn.state() == LOW){
    RightEncoderBtn_State = true;
  }else{RightEncoderBtn_State = false;}

  BackBtn.update();
  if(BackBtn.state() == LOW){
    LeftEncoderBtn_State = true;
  }else{LeftEncoderBtn_State = false;}

  RunBtn.update();
  if(RunBtn.state() == LOW){
    RunBtn_State = true;
  }else{RunBtn_State = false;}

  // StopBtn.update();
  // if(StopBtn.state() == LOW){
  //   EmergStop_State = true;
  // }else{EmergStop_State = false;}

    NunBtn1.update();
  if(NunBtn1.state() == LOW){
    NunchuckBtn1_State = true;
  }else{NunchuckBtn1_State = false;}

    NunBtn2.update();
  if(NunBtn2.state() == LOW){
    NunchuckBtn2_State = true;
  }else{NunchuckBtn2_State = false;}

    NunAnaBtn.update();
  if(NunAnaBtn.state() == LOW){
    AnalogSW_State = true;
  }else{AnalogSW_State = false;}
  delay(50);
} //@ Close Read All Buttons

void ReadAnalog(){ //@ Likely some vars calculated here that arent needed
  SlidePotA_Val = analogRead(SlidePotA_Pin);
  SlidePotB_Val = analogRead(SlidePotB_Pin);
  RotaryPot_Val = analogRead(RotaryPotA_Pin);
  AnalogVert_Val = analogRead(AnalogVert_Pin);
  AnalogHoriz_Val = analogRead(AnalogHoriz_Pin);
  SlidePotA_Map = map(SlidePotA_Val, 0,1023, 0, 100);
  SlidePotB_Map = map(SlidePotB_Val, 0,1023, 0, 100);
  RotaryPot_Map = map(RotaryPot_Val, 0,1023, 0, 100);
  AnalogHoriz_Map = map(AnalogHoriz_Val, 0,1023, 0, 100);
  AnalogVert_Map = map(AnalogVert_Val, 0,1023, 0, 100);

  ManualFwd = map(AnalogVert_Val, 512,1023,0,100);
  ManualFwd = constrain(ManualFwd, 0, 100);
  ManualRev = map(AnalogVert_Val, 512,0,0,100);
  ManualRev = constrain(ManualRev, 0, 100);

  ManualLeft = map(AnalogHoriz_Val, 512,1023,0,100);
  ManualLeft = constrain(ManualLeft, 0, 100);
  ManualRight = map(AnalogHoriz_Val, 512,0,0,100);
  ManualRight = constrain(ManualRight, 0, 100);

  FwdRunLength = SlidePotA_Map;
  RevRunLength = SlidePotB_Map;
  CenterOffset = RotaryPot_Map;
} //@  Close Read all analog

void ReadEncoders(bool ResetEncoders){ //@ Properly loop encoder vals?
  if(ResetEncoders == true){
    EncLeft.write(0);
    EncRight.write(0);
    EncoderL_Val = 0;
    EncoderR_Val = 0;
    EncoderR_Mapped = 0;
    EncoderL_Mapped = 0;
    RightEnc100 = 0;
  }
  int newLeft, newRight;
  newLeft = EncLeft.read();
  newRight = EncRight.read();
  if (newLeft != EncoderL_Val || newRight != EncoderR_Val) {
    EncoderL_Val = newLeft;
    EncoderR_Val = newRight;
    EncoderL_Mapped = map(EncoderL_Val, 0, 65535, 65535, 0);
    EncoderR_Mapped = map(EncoderR_Val, 0, 65535, 65535, 0);
    RightEnc100 = map (EncoderR_Mapped, 0,100,1,100);
    if(RightEnc100 > 100){
      RightEnc100 = 0;
      EncRight.write(0);
      EncoderR_Val = 0;
    }
    if(RightEnc100 <= -1){
      RightEnc100 = 100;
      EncRight.write(65535);
      EncoderR_Val = 65535;
    }
  }
}  //@  Close Read Encoders

void UpdateLED(){ //# Done
  if(ErrorState == true){FaultLED_State = true;}else{FaultLED_State = false;}
  if(Motor->isRunning() == true){RunningLED_State = true;}else{RunningLED_State = false;}
  if(MotDrvDisable_State == false){MotorEnableLED_State = true;}else{MotorEnableLED_State = false;}

  digitalWrite(FaultLED_Pin, FaultLED_State);
  digitalWrite(RunBtnLED_Pin, RunBtnLED_State);
  digitalWrite(RunningLED_Pin, RunningLED_State);
  digitalWrite(MotorEnableLED_Pin, MotorEnableLED_State);
  digitalWrite(MotorDirectionLED_Pin, MotorDirectionLED_State);
  digitalWrite(Limit1_LED_Pin, Limit1LED_State);
  digitalWrite(Limit2_LED_Pin, Limit2LED_State);
}  //@  Close Update LED

void ReadDip(){ //# Done
  DIP_States[0] = digitalRead(DIP1_Pin);
  if(DIP_States[0] == false){
    DIP_States[0] = true;
  }
  else{DIP_States[0] = false;}

  DIP_States[1] = digitalRead(DIP2_Pin);
  if(DIP_States[1] == false){
    DIP_States[1] = true;
  }
  else{DIP_States[1] = false;}

  DIP_States[2] = digitalRead(DIP3_Pin);
  if(DIP_States[2] == false){
    DIP_States[2] = true;
  }
  else{DIP_States[2] = false;}

  DIP_States[3] = digitalRead(DIP4_Pin);
  if(DIP_States[3] == false){
    DIP_States[3] = true;
  }
  else{DIP_States[3] = false;}

  DIP_States[4] = digitalRead(DIP5_Pin);
  if(DIP_States[4] == false){
    DIP_States[4] = true;
  }
  else{DIP_States[4] = false;}

  DIP_States[5] = digitalRead(DIP6_Pin);
  if(DIP_States[5] == false){
    DIP_States[5] = true;
  }
  else{DIP_States[5] = false;}

  DIP_States[6] = digitalRead(DIP7_Pin);
  if(DIP_States[6] == false){
    DIP_States[6] = true;
    }
  else{DIP_States[6] = false;}

  DIP_States[7] = digitalRead(DIP8_Pin);
  if(DIP_States[7] == false){
    DIP_States[7] = true;
  }
  else{DIP_States[7] = false;}
}//@ Close ReadDip Function

void DipDebug(){ //@ still have one dip switch to assign
  bool clicked = false;
  if(DIP_States[0] == true){  //* LED Test
    digitalWrite(RunBtnLED_Pin, HIGH);
    digitalWrite(MotorEnableLED_Pin, HIGH);
    digitalWrite(MotorDirectionLED_Pin, HIGH);
    digitalWrite(RunningLED_Pin, HIGH);
    digitalWrite(FaultLED_Pin, HIGH);
    digitalWrite(Limit1_LED_Pin, HIGH);
    digitalWrite(Limit2_LED_Pin, HIGH);
    delay(1000);
    digitalWrite(RunBtnLED_Pin, LOW);
    digitalWrite(MotorEnableLED_Pin, LOW);
    digitalWrite(MotorDirectionLED_Pin, LOW);
    digitalWrite(RunningLED_Pin, LOW);
    digitalWrite(FaultLED_Pin, LOW);
    digitalWrite(Limit1_LED_Pin, LOW);
    digitalWrite(Limit2_LED_Pin, LOW);
  }
  if(DIP_States[1] == true){  //* Button Test
    lcd.clear();
    lcd.setCursor(2, 0);
    lcd.print("Button Test Mode");

    lcd.setCursor(0, 1);
    lcd.print("Press Right Encoder");
    while(clicked == false){
      ReadButtons();
      if(RightEncoderBtn_State == true){
        lcd.setCursor(0,3);
        lcd.print("Right Encoder Button");
        delay(1000);
        lcd.setCursor(0,3);
        lcd.print("                    ");
        clicked = true;
      }
    }

    clicked = false;
    lcd.setCursor(0, 1);
    lcd.print("Press Left Encoder");
    while(clicked == false){
      ReadButtons();
      if(LeftEncoderBtn_State == true){
        lcd.setCursor(0,3);
        lcd.print("Left Encoder Button");
        delay(1000);
        lcd.setCursor(0,3);
        lcd.print("                    ");
        clicked = true;
      }
    }

    clicked = false;
    lcd.setCursor(0, 1);
    lcd.print("Press Emerg Stop    ");
    while(clicked == false){
      ReadButtons();
      if(EmergStop_State == true){
        lcd.setCursor(0,3);
        lcd.print("Emerg Stop Button");
        delay(1000);
        lcd.setCursor(0,3);
        lcd.print("                    ");
        clicked = true;
      }
    }

    clicked = false;
    lcd.setCursor(0, 1);
    lcd.print("Press Run Button    ");
    while(clicked == false){
      ReadButtons();
      if(RunBtn_State == true){
        lcd.setCursor(0,3);
        lcd.print("Run Button");
        delay(1000);
        lcd.setCursor(0,3);
        lcd.print("                    ");
        clicked = true;
      }
    }

    clicked = false;
    lcd.setCursor(0, 1);
    lcd.print("Press Nunchuck 1 Btn");
    while(clicked == false){
      ReadButtons();
      if(NunchuckBtn1_State == true){
        lcd.setCursor(0,3);
        lcd.print("Nunchuck Button 1");
        delay(1000);
        lcd.setCursor(0,3);
        lcd.print("                    ");
        clicked = true;
      }
    }

    clicked = false;
    lcd.setCursor(0, 1);
    lcd.print("Press Nunchuck 2 Btn");
    while(clicked == false){
      ReadButtons();
      if(NunchuckBtn2_State == true){
        lcd.setCursor(0,3);
        lcd.print("Nunchuck Button 2");
        delay(1000);
        lcd.setCursor(0,3);
        lcd.print("                    ");
        clicked = true;
      }
    }

    clicked = false;
    lcd.setCursor(0, 1);
    lcd.print("Press Stick Btn");
    while(clicked == false){
      ReadButtons();
      if(AnalogSW_State == true){
        lcd.setCursor(0,3);
        lcd.print("Nunchuck Analog Btn");
        delay(1000);
        lcd.setCursor(0,3);
        lcd.print("                    ");
        clicked = true;
      }
    }
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Checked All Buttons.");
    lcd.setCursor(0,1);
    lcd.print("The Button Names    ");
    lcd.setCursor(0,2);
    lcd.print("Should Have Matched ");
    lcd.setCursor(0,3);
    lcd.print("What Was Displayed. ");
    delay(10000);
  }
  if(DIP_States[2] == true){  //* Raw Analog Test
    int SliderARawOld;
    int SliderBRawOld;
    int RotaryRawOld;
    int HorizRawOld;
    int VertRawOld;
    lcd.clear();
    lcd.setCursor(2, 0);
    lcd.print("Analog Test Mode");
    lcd.setCursor(0,1);
    lcd.print("SliderA=");
    lcd.setCursor(12,1);
    lcd.print(" B=");

    lcd.setCursor(0,2);
    lcd.print("Stick H=");
    lcd.setCursor(12,2);
    lcd.print(" V=");

    lcd.setCursor(0,3);
    lcd.print("Rotary=");
    lcd.setCursor(16,3);
    lcd.print("Nxt>");
    clicked = false;
    while(clicked == false){
      ReadAnalog();
      ReadButtons();
      if(RightEncoderBtn_State == true){
        lcd.clear();
        clicked = true;
        }
      
      if(SlidePotA_Val != SliderARawOld || SlidePotB_Val != SliderBRawOld){
        lcd.setCursor(8,1);
        lcd.print("    ");
      lcd.setCursor(8,1);
        lcd.print(SlidePotA_Val);
        lcd.setCursor(15,1);
        lcd.print("    ");
      lcd.setCursor(15,1);
        lcd.print(SlidePotB_Val);
        SliderARawOld = SlidePotA_Val;
        SliderBRawOld = SlidePotB_Val;
      }

      if(AnalogHoriz_Val != HorizRawOld || AnalogVert_Val != VertRawOld){
        lcd.setCursor(8,2);
        lcd.print("    ");
        lcd.setCursor(8,2);
        lcd.print(AnalogHoriz_Val);
        lcd.setCursor(15,2);
        lcd.print("    ");
        lcd.setCursor(15,2);
        lcd.print(AnalogVert_Val);
        HorizRawOld = AnalogHoriz_Val;
        VertRawOld = AnalogVert_Val;
      }

      if(RotaryPot_Val != RotaryRawOld){
        lcd.setCursor(8,3);
        lcd.print("    ");
        lcd.setCursor(8,3);
        lcd.print(RotaryPot_Val);
        RotaryRawOld = RotaryPot_Val;
      }
    }
  }
  if(DIP_States[3] == true){  //* Map Analog Test
    int SliderAMapOld;
    int SliderBMapOld;
    int RotaryMapOld;
    int HorizMapOld;
    int VertMapOld;
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Map Analog Test Mode");
    lcd.setCursor(0,1);
    lcd.print("SliderA=");
    lcd.setCursor(12,1);
    lcd.print(" B=");

    lcd.setCursor(0,2);
    lcd.print("Stick H=");
    lcd.setCursor(12,2);
    lcd.print(" V=");

    lcd.setCursor(0,3);
    lcd.print("Rotary=");
    lcd.setCursor(16,3);
    lcd.print("Nxt>");
    clicked = false;
    while(clicked == false){
      ReadAnalog();
      ReadButtons();
      if(RightEncoderBtn_State == true){
        lcd.clear();
        clicked = true;
        }
      
      if(SlidePotA_Map != SliderAMapOld || SlidePotB_Map != SliderBMapOld){
      lcd.setCursor(8,1);
      lcd.print("    ");
      lcd.setCursor(8,1);
      lcd.print(SlidePotA_Map);
      lcd.setCursor(15,1);
      lcd.print("    ");
      lcd.setCursor(15,1);
      lcd.print(SlidePotB_Map);
      SliderAMapOld = SlidePotA_Map;
      SliderBMapOld = SlidePotB_Map;
      }

      if(AnalogHoriz_Map != HorizMapOld || AnalogVert_Map != VertMapOld){
        lcd.setCursor(8,2);
        lcd.print("    ");
        lcd.setCursor(8,2);
        lcd.print(AnalogHoriz_Map);
        lcd.setCursor(15,2);
        lcd.print("    ");
        lcd.setCursor(15,2);
        lcd.print(AnalogVert_Map);
        HorizMapOld = AnalogHoriz_Map;
        VertMapOld = AnalogVert_Map;
      }

      if(RotaryPot_Map != RotaryMapOld){
        lcd.setCursor(8,3);
        lcd.print("    ");
        lcd.setCursor(8,3);
        lcd.print(RotaryPot_Map);
        RotaryMapOld = RotaryPot_Map;
      }
    }
  }
  if(DIP_States[4] == true){  //* Encoder test
    int MapLeftOld;
    int MapRightOld;
    int RawLeftOld;
    int RawRightOld;
    lcd.clear();
    lcd.setCursor(1, 0);
    lcd.print("Encoder Test Mode");
    lcd.setCursor(8,1);
    lcd.print("Left |");
    lcd.setCursor(15,1);
    lcd.print("Right");
    lcd.setCursor(1,2);
    lcd.print("Raw");
    lcd.setCursor(1,3);
    lcd.print("Map");
    while(clicked == false){
      ReadButtons();
      ReadEncoders(false);
      if(RightEncoderBtn_State == true){
        lcd.clear();
        clicked = true;
      }
      if(EncoderL_Val != RawLeftOld || EncoderR_Val != RawRightOld){
        lcd.setCursor(8,2);
        lcd.print("     ");
        lcd.setCursor(8,2);
        lcd.print(EncoderL_Val);
        lcd.setCursor(15,2);
        lcd.print("     ");
        lcd.setCursor(15,2);
        lcd.print(EncoderR_Val);
        RawLeftOld = EncoderL_Val;
        RawRightOld = EncoderR_Val;
      }
      if(EncoderL_Mapped != MapLeftOld || EncoderR_Mapped != MapRightOld){
        lcd.setCursor(8,3);
        lcd.print("     ");
        lcd.setCursor(8,3);
        lcd.print(EncoderL_Mapped);
        lcd.setCursor(15,3);
        lcd.print("     ");
        lcd.setCursor(15,3);
        lcd.print(EncoderR_Mapped);
        MapLeftOld = EncoderL_Mapped;
        MapRightOld = EncoderR_Mapped;
      }
    }
  }
  if(DIP_States[5] == true){}
  if(DIP_States[6] == true){  //* Direct Manual Mode, Skip Setup Menu and Homing, 
  UsePresets = false; 
  AutoIncPresets = false; 
  SetRuntimeParams = false;  
  MenuLevel = 40; 
  GlobalFlag = 2;
  }
  if(DIP_States[7] == true){} //* Disable Homing Startup, done in the Homing function
} //@ Close DipDebug

void ThePlayground(){

    Motor->setSpeedInHz(5000);
    Motor->setAcceleration(30000);
    Motor->moveTo(10000, true);
    CurrentPosition = Motor->getCurrentPosition();
    Serial.println(CurrentPosition);
}