static void lcd_move_z()
{
    if (encoderPosition != 0)
    {
        refresh_cmd_timeout();
        current_position[Z_AXIS] += float((int)encoderPosition) * move_menu_scale;
        if (min_software_endstops && current_position[Z_AXIS] < Z_MIN_POS)
            current_position[Z_AXIS] = Z_MIN_POS;
        if (max_software_endstops && current_position[Z_AXIS] > Z_MAX_POS)
            current_position[Z_AXIS] = Z_MAX_POS;
        encoderPosition = 0;
        #ifdef DELTA
        calculate_delta(current_position);
        plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS]/60, active_extruder);
        #else
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS]/60, active_extruder);
        #endif
        lcdDrawUpdate = 1;
    }
    if (lcdDrawUpdate)
    {
        lcd_implementation_drawedit(PSTR("Z"), ftostr31(current_position[Z_AXIS]));
    }
    if (LCD_CLICKED)
    {
        lcd_quick_feedback();
        currentMenu = lcd_move_menu_axis;
        encoderPosition = 0;
    }
}

static void lcd_move_e()
{
    if (encoderPosition != 0)
    {
        current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale;
        encoderPosition = 0;
        #ifdef DELTA
        calculate_delta(current_position);
        plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder);
        #else
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS]/60, active_extruder);
        #endif
        lcdDrawUpdate = 1;
    }
    if (lcdDrawUpdate)
    {
        lcd_implementation_drawedit(PSTR("Extruder"), ftostr31(current_position[E_AXIS]));
    }
    if (LCD_CLICKED)
    {
        lcd_quick_feedback();
        currentMenu = lcd_move_menu_axis;
        encoderPosition = 0;
    }
}

static void lcd_menu_change_material_preheat()
{
    run_history = true;
    setTargetHotend(material[active_extruder].temperature, active_extruder);
    int16_t temp = degHotend(active_extruder) - 20;
    int16_t target = degTargetHotend(active_extruder) - 20 - 10;
    if (temp < 0) temp = 0;
    if (temp > target && !is_command_queued())
        {
            set_extrude_min_temp(0);
            for(uint8_t e=0; e<EXTRUDERS; e++)
                volume_to_filament_length[e] = 1.0;//Set the extrusion to 1mm per given value, so we can move the filament a set distance.

            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], 20.0, retract_feedrate/60.0, active_extruder);

            float old_max_feedrate_e = max_feedrate[E_AXIS];
            float old_retract_acceleration = retract_acceleration;
            max_feedrate[E_AXIS] = FILAMENT_REVERSAL_SPEED;
            retract_acceleration = FILAMENT_LONG_MOVE_ACCELERATION;

            current_position[E_AXIS] = 0;
            plan_set_e_position(current_position[E_AXIS]);
            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], -1.0, FILAMENT_REVERSAL_SPEED, active_extruder);
            for(uint8_t n=0; n<6; n++)
                plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], (n+1)*-FILAMENT_REVERSAL_LENGTH/6, FILAMENT_REVERSAL_SPEED, active_extruder);

            max_feedrate[E_AXIS] = old_max_feedrate_e;
            retract_acceleration = old_retract_acceleration;

            currentMenu = lcd_menu_change_material_remove;
            temp = target;
        }

    uint8_t progress = uint8_t(temp * 125 / target);
    if (progress < minProgress)
        progress = minProgress;
    else
        minProgress = progress;

    lcd_info_screen(lcd_menu_material_main, cancelMaterialInsert);
    lcd_lib_draw_stringP(3, 0, PSTR("Heating printhead"));
    lcd_lib_draw_stringP(3, 10, PSTR("for material removal"));

    char buffer[20];
    memset (buffer,0,sizeof(buffer));


    char* c;

    c = int_to_string(temp, buffer/*, PSTR( DEGREE_C_SYMBOL )*/);
    *c++ = TEMPERATURE_SEPARATOR;
    c = int_to_string(target, c, PSTR( DEGREE_C_SYMBOL ));
    lcd_lib_draw_string_center(20, buffer);

    lcd_progressbar(progress);
    LED_HEAT();
    lcd_lib_update_screen();
}

