void lcd_loading_filament(){
lcd_implementation_clear();
lcd.setCursor(0, 0);
lcd.print(MSG_LOADING_FILAMENT);
lcd.setCursor(0, 2);
lcd.print(MSG_PLEASE_WAIT);
for(int i = 0; i<20; i++){
lcd.setCursor(i, 3);
lcd.print(".");
for(int j = 0;j<10 ; j++){
manage_heater();
manage_inactivity(true);
delay(110);
}
}
}
void loop() {
get_command();
manage_heater();
manage_inactivity(1); //shutdown if not receiving any new commands
}
/*-------------------------------------------------
* And the primary command parser loop
*/
void loop()
{
if(buflen < (BUFSIZE-1))
get_command();
if(buflen)
{
process_commands();
buflen = (buflen-1);
bufindr = (bufindr + 1)%BUFSIZE;
} else {
manage_inactivity();
}
}
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head)
{
manage_heater();
manage_inactivity();
lcd_update();
}
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS])
{
if(degHotend(active_extruder)<extrude_min_temp)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{
#ifdef EASY_LOAD
if (!allow_lengthy_extrude_once) {
#endif
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
#ifdef EASY_LOAD
}
allow_lengthy_extrude_once = false;
#endif
}
#endif // PREVENT_LENGTHY_EXTRUDE
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments)
{
return;
}
block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
block->direction_bits = 0;
#ifndef COREXY
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_AXIS);
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_AXIS);
}
#else
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<X_AXIS);
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<Y_AXIS);
}
#endif
if (target[Z_AXIS] < position[Z_AXIS])
{
block->direction_bits |= (1<<Z_AXIS);
}
if (target[E_AXIS] < position[E_AXIS])
{
block->direction_bits |= (1<<E_AXIS);
}
block->active_extruder = extruder;
//enable active axes
#ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0))
{
enable_x();
enable_y();
}
#else
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
#endif
// Enable all
if(block->steps_e != 0)
{
enable_e0();
enable_e1();
enable_e2();
}
if (block->steps_e == 0)
{
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else
{
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
float delta_mm[4];
#ifndef COREXY
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
{
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else
{
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
}
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
{
if (segment_time < minsegmenttime)
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0/inverse_second);
#endif
}
}
#endif
// END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++)
{
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
if((direction_change & (1<<X_AXIS)) == 0)
{
x_segment_time[0] += segment_time;
}
else
{
x_segment_time[2] = x_segment_time[1];
x_segment_time[1] = x_segment_time[0];
x_segment_time[0] = segment_time;
}
if((direction_change & (1<<Y_AXIS)) == 0)
{
y_segment_time[0] += segment_time;
}
else
{
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME)
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif
// Correct the speed
if( speed_factor < 1.0)
{
for(unsigned char i=0; i < 4; i++)
{
current_speed[i] *= speed_factor;
}
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count/block->millimeters;
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
{
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else
{
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
// Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
}
block->acceleration = block->acceleration_st / steps_per_mm;
block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk/2;
float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2)
vmax_junction = min(vmax_junction, max_z_jerk/2);
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2)
vmax_junction = min(vmax_junction, max_e_jerk/2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
// if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) {
vmax_junction_factor = (max_xy_jerk/jerk);
}
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
}
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
}
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) {
block->nominal_length_flag = true;
}
else {
block->nominal_length_flag = false;
}
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
safe_speed/block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
st_wake_up();
}
void lcd_adjust_z(){
int enc_dif = 0;
int cursor_pos = 1;
int fsm = 0;
lcd_implementation_clear();
lcd.setCursor(0, 0);
lcd.print(MSG_ADJUSTZ);
lcd.setCursor(1, 1);
lcd.print(MSG_YES);
lcd.setCursor(1, 2);
lcd.print(MSG_NO);
lcd.setCursor(0, 1);
lcd.print(">");
enc_dif = encoderDiff;
while(fsm == 0){
manage_heater();
manage_inactivity(true);
if( abs((enc_dif - encoderDiff))>4 ){
if ( (abs(enc_dif-encoderDiff)) > 1 ){
if (enc_dif > encoderDiff ){
cursor_pos --;
}
if (enc_dif < encoderDiff ){
cursor_pos ++;
}
if(cursor_pos >2){
cursor_pos = 2;
}
if(cursor_pos <1){
cursor_pos = 1;
}
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 2);
lcd.print(" ");
lcd.setCursor(0, cursor_pos);
lcd.print(">");
enc_dif = encoderDiff;
delay(100);
}
}
if(lcd_clicked()){
fsm = cursor_pos;
if(fsm == 1){
EEPROM_read_B(4093,&babystepMem[0]);
EEPROM_read_B(4091,&babystepMem[1]);
EEPROM_read_B(4089,&babystepMem[2]);
babystepsTodo[Z_AXIS] = babystepMem[2];
}else{
babystepMem[0] = 0;
babystepMem[1] = 0;
babystepMem[2] = 0;
EEPROM_save_B(4093,&babystepMem[0]);
EEPROM_save_B(4091,&babystepMem[1]);
EEPROM_save_B(4089,&babystepMem[2]);
}
delay(500);
}
};
lcd_implementation_clear();
lcd_return_to_status();
}
void lcd_alright(){
int enc_dif = 0;
int cursor_pos = 1;
lcd_implementation_clear();
lcd.setCursor(0, 0);
lcd.print(MSG_CORRECTLY);
lcd.setCursor(1, 1);
lcd.print(MSG_YES);
lcd.setCursor(1, 2);
lcd.print(MSG_NOT_LOADED);
lcd.setCursor(1, 3);
lcd.print(MSG_NOT_COLOR);
lcd.setCursor(0, 1);
lcd.print(">");
enc_dif = encoderDiff;
while(lcd_change_fil_state == 0){
manage_heater();
manage_inactivity(true);
if( abs((enc_dif - encoderDiff))>4 ){
if ( (abs(enc_dif-encoderDiff)) > 1 ){
if (enc_dif > encoderDiff ){
cursor_pos --;
}
if (enc_dif < encoderDiff ){
cursor_pos ++;
}
if(cursor_pos >3){
cursor_pos = 3;
}
if(cursor_pos <1){
cursor_pos = 1;
}
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 2);
lcd.print(" ");
lcd.setCursor(0, 3);
lcd.print(" ");
lcd.setCursor(0, cursor_pos);
lcd.print(">");
enc_dif = encoderDiff;
delay(100);
}
}
if(lcd_clicked()){
lcd_change_fil_state = cursor_pos;
delay(500);
}
};
lcd_implementation_clear();
lcd_return_to_status();
}
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
#endif //LEVEL_SENSOR
{
// Calculate the buffer head after we push this byte
next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
#ifndef DOGLCD
buffer_recursivity++;
#endif // DOGLCD
while(block_buffer_tail == next_buffer_head)
{
next_buffer_head = next_block_index(block_buffer_head);
temp::TemperatureManager::single::instance().manageTemperatureControl();
#ifndef DOGLCD
manage_inactivity();
#endif //DOGLCD
lcd_update();
#ifdef DOGLCD
if (stop_planner_buffer == true)
{
stop_planner_buffer = false;
planner_buffer_stopped = true;
return;
}
#endif // DOGLCD
}
#ifndef DOGLCD
buffer_recursivity--;
#endif // DOGLCD
#ifdef LEVEL_SENSOR
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // LEVEL_SENSOR
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS])
{
if(degHotend(active_extruder)<extrude_min_temp)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= volumetric_multiplier[active_extruder];
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments)
{
return;
}
block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
block->direction_bits = 0;
#ifndef COREXY
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_AXIS);
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_AXIS);
}
#else
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_HEAD); //AlexBorro: Save the real Extruder (head) direction in X Axis
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_HEAD); //AlexBorro: Save the real Extruder (head) direction in Y Axis
}
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<X_AXIS); //AlexBorro: Motor A direction (Incorrectly implemented as X_AXIS)
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<Y_AXIS); //AlexBorro: Motor B direction (Incorrectly implemented as Y_AXIS)
}
#endif
if (target[Z_AXIS] < position[Z_AXIS])
{
block->direction_bits |= (1<<Z_AXIS);
}
if (target[E_AXIS] < position[E_AXIS])
{
block->direction_bits |= (1<<E_AXIS);
}
block->active_extruder = extruder;
//enable active axes
#ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0))
{
enable_x();
enable_y();
}
#else
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
#endif
// Enable extruder(s)
if(block->steps_e != 0)
{
if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder
{
if(g_uc_extruder_last_move[0] > 0) g_uc_extruder_last_move[0]--;
if(g_uc_extruder_last_move[1] > 0) g_uc_extruder_last_move[1]--;
if(g_uc_extruder_last_move[2] > 0) g_uc_extruder_last_move[2]--;
if(g_uc_extruder_last_move[3] > 0) g_uc_extruder_last_move[3]--;
switch(extruder)
{
case 0:
enable_e0();
g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 1:
enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 2:
enable_e2();
g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 3:
enable_e3();
g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
break;
}
}
else //enable all
{
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
}
if (block->steps_e == 0)
{
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else
{
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
/* This part of the code calculates the total length of the movement.
For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
#ifndef COREXY
float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
float delta_mm[6];
delta_mm[X_HEAD] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_HEAD] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
{
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else
{
#ifndef COREXY
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
#else
block->millimeters = sqrt(square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]));
#endif
}
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
{
if (segment_time < minsegmenttime)
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0/inverse_second);
#endif
}
}
#endif
// END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#ifdef FILAMENT_SENSOR
//FMM update ring buffer used for delay with filament measurements
if((extruder==FILAMENT_SENSOR_EXTRUDER_NUM) && (delay_index2 > -1)) //only for extruder with filament sensor and if ring buffer is initialized
{
delay_dist = delay_dist + delta_mm[E_AXIS]; //increment counter with next move in e axis
while (delay_dist >= (10*(MAX_MEASUREMENT_DELAY+1))) //check if counter is over max buffer size in mm
delay_dist = delay_dist - 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
while (delay_dist<0)
delay_dist = delay_dist + 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
delay_index1=delay_dist/10.0; //calculate index
//ensure the number is within range of the array after converting from floating point
if(delay_index1<0)
delay_index1=0;
else if (delay_index1>MAX_MEASUREMENT_DELAY)
delay_index1=MAX_MEASUREMENT_DELAY;
if(delay_index1 != delay_index2) //moved index
{
meas_sample=widthFil_to_size_ratio()-100; //subtract off 100 to reduce magnitude - to store in a signed char
}
while( delay_index1 != delay_index2)
{
delay_index2 = delay_index2 + 1;
if(delay_index2>MAX_MEASUREMENT_DELAY)
delay_index2=delay_index2-(MAX_MEASUREMENT_DELAY+1); //loop around buffer when incrementing
if(delay_index2<0)
delay_index2=0;
else if (delay_index2>MAX_MEASUREMENT_DELAY)
delay_index2=MAX_MEASUREMENT_DELAY;
measurement_delay[delay_index2]=meas_sample;
}
}
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++)
{
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
if((direction_change & (1<<X_AXIS)) == 0)
{
x_segment_time[0] += segment_time;
}
else
{
x_segment_time[2] = x_segment_time[1];
x_segment_time[1] = x_segment_time[0];
x_segment_time[0] = segment_time;
}
if((direction_change & (1<<Y_AXIS)) == 0)
{
y_segment_time[0] += segment_time;
}
else
{
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME)
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif // XY_FREQUENCY_LIMIT
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters;
long bsx = block->steps_x, bsy = block->steps_y, bsz = block->steps_z, bse = block->steps_e;
if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else if (bse == 0) {
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
// Limit acceleration per axis
unsigned long acc_st = block->acceleration_st,
xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS];
if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0;
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dz = fabs(csz - previous_speed[Z_AXIS]),
de = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dx * dx + dy * dy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUSION_AREA * EXTRUSION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate();
st_wake_up();
}
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
#endif //ENABLE_AUTO_BED_LEVELING
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) {
manage_heater();
manage_inactivity();
lcd_update();
}
#ifdef MESH_BED_LEVELING
if (mbl.active) z += mbl.get_z(x, y);
#endif
#ifdef ENABLE_AUTO_BED_LEVELING
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[NUM_AXIS];
target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
float dx = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS],
de = target[E_AXIS] - position[E_AXIS];
#ifdef PREVENT_DANGEROUS_EXTRUDE
if (de) {
if (degHotend(active_extruder) < extrude_min_temp) {
position[E_AXIS] = target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#ifdef PREVENT_LENGTHY_EXTRUDE
if (labs(de) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#ifdef COREXY
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps[A_AXIS] = labs(dx + dy);
block->steps[B_AXIS] = labs(dx - dy);
#else
// default non-h-bot planning
block->steps[X_AXIS] = labs(dx);
block->steps[Y_AXIS] = labs(dy);
#endif
block->steps[Z_AXIS] = labs(dz);
block->steps[E_AXIS] = labs(de);
block->steps[E_AXIS] *= volumetric_multiplier[active_extruder];
block->steps[E_AXIS] *= extruder_multiply[active_extruder];
block->steps[E_AXIS] /= 100;
block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments) return;
block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
uint8_t db = 0;
#ifdef COREXY
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) db |= BIT(Y_HEAD); // ...and Y
if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
#else
if (dx < 0) db |= BIT(X_AXIS);
if (dy < 0) db |= BIT(Y_AXIS);
#endif
if (dz < 0) db |= BIT(Z_AXIS);
if (de < 0) db |= BIT(E_AXIS);
block->direction_bits = db;
block->active_extruder = extruder;
//enable active axes
#ifdef COREXY
if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
enable_x();
enable_y();
}
#else
if (block->steps[X_AXIS]) enable_x();
if (block->steps[Y_AXIS]) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if (block->steps[Z_AXIS]) enable_z();
#endif
// Enable extruder(s)
if (block->steps[E_AXIS]) {
if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder
for (int i=0; i<EXTRUDERS; i++)
if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
switch(extruder) {
case 0:
enable_e0();
g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE * 2;
#if EXTRUDERS > 1
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
#endif
break;
#if EXTRUDERS > 1
case 1:
enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
break;
#if EXTRUDERS > 2
case 2:
enable_e2();
g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
break;
#if EXTRUDERS > 3
case 3:
enable_e3();
g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
if (g_uc_extruder_last_move[2] == 0) disable_e2();
break;
#endif // EXTRUDERS > 3
#endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1
}
}
else { // enable all
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
}
if (block->steps[E_AXIS]) {
if (feed_rate < minimumfeedrate) feed_rate = minimumfeedrate;
}
else if (feed_rate < mintravelfeedrate) feed_rate = mintravelfeedrate;
/**
* This part of the code calculates the total length of the movement.
* For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
* But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
* and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
* So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
#ifdef COREXY
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
#else
float delta_mm[4];
delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[active_extruder] * extruder_multiply[active_extruder] / 100.0;
if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else {
block->millimeters = sqrt(
#ifdef COREXY
square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD])
#else
square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS])
#endif
+ square(delta_mm[Z_AXIS])
);
}
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued = movesplanned();
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
#if defined(OLD_SLOWDOWN) || defined(SLOWDOWN)
bool mq = moves_queued > 1 && moves_queued < BLOCK_BUFFER_SIZE / 2;
#ifdef OLD_SLOWDOWN
if (mq) feed_rate *= 2.0 * moves_queued / BLOCK_BUFFER_SIZE;
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if (mq) {
if (segment_time < minsegmenttime) {
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0 / inverse_second);
#endif
}
}
#endif
#endif
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#ifdef FILAMENT_SENSOR
//FMM update ring buffer used for delay with filament measurements
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized
const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10;
delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis
while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer
while (delay_dist < 0) delay_dist += MMD10;
delay_index1 = delay_dist / 10.0; // calculate index
delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above)
if (delay_index1 != delay_index2) { // moved index
meas_sample = widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char
while (delay_index1 != delay_index2) {
// Increment and loop around buffer
if (++delay_index2 >= MMD) delay_index2 -= MMD;
delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY);
measurement_delay[delay_index2] = meas_sample;
}
}
}
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[NUM_AXIS];
float speed_factor = 1.0; //factor <=1 do decrease speed
for (int i = 0; i < NUM_AXIS; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
float cs = fabs(current_speed[i]), mf = max_feedrate[i];
if (cs > mf) speed_factor = min(speed_factor, mf / cs);
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
long xs0 = axis_segment_time[X_AXIS][0],
xs1 = axis_segment_time[X_AXIS][1],
xs2 = axis_segment_time[X_AXIS][2],
ys0 = axis_segment_time[Y_AXIS][0],
ys1 = axis_segment_time[Y_AXIS][1],
ys2 = axis_segment_time[Y_AXIS][2];
if ((direction_change & BIT(X_AXIS)) != 0) {
xs2 = axis_segment_time[X_AXIS][2] = xs1;
xs1 = axis_segment_time[X_AXIS][1] = xs0;
xs0 = 0;
}
xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;
if ((direction_change & BIT(Y_AXIS)) != 0) {
ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
ys0 = 0;
}
ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;
long max_x_segment_time = max(xs0, max(xs1, xs2)),
max_y_segment_time = max(ys0, max(ys1, ys2)),
min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
if (min_xy_segment_time < MAX_FREQ_TIME) {
float low_sf = speed_factor * min_xy_segment_time / MAX_FREQ_TIME;
speed_factor = min(speed_factor, low_sf);
}
#endif // XY_FREQUENCY_LIMIT
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters;
long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else if (bse == 0) {
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
// Limit acceleration per axis
unsigned long acc_st = block->acceleration_st,
xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS];
if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0;
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dz = fabs(csz - previous_speed[Z_AXIS]),
de = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dx * dx + dy * dy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if (!bse || (!bsx && !bsy && !bsz)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (cse * cse * EXTRUSION_AREA * EXTRUSION_AREA) * 256;
block->advance = advance;
block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate();
st_wake_up();
} // plan_buffer_line()
void linear_move() { // make linear move with preset speeds and destinations, see G0 and G1
//Determine direction of movement
if (destination_x > current_x) {
p_X_dir = !INVERT_X_DIR;
} else {
p_X_dir = INVERT_X_DIR;
}
if (destination_y > current_y) {
p_Y_dir = !INVERT_Y_DIR;
} else {
p_Y_dir = INVERT_Y_DIR;
}
if (destination_z > current_z) {
p_Z_dir = !INVERT_Z_DIR;
} else {
p_Z_dir = INVERT_Z_DIR;
}
if (destination_e > current_e) {
p_E_dir = !INVERT_E_DIR;
} else {
p_E_dir = INVERT_E_DIR;
}
//Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration.
if (x_steps_remaining) enable_x();
if (y_steps_remaining) enable_y();
if (z_steps_remaining) enable_z();
if (e_steps_remaining) enable_e();
check_x_min_endstop();
check_y_min_endstop();
check_z_min_endstop();
previous_millis_heater = millis();
while (x_steps_remaining + y_steps_remaining + z_steps_remaining + e_steps_remaining > 0) { // move until no more steps remain
if (x_steps_remaining>0) {
if ((micros()-previous_micros_x) >= x_interval) {
do_x_step();
x_steps_remaining--;
}
check_x_min_endstop();
led1 = 1;
} else {
led1 = 0;
wait_us(2);
}
if (y_steps_remaining>0) {
if ((micros()-previous_micros_y) >= y_interval) {
do_y_step();
y_steps_remaining--;
}
check_y_min_endstop();
led2=1;
} else {
led2=0;
wait_us(2);
}
if (z_steps_remaining>0) {
if ((micros()-previous_micros_z) >= z_interval) {
do_z_step();
z_steps_remaining--;
}
check_z_min_endstop();
led3=1;
} else {
led3=0;
wait_us(2);
}
if (e_steps_remaining>0) {
if ((micros()-previous_micros_e) >= e_interval) {
do_e_step();
e_steps_remaining--;
led4=1;
}
} else {
led4=0;
wait_us(2);
}
if ( (millis() - previous_millis_heater) >= 500 ) {
manage_heater();
previous_millis_heater = millis();
manage_inactivity(2);
}
wait_us(2);
}
led1=0;
led2=0;
led3=0;
led4=0;
if (DISABLE_X) disable_x();
if (DISABLE_Y) disable_y();
if (DISABLE_Z) disable_z();
if (DISABLE_E) disable_e();
// Update current position partly based on direction, we probably can combine this with the direction code above...
if (destination_x > current_x) current_x = current_x + x_steps_to_take/x_steps_per_unit;
else current_x = current_x - x_steps_to_take/x_steps_per_unit;
if (destination_y > current_y) current_y = current_y + y_steps_to_take/y_steps_per_unit;
else current_y = current_y - y_steps_to_take/y_steps_per_unit;
if (destination_z > current_z) current_z = current_z + z_steps_to_take/z_steps_per_unit;
else current_z = current_z - z_steps_to_take/z_steps_per_unit;
if (destination_e > current_e) current_e = current_e + e_steps_to_take/e_steps_per_unit;
else current_e = current_e - e_steps_to_take/e_steps_per_unit;
}
/*----------------------------------------
*
* Start the massive Gcode processing loop
*-----------------------------------------
*/
void process_commands()
{
int result;
int motorChannel, motorPower;
unsigned long codenum;
char *starpos = NULL;
if(code_seen('G'))
{
switch((int)code_value())
{
case 0: // G0 -> G1
case 1: // G1
break;
case 2:
break;
case 3:
break;
case 4: // G4 dwell
//LCD_MESSAGEPGM(MSG_DWELL);
codenum = 0;
if(code_seen('P')) codenum = code_value(); // milliseconds to wait
if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
//codenum += millis(); // keep track of when we started waiting
previous_millis_cmd = 0;//millis();
while(++previous_millis_cmd < codenum ){
manage_inactivity();
_delay_ms(1);
}
break;
case 5: // G5 - Absolute command motor C<Channel> P<motor power -1000 to 1000>
if(!Stopped) {
if(code_seen('C')) {
motorChannel = code_value(); // motor channel 1,2
if(code_seen('P')) {
motorPower = code_value(); // motor power -1000,1000
fault = 0; // clear fault flag
motorControl->commandMotorPower(motorChannel, motorPower);
} // code P
} // code C
} // stopped
break;
case 28:
previous_millis_cmd = 0;
break;
case 90: // G90
break;
case 91: // G91
break;
case 92: // G92
break;
case 99: // G99 start watchdog timer. G99 T<time_in_millis> values are 15,30,60,120,250,500,1000,4000,8000 default 4000
if( code_seen('T') ) {
int time_val = code_value();
watchdog_timer = new WatchdogTimer();
watchdog_timer->watchdog_init(time_val);
}
break;
case 100: // G100 reset watchog timer before time interval is expired, otherwise a reset occurs
if( watchdog_timer != NULL )
watchdog_timer->watchdog_reset();
break;
} // switch
} // if code
/*-------------------------------------
* M Code processing
*--------------------------------------
*/
else if(code_seen('M'))
{
switch( (int)code_value() )
{
case 0: // M0 Set real time output off
realtime_output = 0;
break;
case 1: // M1 Set real time output on
realtime_output = 1;
break;
case 2: // M2 [C<channel> E<encoder pin>] set smart controller (default) with optional encoder pin per channel
motor_status = 0;
motorControl = &roboteqDevice;
if(code_seen('C')) {
channel = code_value();
if(code_seen('E')) {
pin_number = code_value();
motorControl->createEncoder(channel, pin_number);
}
}
break;
//Set PWM motor driver, map pin to channel, this will check to prevent free running motors during inactivity
//For a PWM motor control subsequent G5 commands are affected here.
case 3: // M3 P<pin> C<channel> D<direction pin> E<default dir> W<encoder pin> [T<timer mode 0-3>] [R<resolution 8,9,10 bits>] [X<prescale 0-7>]
motor_status = 1;
pin_number = -1;
encode_pin = 0;
motorControl = (AbstractMotorControl*)&hBridgeDriver;
((HBridgeDriver*)motorControl)->setMotors((PWM**)&ppwms);
((HBridgeDriver*)motorControl)->setDirectionPins((Digital**)&pdigitals);
if(code_seen('P'))
pin_number = code_value();
else
break;
if(code_seen('C')) {
channel = code_value();
if( code_seen('D'))
dir_pin = code_value();
else
break;
if( code_seen('E'))
dir_default = code_value();
else
break;
if( code_seen('W'))
encode_pin = code_value();
((HBridgeDriver*)motorControl)->createPWM(channel, pin_number, dir_pin, dir_default, timer_pre, timer_res);
if(encode_pin)
motorControl->createEncoder(channel, encode_pin);
} // code C
break;
case 4:
// BLDC motor
// BLDC: lo1,lo2,lo3,hi1,hi2,hi3,hall1,hall2,hall3,enable
// hall sensors hall1:bit0,hall2:bit1,hall2:bit2 of commu array offset
// BLDC3PhaseSensor* tmotor1 = new BLDC3PhaseSensor(29,31,33,8,9,10,64,65,66);
// tmotor2 = new BLDC3PhaseSensor(35,37,39,11,12,13,67,68,69,30);
// tmotor1->motor_init();
// tmotor2->Motor_init();
break;
case 11: // M11 C<channel> D<duration> Set maximum motor cycle duration for given channel.
if( code_seen('C') ) {
channel = code_value();
if( code_seen('D') )
motorControl->setDuration(channel, code_value());
}
break;
case 12: // M12 C<channel> P<offset> set amount to add to G5 for min motor power
if( code_seen('C') ) {
channel = code_value();
if( code_seen('P'))
motorControl->setMinMotorPower(channel, code_value());
}
break;
case 17:
break;
case 18: //M18
break;
case 31: //M31 take time since the start of the SD print or an M109 command
stoptime=0;//millis();
char time[30];
t =(stoptime-starttime)/1000;
int sec,min;
min=t/60;
sec=t%60;
sprintf_P(time, PSTR("%i min, %i sec"), min, sec);
SERIAL_PGMLN(timeCntrlHdr);
SERIAL_PGM("1 ");
SERIAL_PORT.println(time);
SERIAL_PORT.flush();
//lcd_setstatus(time);
//autotempShutdown();
break;
case 33: // M33 P<ultrasonic pin> D<min. distance in cm> [E<direction 1- forward facing, 0 - reverse facing sensor>]
// link Motor controller to ultrasonic sensor, the sensor must exist via M301
motor_status = 1;
pin_number = 0;
if(code_seen('P')) {
pin_number = code_value();
if( code_seen('D'))
dist = code_value();
else
break;
dir_face = 1; // default forward
if( code_seen('E'))
dir_face = code_value(); // optional
motorControl->linkDistanceSensor((Ultrasonic**)psonics, pin_number, dist, dir_face);
} // code_seen = 'P'
break;
case 35: //M35 - Clear all digital pins
for(int i = 0; i < 10; i++) {
if(pdigitals[i]) {
unassignPin(pdigitals[i]->pin);
delete pdigitals[i];
pdigitals[i] = 0;
}
}
break;
case 36: //M36 - Clear all analog pins
for(int i = 0; i < 10; i++) {
if(panalogs[i]) {
unassignPin(panalogs[i]->pin);
delete panalogs[i];
panalogs[i] = 0;
}
}
break;
case 37: //M36 - Clear all PWM pins
for(int i = 0; i < 10; i++) {
if(ppwms[i]) {
unassignPin(ppwms[i]->pin);
delete ppwms[i];
ppwms[i] = 0;
}
}
break;
case 38: //M38 - Remove PWM pin P<pin>
pin_number = -1;
if (code_seen('P')) {
pin_number = code_value();
if(unassignPin(pin_number) ) {
for(int i = 0; i < 10; i++) {
if(ppwms[i] && ppwms[i]->pin == pin_number) {
delete ppwms[i];
ppwms[i] = 0;
} // pwms == pin_number
} // i iterate pwm array
} // unassign pin
} // code P
break;
case 39: //M39 - Remove Persistent Analog pin P<pin>
pin_number = -1;
if (code_seen('P')) {
pin_number = code_value();
if(unassignPin(pin_number) ) {
for(int i = 0; i < 10; i++) {
if(panalogs[i] && panalogs[i]->pin == pin_number) {
delete panalogs[i];
panalogs[i] = 0;
break;
}
}
}
}
break;
case 40: //M40 - Remove persistent digital pin P<pin>
pin_number = -1;
if (code_seen('P')) {
pin_number = code_value();
if(unassignPin(pin_number) ) {
for(int i = 0; i < 10; i++) {
if(pdigitals[i] && pdigitals[i] == pin_number) {
delete pdigitals[i];
pdigitals[i] = 0;
break;
}
}
}
}
break;
case 41: //M41 - Create persistent digital pin, Write digital pin HIGH P<pin> (this gives you a 5v source on pin)
pin_number = -1;
if (code_seen('P')) {
pin_number = code_value();
if( assignPin(pin_number) ) {
dpin = new Digital(pin_number);
dpin->setPin(pin_number);
dpin->pinMode(OUTPUT);
dpin->digitalWrite(HIGH);
for(int i = 0; i < 10; i++) {
if(!pdigitals[i]) {
pdigitals[i] = dpin;
break;
}
}
} else {
for(int i = 0; i < 10; i++) {
if(pdigitals[i] && pdigitals[i]->pin == pin_number) {
pdigitals[i]->setPin(pin_number);
pdigitals[i]->pinMode(OUTPUT);
pdigitals[i]->digitalWrite(HIGH);
break;
}
}
}
}
break;
case 42: //M42 - Create persistent digital pin, Write digital pin LOW P<pin> (This gives you a grounded pin)
pin_number = -1;
if (code_seen('P')) {
pin_number = code_value();
if( assignPin(pin_number) ) {
dpin = new Digital(pin_number);
dpin->pinMode(OUTPUT);
dpin->digitalWrite(LOW);
for(int i = 0; i < 10; i++) {
if(!pdigitals[i]) {
pdigitals[i] = dpin;
break;
}
}
} else {
for(int i = 0; i < 10; i++) {
if(pdigitals[i] && pdigitals[i]->pin == pin_number) {
pdigitals[i]->setPin(pin_number);
pdigitals[i]->pinMode(OUTPUT);
pdigitals[i]->digitalWrite(LOW);
break;
}
}
}
}
break;
case 43: // M43 Read from digital pin P<pin>
pin_number = -1;
if (code_seen('P'))
pin_number = code_value();
if( assignPin(pin_number) ) {
dpin = new Digital(pin_number);
dpin->pinMode(INPUT);
int res = dpin->digitalRead();
delete &dpin;
unassignPin(pin_number); // reset it since this is a one-shot
SERIAL_PGMLN(digitalPinHdr);
SERIAL_PGM("1 ");
SERIAL_PORT.println(pin_number);
SERIAL_PGM("2 ");
SERIAL_PORT.println(res);
SERIAL_PORT.println();
SERIAL_PORT.flush();
}
break;
case 44: // M44 -Read digital pin with pullup P<pin>
pin_number = -1;
if (code_seen('P'))
pin_number = code_value();
if( assignPin(pin_number) ) {
dpin = new Digital(pin_number);
dpin->pinMode(INPUT_PULLUP);
int res = dpin->digitalRead();
delete &dpin;
unassignPin(pin_number); // reset it since this is a one-shot
SERIAL_PGMLN(digitalPinHdr);
SERIAL_PGM("1 ");
SERIAL_PORT.println(pin_number);
SERIAL_PGM("2 ");
SERIAL_PORT.println(res);
SERIAL_PORT.flush();
}
break;
// PWM value between 0 and 255, default timer mode is 2; clear on match, default resolution is 8 bits, default prescale is 1
// Prescale: 1,2,4,6,7,8,9 = none, 8, 64, 256, 1024, external falling, external rising
// Use M445 to disable pin permanently or use timer more 0 to stop pulse without removing pin assignment
case 45: // M45 - set up PWM P<pin> S<power val 0-255> [T<timer mode 0-3>] [R<resolution 8,9,10 bits>] [X<prescale 0-7>]
pin_number = -1;
if(code_seen('P') )
pin_number = code_value();
else
break;
if (code_seen('S')) {
int pin_status = code_value();
int timer_mode = 2;
int timer_res = 8;
int timer_pre = 1;
if( pin_status < 0 || pin_status > 255)
pin_status = 0;
// this is a semi-permanent pin assignment so dont add if its already assigned
if( assignPin(pin_number) ) {
// timer mode 0-3: 0 stop, 1 toggle on compare match, 2 clear on match, 3 set on match (see HardwareTimer)
if( code_seen('T') ) {
timer_mode = code_value();
if( timer_mode < 0 || timer_mode > 3 )
timer_mode = 0;
}
// timer bit resolution 8,9, or 10 bits
if( code_seen('R')) {
timer_res = code_value();
if( timer_res < 8 || timer_res > 10 )
timer_res = 8;
}
// X - prescale 0-7 for power of 2
if( code_seen('X') ) {
timer_pre = code_value();
if( timer_pre < 0 || timer_pre > 7 )
timer_pre = 0;
}
for(int i = 0; i < 10; i++) {
if(ppwms[i] == NULL) {
ppin = new PWM(pin_number);
ppin->init(pin_number);
ppin->setPWMPrescale(timer_pre);
ppin->setPWMResolution(timer_res);
ppin->pwmWrite(pin_status,timer_mode); // default is 2, clear on match. to turn off, use 0
ppwms[i] = ppin;
break;
}
}
} else { // reassign pin with new mode and value
for(int i = 0; i < 10; i++) {
if(ppwms[i] && ppwms[i]->pin == pin_number) {
// timer mode 0-3: 0 stop, 1 toggle on compare match, 2 clear on match, 3 set on match (see HardwareTimer)
if( code_seen('T') ) {
timer_mode = code_value();
if( timer_mode < 0 || timer_mode > 3 )
timer_mode = 2; // mess up the code get clear on match default
}
//ppwms[i]->init();
ppwms[i]->pwmWrite(pin_status, timer_mode);
break;
}
}
}
}
break;
case 46: // M46 -Read analog pin P<pin>
pin_number = -1;
if (code_seen('P'))
pin_number = code_value();
if( assignPin(pin_number) ) {
apin = new Analog(pin_number);
int res = apin->analogRead();
res = apin->analogRead(); // de-jitter
delete &apin;
unassignPin(pin_number);
SERIAL_PGMLN(analogPinHdr);
SERIAL_PGM("1 ");
SERIAL_PORT.println(pin_number);
SERIAL_PGM("2 ");
SERIAL_PORT.println(res);
SERIAL_PORT.flush();
}
break;
case 80: // M80 - Turn on Power Supply
#if defined(PS_ON_PIN) && PS_ON_PIN > -1
SET_OUTPUT(PS_ON_PIN); //GND
WRITE(PS_ON_PIN, PS_ON_AWAKE);
// If you have a switch on suicide pin, this is useful
#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
SET_OUTPUT(SUICIDE_PIN);
WRITE(SUICIDE_PIN, HIGH);
#endif
#endif
break;
case 81: // M81 - Turn off Power
if( motorControl->isConnected())
motorControl->commandEmergencyStop();
#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
suicide();
#elif defined(PS_ON_PIN) && PS_ON_PIN > -1
SET_OUTPUT(PS_ON_PIN);
WRITE(PS_ON_PIN, PS_ON_ASLEEP);
#endif
_delay_ms(1000); // Wait 1 sec before switch off
break;
case 82:
break;
case 83:
break;
case 84: // M84
break;
case 85: // M85
break;
case 92: // M92
break;
case 104: // M104
break;
case 105 : // M105
SERIAL_PORT.println();
break;
case 106: //M106
break;
case 107: //M107
break;
case 109: // M109
break;
case 115: // M115
SERIAL_PGMLN(MSG_M115_REPORT);
break;
case 117:
break;
case 114: // M114
break;
case 120: // M120
break;
case 121: // M121
break;
case 119: // M119
break;
case 140: // M140
break;
case 150: // M150 - set an RGB value
byte red;
byte grn;
byte blu;
if(code_seen('R')) red = code_value();
if(code_seen('U')) grn = code_value();
if(code_seen('B')) blu = code_value();
//SendColors(red,grn,blu);
break;
case 190: // M190
break;
case 200:
break;
case 201: // M201
//reset_acceleration_rates();
break;
case 202: // M202
break;
case 203:
break;
case 204: // M204 acceleration T
if(code_seen('T')) ;//max_acceleration = code_value();
break;
case 205: //M205 advanced settings: maximum travel speed T=travel
if(code_seen('T')) ;//maxtravelspeed = code_value();
break;
case 206:
break;
case 207:
break;
case 208:
break;
case 209:
break;
case 218:
break;
case 220:
if(code_seen('S'))
{
}
break;
case 221:
if(code_seen('S'))
{
}
break;
case 226: // M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required, default is opposite current state
if(code_seen('P')) {
int pin_number = code_value(); // pin number
int pin_state = -1; // required pin state - default is inverted
if(code_seen('S'))
pin_state = code_value(); // required pin state
if( assignPin(pin_number) ) {
Digital pn = new Digital(pin_number);
pn.pinMode(INPUT);
int target;
switch(pin_state){
case 1:
target = HIGH;
break;
case 0:
target = LOW;
break;
default:
target = !pn.digitalRead(); // opposite of current state
break;
} // switch
while(pn.digitalRead() != target) {
manage_inactivity();
} // while
delete &pn;
unassignPin(pin_number);
} // if pin_number
} // code seen p
break;
case 227: // M227 P<pin number> S<pin state>- INPUT_PULLUP Wait until the specified pin reaches the state required, default is opposite current state
if(code_seen('P')) {
int pin_number = code_value(); // pin number
int pin_state = -1; // required pin state - default is inverted
if(code_seen('S'))
pin_state = code_value(); // required pin state
if( assignPin(pin_number) ) {
Digital pn = new Digital(pin_number);
pn.pinMode(INPUT_PULLUP);
int target;
switch(pin_state){
case 1:
target = HIGH;
break;
case 0:
target = LOW;
break;
default:
target = !pn.digitalRead(); // opposite of current state
break;
} // switch
while(pn.digitalRead() != target) {
manage_inactivity();
} // while
delete &pn;
unassignPin(pin_number);
} // if pin_number
} // code seen p
break;
case 240: // M240 Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
/*
#if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1
const uint8_t NUM_PULSES=16;
const float PULSE_LENGTH=0.01524;
for(int i=0; i < NUM_PULSES; i++) {
WRITE(PHOTOGRAPH_PIN, HIGH);
_delay_ms(PULSE_LENGTH);
WRITE(PHOTOGRAPH_PIN, LOW);
_delay_ms(PULSE_LENGTH);
}
delay(7.33);
for(int i=0; i < NUM_PULSES; i++) {
WRITE(PHOTOGRAPH_PIN, HIGH);
_delay_ms(PULSE_LENGTH);
WRITE(PHOTOGRAPH_PIN, LOW);
_delay_ms(PULSE_LENGTH);
}
#endif
*/
break;
case 250:
break;
case 280:
break;
case 300: // M300 - emit ultrasonic pulse on given pin and return duration P<pin number>
uspin = code_seen('P') ? code_value() : 0;
if (uspin > 0) {
Ultrasonic* upin = new Ultrasonic(uspin);
pin_number = upin->getPin();
SERIAL_PGMLN(sonicCntrlHdr);
SERIAL_PGM("1 "); // pin
SERIAL_PORT.println(pin_number);
SERIAL_PGM("2 "); // sequence
SERIAL_PORT.println(upin->getRange()); // range
delete upin;
}
break;
case 301: // M301 - toggle ultrasonic during inactivity P<pin>
// wont assign pin 0 as its sensitive
uspin = code_seen('P') ? code_value() : 0;
// this is a permanent pin assignment so dont add if its already assigned
if( assignPin(uspin) ) {
for(int i = 0; i < 10; i++) {
if(!psonics[i]) {
psonics[i] = new Ultrasonic(uspin);
break;
}
}
}
break;
case 302: // M302 - disable ultrasonic during inactivity P<pin>
uspin = code_seen('P') ? code_value() : 0;
unassignPin(uspin);
for(int i = 0; i < 10; i++) {
if(psonics[i] && psonics[i] == uspin) {
delete psonics[i];
psonics[i] = 0;
break;
}
}
break;
case 303: // M303 - toggle analog read during inactivity P<pin> with exclusion range 0-1024 via L<min> H<max> if present
// wont assign pin 0 as its sensitive
// if optional L and H values exclude readings in that range
uspin = code_seen('P') ? code_value() : 0;
// this is a permanent pin assignment so dont add if its already assigned
if( assignPin(uspin) ) {
for(int i = 0; i < 10; i++) {
if(!panalogs[i]) {
analogRanges[0][i] = code_seen('L') ? code_value() : 0;
analogRanges[1][i] = code_seen('H') ? code_value() : 0;
panalogs[i] = new Analog(uspin);
panalogs[i]->pinMode(INPUT);
break;
}
}
} else { // reassign values for assigned pin
for(int i = 0; i < 10; i++) {
if(panalogs[i] && panalogs[i]->pin == uspin) {
analogRanges[0][i] = code_seen('L') ? code_value() : 0;
analogRanges[1][i] = code_seen('H') ? code_value() : 0;
break;
}
}
}
break;
case 304:// M304 - toggle analog read INPUT_PULLUP during inactivity P<pin> with exclusion range 0-1024 via L<min> H<max> if present
// if optional L and H values exclude readings in that range
uspin = code_seen('P') ? code_value() : 0;
// this is a permanent pin assignment so dont add if its already assigned
if( assignPin(uspin) ) {
for(int i = 0; i < 10; i++) {
if(!panalogs[i]) {
analogRanges[0][i] = code_seen('L') ? code_value() : 0;
analogRanges[1][i] = code_seen('H') ? code_value() : 0;
panalogs[i] = new Analog(uspin);
panalogs[i]->pinMode(INPUT_PULLUP);
break;
}
}
} else { // reassign values for assigned pin
for(int i = 0; i < 10; i++) {
if(panalogs[i] && panalogs[i]->pin == uspin) {
analogRanges[0][i] = code_seen('L') ? code_value() : 0;
analogRanges[1][i] = code_seen('H') ? code_value() : 0;
break;
}
}
}
break;
case 305: // M305 - toggle digital read during inactivity M305 P<pin> T<target> 0 or 1 for target value, default 0
// wont assign pin 0 as its sensitive
// Looks for target value for pin during inactive, if so publish with <digitalpin> header and 1 - pin, 2 - value
uspin = code_seen('P') ? code_value() : 0;
digitarg = code_seen('T') ? code_value() : 0;
// this is a permanent pin assignment so dont add if its already assigned
if( assignPin(uspin) ) {
for(int i = 0; i < 10; i++) {
if(!pdigitals[i]) {
pdigitals[i] = new Digital(uspin);
pdigitals[i]->pinMode(INPUT);
digitalTarget[i] = digitarg;
break;
}
}
} else {
for(int i = 0; i < 10; i++) {
if(pdigitals[i] && pdigitals[i]->pin == uspin) {
digitalTarget[i] = digitarg;
break;
}
}
}
break;
case 306:// M306 - INPUT_PULLUP toggle digital read during inactivity M306 P<pin> T<target> 0 or 1 for target value, default 0
// Looks for target value for pin during inactive, if so publish with <digitalpin> header and 1 - pin 2 - value
uspin = code_seen('P') ? code_value() : 0;
digitarg = code_seen('T') ? code_value() : 0;
// this is a permanent pin assignment so dont add if its already assigned
if( assignPin(uspin) ) {
for(int i = 0; i < 10; i++) {
if(!pdigitals[i]) {
pdigitals[i] = new Digital(uspin);
pdigitals[i]->pinMode(INPUT_PULLUP);
digitalTarget[i] = digitarg;
break;
}
}
} else {
for(int i = 0; i < 10; i++) {
if(pdigitals[i] && pdigitals[i]->pin == uspin) {
digitalTarget[i] = digitarg;
break;
}
}
}
break;
case 349: // M349
/*
if(code_seen('P')) bedKp = code_value();
if(code_seen('I')) bedKi = scalePID_i(code_value());
if(code_seen('D')) bedKd = scalePID_d(code_value());
updatePID();
SERIAL_PROTOCOL(MSG_OK);
SERIAL_PROTOCOL(" p:");
SERIAL_PROTOCOL(bedKp);
SERIAL_PROTOCOL(" i:");
SERIAL_PROTOCOL(unscalePID_i(bedKi));
SERIAL_PROTOCOL(" d:");
SERIAL_PROTOCOL(unscalePID_d(bedKd));
SERIAL_PROTOCOLLN("");
*/
break;
case 350:
break;
case 351:
break;
case 400:
break;
case 401:
break;
case 402:
break;
case 444: // M444 - set battery warning threshold V<volts*10>!!!!!!!!!!!!!!!!!! REMEMBER ITS TIMES 10 !!!!!!!!!!!!!!!!!!
if(code_seen('V')) {
BatteryThreshold = code_value(); // times 10!!!
}
break;
case 445: // M445 - Turn off pulsed write pin - disable PWM P<pin>
pin_number = -1;
if (code_seen('P'))
pin_number = code_value();
unassignPin(pin_number);
for(int i = 0; i < 10; i++) {
if(ppwms[i] && ppwms[i] == pin_number) {
ppwms[i]->pwmWrite(0,0); // default is 2, clear on match. to turn off, use 0
delete ppwms[i];
ppwms[i] = 0;
break;
}
}
break;
case 500: // M500 Store settings in EEPROM
Config_StoreSettings();
break;
case 501: // M501 Read settings from EEPROM
Config_RetrieveSettings();
break;
case 502: // M502 Revert to default settings
Config_ResetDefault();
break;
case 503: // M503 print settings currently in memory
Config_PrintSettings();
break;
case 540:
//if(code_seen('S')) abort_on_endstop_hit = code_value() > 0;
break;
case 600:
break;
case 605:
break;
case 666:
break;
case 700: // return stats
// Check startup - does nothing if bootloader sets MCUSR to 0
mcu = MCUSR;
if(mcu & 1) SERIAL_PGMLN(MSG_POWERUP);
if(mcu & 2) SERIAL_PGMLN(MSG_EXTERNAL_RESET);
if(mcu & 4) SERIAL_PGMLN(MSG_BROWNOUT_RESET);
if(mcu & 8) SERIAL_PGMLN(MSG_WATCHDOG_RESET);
if(mcu & 32) SERIAL_PGMLN(MSG_SOFTWARE_RESET);
MCUSR=0;
SERIAL_PGM(VERSION_STRING);
#ifdef STRING_VERSION_CONFIG_H
#ifdef STRING_CONFIG_H_AUTHOR
SERIAL_PGM(MSG_CONFIGURATION_VER);
SERIAL_PGM(STRING_VERSION_CONFIG_H);
SERIAL_PGM(MSG_AUTHOR);
SERIAL_PGMLN(STRING_CONFIG_H_AUTHOR);
SERIAL_PGM("Compiled: ");
SERIAL_PGM(__DATE__);
#endif
#endif
SERIAL_PGM(MSG_FREE_MEMORY);
SERIAL_PORT.println(freeMemory());
SERIAL_PORT.flush();
break;
case 701: // Report digital pins in use
SERIAL_PGMLN("Digital Pins:");
for(int i = 0; i < 10; i++) {
if( pdigitals[i] ) {
SERIAL_PORT.print(pdigitals[i]->pin);
switch(pdigitals[i]->mode) {
case INPUT:
SERIAL_PGMLN(" INPUT");
break;
case INPUT_PULLUP:
SERIAL_PGMLN(" INPUT_PULLUP");
break;
case OUTPUT:
SERIAL_PGMLN(" OUTPUT");
break;
}
}
}
SERIAL_PORT.println();
SERIAL_PORT.flush();
break;
case 702: // Report analog pins in use
SERIAL_PGMLN("Analog Pins:");
for(int i = 0; i < 10; i++) {
if( panalogs[i] ) {
SERIAL_PORT.print(panalogs[i]->pin);
switch(panalogs[i]->mode) {
case INPUT:
SERIAL_PGMLN(" INPUT");
break;
case INPUT_PULLUP:
SERIAL_PGMLN(" INPUT_PULLUP");
break;
case OUTPUT:
SERIAL_PGMLN(" OUTPUT");
break;
}
}
}
SERIAL_PORT.println();
SERIAL_PORT.flush();
break;
case 703: // Report ultrasonic pins in use
SERIAL_PGMLN("Ultrasonic Pins:");
for(int i = 0; i < 10; i++) {
if( psonics[i] ) {
SERIAL_PGM("Pin:");
SERIAL_PORT.println(psonics[i]->getPin());
}
}
SERIAL_PORT.flush();
break;
case 704: // Report PWM pins in use
SERIAL_PGMLN("PWM Pins:");
for(int i = 0; i < 10; i++) {
if( ppwms[i] ) {
SERIAL_PGM("Pin:");
SERIAL_PORT.print(ppwms[i]->pin);
SERIAL_PGM(" Timer channel:");
SERIAL_PORT.print(ppwms[i]->channel);
switch(ppwms[i]->mode) {
case INPUT:
SERIAL_PGM(" INPUT");
break;
case INPUT_PULLUP:
SERIAL_PGM(" INPUT_PULLUP");
break;
case OUTPUT:
SERIAL_PGM(" OUTPUT");
break;
}
SERIAL_PORT.println();
}
SERIAL_PORT.flush();
}
break;
case 705:
// see if it has propulsion attributes declared with M3 as interrupt serviced
switch(motor_status) {
case 0:
SERIAL_PGMLN("Multi Channel Motor Controller:");
break;
case 1:
SERIAL_PGMLN("H-Bridge Brushed DC Motor Driver(s):");
break;
case 2:
SERIAL_PGMLN("Brushless DC Motor Driver(s):");
break;
}
for(int i = 0 ; i < motorControl->getChannels(); i++) { //per channel
SERIAL_PGM("Motor channel:");
SERIAL_PORT.print(i+1);
SERIAL_PGM(" Min Power:");
SERIAL_PORT.print(motorControl->getMinMotorPower(i+1));
SERIAL_PGM(" Speed:");
SERIAL_PORT.print(motorControl->getMotorSpeed(i+1));
SERIAL_PGM(" Curr. Dir:");
SERIAL_PORT.print(motorControl->getCurrentDirection(i+1));
SERIAL_PGM(" Default. Dir:");
SERIAL_PORT.println(motorControl->getDefaultDirection(i+1));
SERIAL_PGM(" Encoder Pin:");
if(motorControl->getWheelEncoder(i+1)) {
SERIAL_PORT.print(motorControl->getWheelEncoder(i+1)->pin);
SERIAL_PGM(" Count:");
SERIAL_PORT.print(motorControl->getEncoderCount(i+1));
SERIAL_PGM(" Duration:");
SERIAL_PORT.println(motorControl->getMaxMotorDuration(i+1));
} else {
SERIAL_PGMLN("None.");
}
}
SERIAL_PGM("Ultrasonic pins:");
if( motorControl->totalUltrasonics() ) {
SERIAL_PORT.println(motorControl->totalUltrasonics());
for(int j = 0; j < motorControl->totalUltrasonics(); j++) {
SERIAL_PGM("Pin:");
SERIAL_PORT.print(psonics[motorControl->getUltrasonicIndex(i+1)]->getPin());
SERIAL_PGM(" Facing:");
SERIAL_PORT.print(motorControl->getUltrasonicFacing(i+1));
SERIAL_PGM(" Shutdown cm:");
SERIAL_PORT.println(motorControl->getMinMotorDist(i+1));
}
} else {
SERIAL_PGMLN("None.");
}
SERIAL_PORT.flush();
break;
case 706: // Report all pins in use
for(int i = 0; i < 100; i++) {
if( pinAssignment(i) == PIN_ASSIGNED ) {
SERIAL_PORT.print(i);
SERIAL_PORT.print(',');
}
}
SERIAL_PORT.println();
SERIAL_PORT.flush();
break;
case 797:
break;
case 798: // Report controller status
//SERIAL_PGMLN(motorCntrlHdr);
if( motorControl->isConnected() ) {
for(int i = 0; i < motorControl->getChannels() ; i++ ) {
SERIAL_PGM("Motor Channel:");
SERIAL_PORT.println(i+1);
char* buf = motorControl->getDriverInfo(i+1);
while(*buf) {
SERIAL_PORT.print(*buf);
buf++;
}
}
}
SERIAL_PORT.println();
SERIAL_PORT.flush();
break;
case 799: // Reset Motor controller
motorControl->commandEmergencyStop();
break;
case 800:
break;
case 801: // IMU
prh = new PitchRollHeading();
orient = prh->getPitchRollHeading();
SERIAL_PGMLN(posCntrlHdr);
SERIAL_PGM("1 ");
SERIAL_PORT.println(orient.pitch);
SERIAL_PGM("2 ");
SERIAL_PORT.println(orient.roll);
SERIAL_PGM("3 ");
SERIAL_PORT.println(orient.heading);
break;
case 802: // Acquire analog pin data M802 Pnn Sxxx Mxxx P=Pin number, S=number readings, M=microseconds per reading. X - pullup.
// Publish <dataset> 1 - pin, 2 - reading
if( code_seen('P')) {
apin = new Analog((uint8_t)code_value());
if( code_seen('X') )
apin->pinMode(INPUT_PULLUP);
else
apin->pinMode(INPUT);
}
nread = 0;
if( code_seen('S') ) {
nread = code_value();
}
micros = 0;
if( code_seen('M')) {
micros = (uint32_t)code_value();
}
values = new int(nread);
for(int i = 0; i < nread; i++) {
*(values+i) = apin->analogRead();
for(int j = 0; j < micros; j++) _delay_us(1);
}
SERIAL_PGMLN(datasetHdr);
SERIAL_PORT.println();
for(int i = 0; i < nread; i++) {
SERIAL_PORT.print(i+1); // sequence
SERIAL_PORT.print(' ');
// 0 element is pin number
if( i == 0 )
SERIAL_PORT.println(apin->pin);
else
SERIAL_PORT.println(*(values+i)); // value
}
delete values;
SERIAL_PORT.println();
SERIAL_PORT.flush();
break;
case 803:
break;
case 907:
break;
case 908: // M908 Control digital trimpot directly.
uint8_t current;
if(code_seen('P')) channel=code_value();
if(code_seen('S')) current=code_value();
digitalPotWrite(channel, current);
break;
case 999: // M999: Reset
Stopped = false;
//lcd_reset_alert_level();
gcode_LastN = Stopped_gcode_LastN;
//FlushSerialRequestResend();
if( watchdog_timer != NULL )
delete watchdog_timer;
watchdog_timer = new WatchdogTimer();
watchdog_timer->watchdog_init(15); // 15 ms
} // switch m code
} else { // if M code
SERIAL_PGMLN(MSG_UNKNOWN_COMMAND);
//SERIAL_PORT.println(cmdbuffer[bufindr]);
}
}
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
int current_temp;
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) {
manage_inactivity(1);
#if (MINIMUM_FAN_START_SPEED > 0)
manage_fan_start_speed();
#endif
}
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
current_temp = analog2temp( current_raw );
#if PREVENT_DANGEROUS_EXTRUDE > 0
if(target[E_AXIS]!=position[E_AXIS]) {
if(current_temp < EXTRUDE_MINTEMP && prevent_cold_extrude) {
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
serial_send(TXT_COLD_EXTRUSION_PREVENTED_CRLF);
}
#if PREVENT_LENGTHY_EXTRUDE > 0
if(labs(target[E_AXIS]-position[E_AXIS]) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
serial_send(TXT_LONG_EXTRUSION_PREVENTED_CRLF);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = 0;
// Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e = (long)(block->steps_e * extrudemultiply / 100.0);
block->step_event_count = MAX(block->steps_x, MAX(block->steps_y, MAX(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= DROP_SEGMENTS) return;
// Compute direction bits for this block
block->direction_bits = 0;
if (target[X_AXIS] < position[X_AXIS]) block->direction_bits |= (1<<X_AXIS);
if (target[Y_AXIS] < position[Y_AXIS]) block->direction_bits |= (1<<Y_AXIS);
if (target[Z_AXIS] < position[Z_AXIS]) block->direction_bits |= (1<<Z_AXIS);
if (target[E_AXIS] < position[E_AXIS]) {
block->direction_bits |= (1<<E_AXIS);
//High Feedrate for retract
max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
retract_feedrate_aktiv = 1;
}
else {
if(retract_feedrate_aktiv) {
if(block->steps_e > 0) retract_feedrate_aktiv = 0;
}
else max_E_feedrate_calc = max_feedrate[E_AXIS];
}
#ifdef DELAY_ENABLE
if(block->steps_x != 0) {
enable_x();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_y != 0) {
enable_y();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_z != 0) {
enable_z();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_e != 0) {
enable_e();
delayMicroseconds(DELAY_ENABLE);
}
#else
//enable active axes
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
if(block->steps_z != 0) enable_z();
if(block->steps_e != 0) enable_e();
#endif
if (block->steps_e == 0) {
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else {
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
// slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
int moves_queued=(block_buffer_head-block_buffer_tail + block_buffer_size) & block_buffer_mask;
#ifdef SLOWDOWN
if(moves_queued < (block_buffer_size * 0.5) && moves_queued > MIN_MOVES_QUEUED_FOR_SLOWDOWN)
feed_rate = feed_rate*moves_queued / (float)(block_buffer_size * 0.5);
#endif
float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(float)(axis_steps_per_unit[X_AXIS]);
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(float)(axis_steps_per_unit[Y_AXIS]);
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(float)(axis_steps_per_unit[Z_AXIS]);
//delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/(float)(axis_steps_per_unit[E_AXIS]))*extrudemultiply/100.0;
if ( block->steps_x <= DROP_SEGMENTS && block->steps_y <= DROP_SEGMENTS && block->steps_z <= DROP_SEGMENTS )
block->millimeters = fabs(delta_mm[E_AXIS]);
else
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
float inverse_millimeters = 1.0/(float)(block->millimeters); // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
/*
#ifdef SLOWDOWN
// segment time im micro seconds
long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued>0) && (moves_queued < (BLOCK_BUFFER_SIZE - 4))) {
if (segment_time<minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued);
}
}
else {
if (segment_time<minsegmenttime) segment_time=minsegmenttime;
}
#endif
// END OF SLOW DOWN SECTION
*/
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 3; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = fmin(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second;
if(fabs(current_speed[E_AXIS]) > max_E_feedrate_calc)
speed_factor = fmin(speed_factor, max_E_feedrate_calc / fabs(current_speed[E_AXIS]));
// Correct the speed
if( speed_factor < 1.0) {
for(unsigned char i=0; i < 4; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count/(float)(block->millimeters);
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
else {
block->acceleration_st = ceil(move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
// Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
}
block->acceleration = block->acceleration_st / (float)(steps_per_mm);
block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(float)(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk/2.0;
float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.0) vmax_junction = fmin(vmax_junction, max_z_jerk/2.0);
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.0) vmax_junction = fmin(vmax_junction, max_e_jerk/2.0);
if(G92_reset_previous_speed == 1) {
vmax_junction = 0.1;
G92_reset_previous_speed = 0;
}
vmax_junction = fmin(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
// if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = (max_xy_jerk/(float)(jerk));
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk)
vmax_junction_factor= fmin(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk)
vmax_junction_factor = fmin(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
vmax_junction = fmin(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = fmin(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) block->nominal_length_flag = 1;
else block->nominal_length_flag = 0;
block->recalculate_flag = 1; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed;
calculate_trapezoid_for_block(block, block->entry_speed/(float)(block->nominal_speed),
safe_speed/(float)(block->nominal_speed));
// Move buffer head
CRITICAL_SECTION_START;
block_buffer_head = next_buffer_head;
CRITICAL_SECTION_END;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
#ifdef AUTOTEMP
getHighESpeed();
#endif
st_wake_up();
}