static void lcd_menu_change_material_preheat()
{
#ifdef USE_CHANGE_TEMPERATURE
    setTargetHotend(material[active_extruder].change_temperature, active_extruder);
#else
    setTargetHotend(material[active_extruder].temperature, active_extruder);
#endif
    int16_t temp = degHotend(active_extruder) - 20;
    int16_t target = degTargetHotend(active_extruder) - 20;
    if (temp < 0) temp = 0;
    if (temp > target - 5 && temp < target + 5)
    {
        if ((signed long)(millis() - preheat_end_time) > 0)
        {
            set_extrude_min_temp(0);
            
            plan_set_e_position(0);
            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], 20.0 / volume_to_filament_length[active_extruder], retract_feedrate/60.0, active_extruder);

            float old_max_feedrate_e = max_feedrate[E_AXIS];
            float old_retract_acceleration = retract_acceleration;
            max_feedrate[E_AXIS] = FILAMENT_REVERSAL_SPEED;
            retract_acceleration = FILAMENT_LONG_MOVE_ACCELERATION;

            plan_set_e_position(0);
            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], -1.0 / volume_to_filament_length[active_extruder], FILAMENT_REVERSAL_SPEED, active_extruder);
            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], -FILAMENT_REVERSAL_LENGTH / volume_to_filament_length[active_extruder], FILAMENT_REVERSAL_SPEED, active_extruder);

            max_feedrate[E_AXIS] = old_max_feedrate_e;
            retract_acceleration = old_retract_acceleration;

            currentMenu = lcd_menu_change_material_remove;
            temp = target;
        }
    }
    else
    {
#ifdef USE_CHANGE_TEMPERATURE
        preheat_end_time = millis() + (unsigned long)material[active_extruder].change_preheat_wait_time * 1000L;
#else
        preheat_end_time = millis();
#endif
    }

    uint8_t progress = uint8_t(temp * 125 / target);
    if (progress < minProgress)
        progress = minProgress;
    else
        minProgress = progress;

    lcd_info_screen(post_change_material_menu, cancelMaterialInsert);
    lcd_lib_draw_stringP(3, 10, PSTR("Heating printhead"));
    lcd_lib_draw_stringP(3, 20, PSTR("for material removal"));
    
    lcd_progressbar(progress);

    lcd_lib_update_screen();
}

static void lcd_extrude(float length, float feedrate) {
	current_position[E_AXIS] += length;
  #ifdef DELTA
  calculate_delta(current_position);
  plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], feedrate, active_extruder);
  #else
  plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate, active_extruder);
  #endif
}

      void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)
    #endif

#endif
{
  // If enabled, check for soft limit violations. Placed here all line motions are picked up
  // from everywhere in Grbl.
  if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { limits_soft_check(target); }

  // If in check gcode mode, prevent motion by blocking planner. Soft limits still work.
  if (sys.state == STATE_CHECK_MODE) { return; }

  // NOTE: Backlash compensation may be installed here. It will need direction info to track when
  // to insert a backlash line motion(s) before the intended line motion and will require its own
  // plan_check_full_buffer() and check for system abort loop. Also for position reporting
  // backlash steps will need to be also tracked, which will need to be kept at a system level.
  // There are likely some other things that will need to be tracked as well. However, we feel
  // that backlash compensation should NOT be handled by Grbl itself, because there are a myriad
  // of ways to implement it and can be effective or ineffective for different CNC machines. This
  // would be better handled by the interface as a post-processor task, where the original g-code
  // is translated and inserts backlash motions that best suits the machine.
  // NOTE: Perhaps as a middle-ground, all that needs to be sent is a flag or special command that
  // indicates to Grbl what is a backlash compensation motion, so that Grbl executes the move but
  // doesn't update the machine position values. Since the position values used by the g-code
  // parser and planner are separate from the system machine positions, this is doable.

  // If the buffer is full: good! That means we are well ahead of the robot.
  // Remain in this loop until there is room in the buffer.
  do {
    protocol_execute_runtime(); // Check for any run-time commands
    if (sys.abort) { return; } // Bail, if system abort.
    if ( plan_check_full_buffer() ) { protocol_auto_cycle_start(); } // Auto-cycle start when buffer is full.
    else { break; }
  } while (1);

  #ifdef USE_LINE_NUMBERS
    plan_buffer_line(target, feed_rate, invert_feed_rate, line_number);
  #else
    #ifdef VARIABLE_SPINDLE
      // NNW
      plan_buffer_line(target, feed_rate, invert_feed_rate,rpm);
    #else
      plan_buffer_line(target, feed_rate, invert_feed_rate);
    #endif

  #endif

  // If idle, indicate to the system there is now a planned block in the buffer ready to cycle
  // start. Otherwise ignore and continue on.
  if (!sys.state) { sys.state = STATE_QUEUED; }

}

static void doStartPrint()
{
	// zero the extruder position
	current_position[E_AXIS] = 0.0;
	plan_set_e_position(0);

	// since we are going to prime the nozzle, forget about any G10/G11 retractions that happened at end of previous print
	retracted = false;

	// note that we have primed, so that we know to de-prime at the end
	primed = true;

	// move to priming height
    current_position[Z_AXIS] = PRIMING_HEIGHT;
    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS], 0);

    for(uint8_t e = 0; e<EXTRUDERS; e++)
    {
        if (!LCD_DETAIL_CACHE_MATERIAL(e))
        {
        	// don't prime the extruder if it isn't used in the (Ulti)gcode
        	// traditional gcode files typically won't have the Material lines at start, so we won't prime for those
        	// Also, on dual/multi extrusion files, only prime the extruders that are used in the gcode-file.
            continue;
        }
        active_extruder = e;

        // undo the end-of-print retraction
        plan_set_e_position((0.0 - END_OF_PRINT_RETRACTION) / volume_to_filament_length[e]);
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], END_OF_PRINT_RECOVERY_SPEED, e);

        // perform additional priming
        plan_set_e_position(-PRIMING_MM3);
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], (PRIMING_MM3_PER_SEC * volume_to_filament_length[e]), e);

        // for extruders other than the first one, perform end of print retraction
        if (e > 0)
        {
            plan_set_e_position((END_OF_PRINT_RETRACTION) / volume_to_filament_length[e]);
            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], retract_feedrate/60, e);
        }
    }
    active_extruder = 0;

    postMenuCheck = checkPrintFinished;
    card.startFileprint();
    lifetime_stats_print_start();
    starttime = millis();
}

static void lcd_menu_first_run_bed_level_center_adjust()
{
    LED_GLOW();
    
    if (lcd_lib_encoder_pos == ENCODER_NO_SELECTION)
        lcd_lib_encoder_pos = 0;
    
    if (printing_state == PRINT_STATE_NORMAL && lcd_lib_encoder_pos != 0 && movesplanned() < 4)
    {
        current_position[Z_AXIS] -= float(lcd_lib_encoder_pos) * 0.05;
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 60, 0);
    }
    lcd_lib_encoder_pos = 0;

    if (movesplanned() > 0)
        lcd_info_screen(NULL, NULL, PSTR("CONTINUE"));
    else
        lcd_info_screen(lcd_menu_first_run_bed_level_left_adjust, storeHomingZ_parkHeadForLeftAdjustment, PSTR("CONTINUE"));
    DRAW_PROGRESS_NR(4);
    lcd_lib_draw_string_centerP(10, PSTR("Rotate the button"));
    lcd_lib_draw_string_centerP(20, PSTR("until the nozzle is"));
    lcd_lib_draw_string_centerP(30, PSTR("a millimeter away"));
    lcd_lib_draw_string_centerP(40, PSTR("from the buildplate."));
    lcd_lib_update_screen();
}

static void lcd_menu_first_run_bed_level_paper_center()
{
    LED_GLOW();

    if (lcd_lib_encoder_pos == ENCODER_NO_SELECTION)
        lcd_lib_encoder_pos = 0;

    if (printing_state == PRINT_STATE_NORMAL && lcd_lib_encoder_pos != 0 && movesplanned() < 4)
    {
        current_position[Z_AXIS] -= float(lcd_lib_encoder_pos) * 0.05;
        lcd_lib_encoder_pos = 0;
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 60, 0);
    }

    if (movesplanned() > 0)
        lcd_info_screen(NULL, NULL, PSTR("POKRACOVAT"));
    else
        lcd_info_screen(lcd_menu_first_run_bed_level_paper_left, parkHeadForLeftAdjustment, PSTR("POKRACOVAT"));
    DRAW_PROGRESS_NR_IF_NOT_DONE(8);
    lcd_lib_draw_string_centerP(10, PSTR("Zasunte papir mezi"));
    lcd_lib_draw_string_centerP(20, PSTR("trysku a podlozku,"));
    lcd_lib_draw_string_centerP(30, PSTR("otacejte dokud"));
    lcd_lib_draw_string_centerP(40, PSTR("papir neklade odpor"));
    lcd_lib_update_screen();
}

static void lcd_menu_first_run_bed_level_center_adjust()
{
    LED_GLOW();

    if (lcd_lib_encoder_pos == ENCODER_NO_SELECTION)
        lcd_lib_encoder_pos = 0;

    if (printing_state == PRINT_STATE_NORMAL && lcd_lib_encoder_pos != 0 && movesplanned() < 4)
    {
        current_position[Z_AXIS] -= float(lcd_lib_encoder_pos) * 0.05;
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 60, 0);
    }
    lcd_lib_encoder_pos = 0;

    if (movesplanned() > 0)
        lcd_info_screen(NULL, NULL, PSTR("POKRACOVAT"));
    else
        lcd_info_screen(lcd_menu_first_run_bed_level_left_adjust, parkHeadForLeftAdjustment, PSTR("POKRACOVAT"));
    DRAW_PROGRESS_NR_IF_NOT_DONE(4);
    lcd_lib_draw_string_centerP(10, PSTR("Otacejte tlacitkem"));
    lcd_lib_draw_string_centerP(20, PSTR("dokud nebude tryska"));
    lcd_lib_draw_string_centerP(30, PSTR("priblizne milimetr"));
    lcd_lib_draw_string_centerP(40, PSTR("od tiskove podlozky."));
    lcd_lib_update_screen();
}

// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// (1 minute)/feed_rate time.
// NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line 
// segments, must pass through this routine before being passed to the planner. The seperation of
// mc_line and plan_buffer_line is done primarily to make backlash compensation integration simple
// and direct.
// TODO: Check for a better way to avoid having to push the arguments twice for non-backlash cases.
// However, this keeps the memory requirements lower since it doesn't have to call and hold two 
// plan_buffer_lines in memory. Grbl only has to retain the original line input variables during a
// backlash segment(s).
void mc_line(double x, double y, double z, double c, double feed_rate, uint8_t invert_feed_rate)
{
  // TODO: Backlash compensation may be installed here. Only need direction info to track when
  // to insert a backlash line motion(s) before the intended line motion. Requires its own
  // plan_check_full_buffer() and check for system abort loop. Also for position reporting 
  // backlash steps will need to be also tracked. Not sure what the best strategy is for this,
  // i.e. keep the planner independent and do the computations in the status reporting, or let
  // the planner handle the position corrections. The latter may get complicated.

  // If the buffer is full: good! That means we are well ahead of the robot. 
  // Remain in this loop until there is room in the buffer.
  do {
    protocol_execute_runtime(); // Check for any run-time commands
    if (sys.abort) { return; } // Bail, if system abort.
  } while ( plan_check_full_buffer() );  
  plan_buffer_line(x, y, z, c, feed_rate, invert_feed_rate);
  
  // Auto-cycle start immediately after planner finishes. Enabled/disabled by grbl settings. During 
  // a feed hold, auto-start is disabled momentarily until the cycle is resumed by the cycle-start 
  // runtime command.
  // NOTE: This is allows the user to decide to exclusively use the cycle start runtime command to
  // begin motion or let grbl auto-start it for them. This is useful when: manually cycle-starting
  // when the buffer is completely full and primed; auto-starting, if there was only one g-code 
  // command sent during manual operation; or if a system is prone to buffer starvation, auto-start
  // helps make sure it minimizes any dwelling/motion hiccups and keeps the cycle going. 
  if (sys.auto_start) { st_cycle_start(); }
}

static void lcd_menu_first_run_bed_level_paper_center()
{
    LED_GLOW();
    
    if (lcd_lib_encoder_pos == ENCODER_NO_SELECTION)
        lcd_lib_encoder_pos = 0;
    
    if (printing_state == PRINT_STATE_NORMAL && lcd_lib_encoder_pos != 0 && movesplanned() < 4)
    {
        current_position[Z_AXIS] -= float(lcd_lib_encoder_pos) * 0.05;
        lcd_lib_encoder_pos = 0;
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 60, 0);
    }

    if (movesplanned() > 0)
        lcd_info_screen(NULL, NULL, PSTR("CONTINUE"));
    else
        lcd_info_screen(lcd_menu_first_run_bed_level_paper_left, storeHomingZ_parkHeadForLeftAdjustment, PSTR("CONTINUE"));
    DRAW_PROGRESS_NR(8);
    lcd_lib_draw_string_centerP(10, PSTR("Slide a paper between"));
    lcd_lib_draw_string_centerP(20, PSTR("buildplate and nozzle"));
    lcd_lib_draw_string_centerP(30, PSTR("until you feel a"));
    lcd_lib_draw_string_centerP(40, PSTR("bit resistance."));
    lcd_lib_update_screen();
}

static void lcd_move_x()
{
    if (encoderPosition != 0)
    {
        // Original Line.
        // current_position[X_AXIS] += float((int)encoderPosition) * move_menu_scale;
        // On Prusa i3v turning the knob to the right moves the X to the left so we invert it 
        // here.
        current_position[X_AXIS] -= float((int)encoderPosition) * move_menu_scale;
        if (current_position[X_AXIS] < X_MIN_POS)
            current_position[X_AXIS] = X_MIN_POS;
        if (current_position[X_AXIS] > X_MAX_POS)
            current_position[X_AXIS] = X_MAX_POS;
        encoderPosition = 0;
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 600, active_extruder);
        lcdDrawUpdate = 1;
    }
    if (lcdDrawUpdate)
    {
        lcd_implementation_drawedit(PSTR("X"), ftostr31(current_position[X_AXIS]));
    }
    if (LCD_CLICKED)
    {
        lcd_quick_feedback();
        currentMenu = lcd_move_menu_axis;
        encoderPosition = 0;
    }
}

static void materialInsertReady()
{
    current_position[E_AXIS] -= END_OF_PRINT_RETRACTION;
    plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 25*60, active_extruder);
    cancelMaterialInsert();

}

// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// (1 minute)/feed_rate time.
// NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line 
// segments, must pass through this routine before being passed to the planner. The seperation of
// mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being
// in the planner and to let backlash compensation or canned cycle integration simple and direct.
void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)
{
  // If enabled, check for soft limit violations. Placed here all line motions are picked up
  // from everywhere in Grbl.
  if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { limits_soft_check(target); }    
      
  // If in check gcode mode, prevent motion by blocking planner. Soft limits still work.
  if (sys.state == STATE_CHECK_MODE) { return; }
    
  // TODO: Backlash compensation may be installed here. Only need direction info to track when
  // to insert a backlash line motion(s) before the intended line motion. Requires its own
  // plan_check_full_buffer() and check for system abort loop. Also for position reporting 
  // backlash steps will need to be also tracked. Not sure what the best strategy is for this,
  // i.e. keep the planner independent and do the computations in the status reporting, or let
  // the planner handle the position corrections. The latter may get complicated.
  // TODO: Backlash comp positioning values may need to be kept at a system level, i.e. tracking 
  // true position after a feed hold in the middle of a backlash move. The difficulty is in making 
  // sure that the stepper subsystem and planner are working in sync, and the status report 
  // position also takes this into account.

  // If the buffer is full: good! That means we are well ahead of the robot. 
  // Remain in this loop until there is room in the buffer.
  do {
    protocol_execute_runtime(); // Check for any run-time commands
    if (sys.abort) { return; } // Bail, if system abort.
    if ( plan_check_full_buffer() ) { mc_auto_cycle_start(); } // Auto-cycle start when buffer is full.
    else { break; }
  } while (1);

  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);

  // If idle, indicate to the system there is now a planned block in the buffer ready to cycle 
  // start. Otherwise ignore and continue on.
  if (!sys.state) { sys.state = STATE_QUEUED; }
}

int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */
{
  /* Write your local variable definition here */

  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
      PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/

 setup();
  plan_buffer_line(2,1);
  //
  for(;;){ST_PULSE_TI_Enable();}


  //heat_manager();
  //inactivity_manager();




  /* Write your code here */
  /* For example: for(;;) { } */

  /*** Don't write any code pass this line, or it will be deleted during code generation. ***/
  /*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
  #ifdef PEX_RTOS_START
    PEX_RTOS_START();                  /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
  #endif
  /*** End of RTOS startup code.  ***/
  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

static void _lcd_move(const char *name, int axis, int min, int max) {
  if (encoderPosition != 0) {
    refresh_cmd_timeout();
    current_position[axis] += float((int)encoderPosition) * move_menu_scale;
    if (min_software_endstops && current_position[axis] < min) current_position[axis] = min;
    if (max_software_endstops && current_position[axis] > max) current_position[axis] = max;
    encoderPosition = 0;
    #ifdef DELTA
      calculate_delta(current_position);
      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder);
    #else
      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder);
    #endif
    lcdDrawUpdate = 1;
  }
  if (lcdDrawUpdate) lcd_implementation_drawedit(name, ftostr31(current_position[axis]));
  if (LCD_CLICKED) lcd_goto_menu(lcd_move_menu_axis);
}

/**
*** MoveTo() width specific speed (%)
**/
void LaosMotion::moveTo(int x, int y, int z, int speed)
{
   extern GlobalConfig *cfg;
   action.target.x = ofsx + x/1000.0;
   action.target.y = ofsy + y/1000.0;
   action.target.z = ofsz + z/1000.0;
   action.ActionType = AT_MOVE;
   action.target.feed_rate =  (speed * 60.0 * cfg->speed) / 100;
   plan_buffer_line(&action);
   //printf("To buffer: %d, %d, %d, %d\n", x, y,z,speed);
}

/**
*** LaosMotion() Constructor
*** Make new motion object
**/
LaosMotion::LaosMotion()
{
  extern GlobalConfig *cfg;
#if DO_MOTION_TEST
  tActionRequest act[2];
  int i=0;
  Timer t;
#endif
  pwm.period(1.0 / cfg->pwmfreq);
  pwm = cfg->pwmmin/100.0;
  if ( laser == NULL ) laser = new DigitalOut(LASER_PIN);
  *laser = LASEROFF;

  mark_speed = cfg->speed;
  //start.mode(PullUp);
  xhome.mode(PullUp);
  yhome.mode(PullUp);
  isHome = false;
  plan_init();
  st_init();
  reset();
  mark_speed = cfg->speed;
  bitmap_speed = cfg->xspeed;
  action.param = 0;
  action.target.x = action.target.y = action.target.z = action.target.e =0;
  action.target.feed_rate = 60*mark_speed;

#if DO_MOTION_TEST
  t.start();
  act[0].ActionType = act[1].ActionType =  AT_MOVE;
  act[0].target.feed_rate = 60 * 100;
  act[1].target.feed_rate = 60 * 200;
  act[0].target.x = act[0].target.y = act[0].target.z = act[0].target.e = 0;
  act[1].target.x = act[1].target.y = act[1].target.z = act[1].target.e = 100;
  act[1].target.y = 200;
  while(1)
  {
    while( plan_queue_full() ) led3 = !led3;
    led1 = 1;
    i++;
    if ( i )
      printf("%d PING...\n", t.read_ms());
    else
      printf("%d PONG...\n", t.read_ms());
    if ( i > 1 || i<0) i = 0;
    plan_buffer_line (&act[i]);
    led1 = 0;
  }
#endif

}

static void doStartPrint()
{
    for(uint8_t e = 0; e<EXTRUDERS; e++)
    {
        if (!LCD_DETAIL_CACHE_MATERIAL(e))
            continue;
        active_extruder = e;
        plan_set_e_position(-25.0 / volume_to_filament_length[e]);
        current_position[E_AXIS] = 0.0;
        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 25, e);
        
        if (e > 0)
        {
            plan_set_e_position(20.0 / volume_to_filament_length[e]);
            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 35, e);
        }
    }
    active_extruder = 0;
    
    postMenuCheck = checkPrintFinished;
    card.startFileprint();
    starttime = millis();
}