A port of the Sprinter Firmware to the mbed.
Sprinter.cpp@0:1e3ffdfd19ec, 2012-07-08 (annotated)
- Committer:
- nullsub
- Date:
- Sun Jul 08 16:17:09 2012 +0000
- Revision:
- 0:1e3ffdfd19ec
Working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nullsub | 0:1e3ffdfd19ec | 1 | //https://github.com/kliment/Sprinter/tree/master/Sprinter |
nullsub | 0:1e3ffdfd19ec | 2 | #include "mbed.h" |
nullsub | 0:1e3ffdfd19ec | 3 | #include "configuration.h" |
nullsub | 0:1e3ffdfd19ec | 4 | #include "pins.h" |
nullsub | 0:1e3ffdfd19ec | 5 | #include "Sprinter.h" |
nullsub | 0:1e3ffdfd19ec | 6 | |
nullsub | 0:1e3ffdfd19ec | 7 | #include "SerialBuffered.h" |
nullsub | 0:1e3ffdfd19ec | 8 | |
nullsub | 0:1e3ffdfd19ec | 9 | DigitalOut heat0_led(LED1);//x |
nullsub | 0:1e3ffdfd19ec | 10 | DigitalOut heat1_led(LED2);//y |
nullsub | 0:1e3ffdfd19ec | 11 | //DigitalOut led3(LED3);//z |
nullsub | 0:1e3ffdfd19ec | 12 | DigitalOut p_led(LED_PIN);//e |
nullsub | 0:1e3ffdfd19ec | 13 | |
nullsub | 0:1e3ffdfd19ec | 14 | DigitalOut p_fan(FAN_PIN); |
nullsub | 0:1e3ffdfd19ec | 15 | |
nullsub | 0:1e3ffdfd19ec | 16 | //DigitalOut p_x_enable(X_ENABLE_PIN); |
nullsub | 0:1e3ffdfd19ec | 17 | DigitalOut p_x_dir(X_DIR_PIN); |
nullsub | 0:1e3ffdfd19ec | 18 | DigitalOut p_x_step(X_STEP_PIN); |
nullsub | 0:1e3ffdfd19ec | 19 | //DigitalIn p_x_min(X_MIN_PIN); |
nullsub | 0:1e3ffdfd19ec | 20 | //DigitalIn p_x_max(X_MAX_PIN); |
nullsub | 0:1e3ffdfd19ec | 21 | |
nullsub | 0:1e3ffdfd19ec | 22 | //DigitalOut p_y_enable(Y_ENABLE_PIN); |
nullsub | 0:1e3ffdfd19ec | 23 | DigitalOut p_y_dir(Y_DIR_PIN); |
nullsub | 0:1e3ffdfd19ec | 24 | DigitalOut p_y_step(Y_STEP_PIN); |
nullsub | 0:1e3ffdfd19ec | 25 | //DigitalIn p_y_min(Y_MIN_PIN); |
nullsub | 0:1e3ffdfd19ec | 26 | //DigitalIn p_y_max(Y_MAX_PIN); |
nullsub | 0:1e3ffdfd19ec | 27 | |
nullsub | 0:1e3ffdfd19ec | 28 | //DigitalOut p_z_enable(Z_ENABLE_PIN); |
nullsub | 0:1e3ffdfd19ec | 29 | DigitalOut p_z_dir(Z_DIR_PIN); |
nullsub | 0:1e3ffdfd19ec | 30 | DigitalOut p_z_step(Z_STEP_PIN); |
nullsub | 0:1e3ffdfd19ec | 31 | //DigitalIn p_z_min(Z_MIN_PIN); |
nullsub | 0:1e3ffdfd19ec | 32 | //DigitalIn p_z_max(Z_MAX_PIN); |
nullsub | 0:1e3ffdfd19ec | 33 | |
nullsub | 0:1e3ffdfd19ec | 34 | //DigitalOut p_e_enable(E_ENABLE_PIN); |
nullsub | 0:1e3ffdfd19ec | 35 | DigitalOut p_e_dir(E_DIR_PIN); |
nullsub | 0:1e3ffdfd19ec | 36 | DigitalOut p_e_step(E_STEP_PIN); |
nullsub | 0:1e3ffdfd19ec | 37 | |
nullsub | 0:1e3ffdfd19ec | 38 | DigitalOut p_heater0(HEATER_0_PIN); |
nullsub | 0:1e3ffdfd19ec | 39 | DigitalOut p_heater1(HEATER_1_PIN);//heated-build-platform |
nullsub | 0:1e3ffdfd19ec | 40 | |
nullsub | 0:1e3ffdfd19ec | 41 | AnalogIn p_temp0(TEMP_0_PIN); |
nullsub | 0:1e3ffdfd19ec | 42 | AnalogIn p_temp1(TEMP_1_PIN);//heated-build-platform thermistor |
nullsub | 0:1e3ffdfd19ec | 43 | |
nullsub | 0:1e3ffdfd19ec | 44 | SerialBuffered pc( 4096, USBTX, USBRX); |
nullsub | 0:1e3ffdfd19ec | 45 | char print_buffer[100]; |
nullsub | 0:1e3ffdfd19ec | 46 | |
nullsub | 0:1e3ffdfd19ec | 47 | Timer timer; |
nullsub | 0:1e3ffdfd19ec | 48 | |
nullsub | 0:1e3ffdfd19ec | 49 | void print_string(char * s) { |
nullsub | 0:1e3ffdfd19ec | 50 | while (*s) { |
nullsub | 0:1e3ffdfd19ec | 51 | pc.putc(*s); |
nullsub | 0:1e3ffdfd19ec | 52 | s++; |
nullsub | 0:1e3ffdfd19ec | 53 | } |
nullsub | 0:1e3ffdfd19ec | 54 | } |
nullsub | 0:1e3ffdfd19ec | 55 | |
nullsub | 0:1e3ffdfd19ec | 56 | void print_int(int var) { |
nullsub | 0:1e3ffdfd19ec | 57 | sprintf(print_buffer,"%d",var); |
nullsub | 0:1e3ffdfd19ec | 58 | print_string(print_buffer); |
nullsub | 0:1e3ffdfd19ec | 59 | } |
nullsub | 0:1e3ffdfd19ec | 60 | |
nullsub | 0:1e3ffdfd19ec | 61 | void print_long(long var) { |
nullsub | 0:1e3ffdfd19ec | 62 | sprintf(print_buffer,"%ld", var); |
nullsub | 0:1e3ffdfd19ec | 63 | print_string(print_buffer); |
nullsub | 0:1e3ffdfd19ec | 64 | } |
nullsub | 0:1e3ffdfd19ec | 65 | |
nullsub | 0:1e3ffdfd19ec | 66 | void print_float(float var) { |
nullsub | 0:1e3ffdfd19ec | 67 | sprintf(print_buffer,"%f",var); |
nullsub | 0:1e3ffdfd19ec | 68 | print_string(print_buffer); |
nullsub | 0:1e3ffdfd19ec | 69 | } |
nullsub | 0:1e3ffdfd19ec | 70 | |
nullsub | 0:1e3ffdfd19ec | 71 | int micros() { |
nullsub | 0:1e3ffdfd19ec | 72 | static long long current_us = 0; |
nullsub | 0:1e3ffdfd19ec | 73 | current_us += timer.read_us(); |
nullsub | 0:1e3ffdfd19ec | 74 | timer.reset(); |
nullsub | 0:1e3ffdfd19ec | 75 | return current_us; |
nullsub | 0:1e3ffdfd19ec | 76 | } |
nullsub | 0:1e3ffdfd19ec | 77 | |
nullsub | 0:1e3ffdfd19ec | 78 | int millis() { |
nullsub | 0:1e3ffdfd19ec | 79 | return int(micros()/1000); |
nullsub | 0:1e3ffdfd19ec | 80 | } |
nullsub | 0:1e3ffdfd19ec | 81 | |
nullsub | 0:1e3ffdfd19ec | 82 | // look here for descriptions of gcodes: http://linuxcnc.org/handbook/gcode/g-code.html |
nullsub | 0:1e3ffdfd19ec | 83 | // http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes |
nullsub | 0:1e3ffdfd19ec | 84 | |
nullsub | 0:1e3ffdfd19ec | 85 | //Stepper Movement Variables |
nullsub | 0:1e3ffdfd19ec | 86 | |
nullsub | 0:1e3ffdfd19ec | 87 | char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'}; |
nullsub | 0:1e3ffdfd19ec | 88 | bool move_direction[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 89 | unsigned long axis_previous_micros[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 90 | unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater; |
nullsub | 0:1e3ffdfd19ec | 91 | unsigned long move_steps_to_take[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 92 | #ifdef RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 93 | unsigned long axis_max_interval[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 94 | unsigned long axis_steps_per_sqr_second[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 95 | unsigned long axis_travel_steps_per_sqr_second[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 96 | unsigned long max_interval; |
nullsub | 0:1e3ffdfd19ec | 97 | unsigned long steps_per_sqr_second, plateau_steps; |
nullsub | 0:1e3ffdfd19ec | 98 | #endif |
nullsub | 0:1e3ffdfd19ec | 99 | bool acceleration_enabled = false, accelerating = false; |
nullsub | 0:1e3ffdfd19ec | 100 | unsigned long interval; |
nullsub | 0:1e3ffdfd19ec | 101 | float destination[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; |
nullsub | 0:1e3ffdfd19ec | 102 | float current_position[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; |
nullsub | 0:1e3ffdfd19ec | 103 | unsigned long steps_taken[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 104 | long axis_interval[NUM_AXIS]; // for speed delay |
nullsub | 0:1e3ffdfd19ec | 105 | bool home_all_axis = false;//true; |
nullsub | 0:1e3ffdfd19ec | 106 | int feedrate = 1500, next_feedrate, saved_feedrate; |
nullsub | 0:1e3ffdfd19ec | 107 | float time_for_move; |
nullsub | 0:1e3ffdfd19ec | 108 | long gcode_N, gcode_LastN; |
nullsub | 0:1e3ffdfd19ec | 109 | bool relative_mode = false; //Determines Absolute or Relative Coordinates |
nullsub | 0:1e3ffdfd19ec | 110 | bool relative_mode_e = false; //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode. |
nullsub | 0:1e3ffdfd19ec | 111 | long timediff = 0; |
nullsub | 0:1e3ffdfd19ec | 112 | //experimental feedrate calc |
nullsub | 0:1e3ffdfd19ec | 113 | float d = 0; |
nullsub | 0:1e3ffdfd19ec | 114 | float axis_diff[NUM_AXIS] = {0, 0, 0, 0}; |
nullsub | 0:1e3ffdfd19ec | 115 | #ifdef STEP_DELAY_RATIO |
nullsub | 0:1e3ffdfd19ec | 116 | long long_step_delay_ratio = STEP_DELAY_RATIO * 100; |
nullsub | 0:1e3ffdfd19ec | 117 | #endif |
nullsub | 0:1e3ffdfd19ec | 118 | |
nullsub | 0:1e3ffdfd19ec | 119 | // comm variables |
nullsub | 0:1e3ffdfd19ec | 120 | #define MAX_CMD_SIZE 96 |
nullsub | 0:1e3ffdfd19ec | 121 | #define BUFSIZE 8 |
nullsub | 0:1e3ffdfd19ec | 122 | char cmdbuffer[BUFSIZE][MAX_CMD_SIZE]; |
nullsub | 0:1e3ffdfd19ec | 123 | bool fromsd[BUFSIZE]; |
nullsub | 0:1e3ffdfd19ec | 124 | int bufindr = 0; |
nullsub | 0:1e3ffdfd19ec | 125 | int bufindw = 0; |
nullsub | 0:1e3ffdfd19ec | 126 | int buflen = 0; |
nullsub | 0:1e3ffdfd19ec | 127 | int i = 0; |
nullsub | 0:1e3ffdfd19ec | 128 | char serial_char; |
nullsub | 0:1e3ffdfd19ec | 129 | int serial_count = 0; |
nullsub | 0:1e3ffdfd19ec | 130 | bool comment_mode = false; |
nullsub | 0:1e3ffdfd19ec | 131 | char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc |
nullsub | 0:1e3ffdfd19ec | 132 | |
nullsub | 0:1e3ffdfd19ec | 133 | // Manage heater variables. For a thermistor or AD595 thermocouple, raw values refer to the |
nullsub | 0:1e3ffdfd19ec | 134 | // reading from the analog pin. For a MAX6675 thermocouple, the raw value is the temperature in 0.25 |
nullsub | 0:1e3ffdfd19ec | 135 | // degree increments (i.e. 100=25 deg). |
nullsub | 0:1e3ffdfd19ec | 136 | |
nullsub | 0:1e3ffdfd19ec | 137 | int target_raw = 0; |
nullsub | 0:1e3ffdfd19ec | 138 | int target_temp = 0; |
nullsub | 0:1e3ffdfd19ec | 139 | int current_raw = 0; |
nullsub | 0:1e3ffdfd19ec | 140 | int target_bed_raw = 0; |
nullsub | 0:1e3ffdfd19ec | 141 | int current_bed_raw = 0; |
nullsub | 0:1e3ffdfd19ec | 142 | int tt = 0, bt = 0; |
nullsub | 0:1e3ffdfd19ec | 143 | #ifdef PIDTEMP |
nullsub | 0:1e3ffdfd19ec | 144 | int temp_iState = 0; |
nullsub | 0:1e3ffdfd19ec | 145 | int prev_temp = 0; |
nullsub | 0:1e3ffdfd19ec | 146 | int pTerm; |
nullsub | 0:1e3ffdfd19ec | 147 | int iTerm; |
nullsub | 0:1e3ffdfd19ec | 148 | int dTerm; |
nullsub | 0:1e3ffdfd19ec | 149 | //int output; |
nullsub | 0:1e3ffdfd19ec | 150 | int error; |
nullsub | 0:1e3ffdfd19ec | 151 | int heater_duty = 0; |
nullsub | 0:1e3ffdfd19ec | 152 | const int temp_iState_min = 256L * -PID_INTEGRAL_DRIVE_MAX / PID_IGAIN; |
nullsub | 0:1e3ffdfd19ec | 153 | const int temp_iState_max = 256L * PID_INTEGRAL_DRIVE_MAX / PID_IGAIN; |
nullsub | 0:1e3ffdfd19ec | 154 | #endif |
nullsub | 0:1e3ffdfd19ec | 155 | #ifndef HEATER_CURRENT |
nullsub | 0:1e3ffdfd19ec | 156 | #define HEATER_CURRENT 255 |
nullsub | 0:1e3ffdfd19ec | 157 | #endif |
nullsub | 0:1e3ffdfd19ec | 158 | #ifdef SMOOTHING |
nullsub | 0:1e3ffdfd19ec | 159 | uint32_t nma = 0; |
nullsub | 0:1e3ffdfd19ec | 160 | #endif |
nullsub | 0:1e3ffdfd19ec | 161 | #ifdef WATCHPERIOD |
nullsub | 0:1e3ffdfd19ec | 162 | int watch_raw = -1000; |
nullsub | 0:1e3ffdfd19ec | 163 | unsigned long watchmillis = 0; |
nullsub | 0:1e3ffdfd19ec | 164 | #endif |
nullsub | 0:1e3ffdfd19ec | 165 | #ifdef MINTEMP |
nullsub | 0:1e3ffdfd19ec | 166 | int minttemp = temp2analogh(MINTEMP); |
nullsub | 0:1e3ffdfd19ec | 167 | #endif |
nullsub | 0:1e3ffdfd19ec | 168 | #ifdef MAXTEMP |
nullsub | 0:1e3ffdfd19ec | 169 | int maxttemp = temp2analogh(MAXTEMP); |
nullsub | 0:1e3ffdfd19ec | 170 | #endif |
nullsub | 0:1e3ffdfd19ec | 171 | |
nullsub | 0:1e3ffdfd19ec | 172 | //Inactivity shutdown variables |
nullsub | 0:1e3ffdfd19ec | 173 | unsigned long previous_millis_cmd = 0; |
nullsub | 0:1e3ffdfd19ec | 174 | unsigned long max_inactive_time = 0; |
nullsub | 0:1e3ffdfd19ec | 175 | unsigned long stepper_inactive_time = 0; |
nullsub | 0:1e3ffdfd19ec | 176 | |
nullsub | 0:1e3ffdfd19ec | 177 | void setup() { |
nullsub | 0:1e3ffdfd19ec | 178 | pc.baud(BAUDRATE); |
nullsub | 0:1e3ffdfd19ec | 179 | print_string("start\r\n"); |
nullsub | 0:1e3ffdfd19ec | 180 | for (int i = 0; i < BUFSIZE; i++) { |
nullsub | 0:1e3ffdfd19ec | 181 | fromsd[i] = false; |
nullsub | 0:1e3ffdfd19ec | 182 | } |
nullsub | 0:1e3ffdfd19ec | 183 | //Initialize Enable Pins - steppers default to disabled. |
nullsub | 0:1e3ffdfd19ec | 184 | #if (X_ENABLE_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 185 | if (!X_ENABLE_ON) p_x_enable = 1; |
nullsub | 0:1e3ffdfd19ec | 186 | #endif |
nullsub | 0:1e3ffdfd19ec | 187 | #if (Y_ENABLE_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 188 | if (!Y_ENABLE_ON) p_y_enable = 1; |
nullsub | 0:1e3ffdfd19ec | 189 | #endif |
nullsub | 0:1e3ffdfd19ec | 190 | #if (Z_ENABLE_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 191 | if (!Z_ENABLE_ON) p_z_enable = 1; |
nullsub | 0:1e3ffdfd19ec | 192 | #endif |
nullsub | 0:1e3ffdfd19ec | 193 | #if (E_ENABLE_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 194 | if (!E_ENABLE_ON) p_e_enable = 1; |
nullsub | 0:1e3ffdfd19ec | 195 | #endif |
nullsub | 0:1e3ffdfd19ec | 196 | |
nullsub | 0:1e3ffdfd19ec | 197 | #if (HEATER_0_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 198 | p_heater0 = 0; //WRITE(HEATER_0_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 199 | heat0_led = 0; |
nullsub | 0:1e3ffdfd19ec | 200 | #endif |
nullsub | 0:1e3ffdfd19ec | 201 | #if (HEATER_1_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 202 | p_heater1 = 0; //WRITE(HEATER_1_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 203 | heat1_led = 0; |
nullsub | 0:1e3ffdfd19ec | 204 | #endif |
nullsub | 0:1e3ffdfd19ec | 205 | |
nullsub | 0:1e3ffdfd19ec | 206 | //Initialize Alarm Pin |
nullsub | 0:1e3ffdfd19ec | 207 | #if (ALARM_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 208 | p_alarm = 0; //WRITE(ALARM_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 209 | #endif |
nullsub | 0:1e3ffdfd19ec | 210 | |
nullsub | 0:1e3ffdfd19ec | 211 | //Initialize LED Pin |
nullsub | 0:1e3ffdfd19ec | 212 | #if (LED_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 213 | p_led = 0; //WRITE(LED_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 214 | #endif |
nullsub | 0:1e3ffdfd19ec | 215 | |
nullsub | 0:1e3ffdfd19ec | 216 | #ifdef RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 217 | setup_acceleration(); |
nullsub | 0:1e3ffdfd19ec | 218 | #endif |
nullsub | 0:1e3ffdfd19ec | 219 | } |
nullsub | 0:1e3ffdfd19ec | 220 | |
nullsub | 0:1e3ffdfd19ec | 221 | void loop() { |
nullsub | 0:1e3ffdfd19ec | 222 | if (buflen<3) |
nullsub | 0:1e3ffdfd19ec | 223 | get_command(); |
nullsub | 0:1e3ffdfd19ec | 224 | |
nullsub | 0:1e3ffdfd19ec | 225 | if (buflen) { |
nullsub | 0:1e3ffdfd19ec | 226 | process_commands(); |
nullsub | 0:1e3ffdfd19ec | 227 | buflen = (buflen-1); |
nullsub | 0:1e3ffdfd19ec | 228 | bufindr = (bufindr + 1)%BUFSIZE; |
nullsub | 0:1e3ffdfd19ec | 229 | } |
nullsub | 0:1e3ffdfd19ec | 230 | //check heater every n milliseconds |
nullsub | 0:1e3ffdfd19ec | 231 | manage_heater(); |
nullsub | 0:1e3ffdfd19ec | 232 | manage_inactivity(1); |
nullsub | 0:1e3ffdfd19ec | 233 | } |
nullsub | 0:1e3ffdfd19ec | 234 | |
nullsub | 0:1e3ffdfd19ec | 235 | int main() { |
nullsub | 0:1e3ffdfd19ec | 236 | timer.start(); |
nullsub | 0:1e3ffdfd19ec | 237 | setup(); |
nullsub | 0:1e3ffdfd19ec | 238 | while (1) { |
nullsub | 0:1e3ffdfd19ec | 239 | loop(); |
nullsub | 0:1e3ffdfd19ec | 240 | } |
nullsub | 0:1e3ffdfd19ec | 241 | } |
nullsub | 0:1e3ffdfd19ec | 242 | |
nullsub | 0:1e3ffdfd19ec | 243 | inline void get_command() { |
nullsub | 0:1e3ffdfd19ec | 244 | while ( pc.readable() != 0 && buflen < BUFSIZE) { |
nullsub | 0:1e3ffdfd19ec | 245 | serial_char = pc.getc(); |
nullsub | 0:1e3ffdfd19ec | 246 | if (serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) ) { |
nullsub | 0:1e3ffdfd19ec | 247 | if (!serial_count) { //if empty line |
nullsub | 0:1e3ffdfd19ec | 248 | comment_mode = false; // for new command |
nullsub | 0:1e3ffdfd19ec | 249 | return; |
nullsub | 0:1e3ffdfd19ec | 250 | } |
nullsub | 0:1e3ffdfd19ec | 251 | cmdbuffer[bufindw][serial_count] = 0; //terminate string |
nullsub | 0:1e3ffdfd19ec | 252 | fromsd[bufindw] = false; |
nullsub | 0:1e3ffdfd19ec | 253 | if (strstr(cmdbuffer[bufindw], "N") != NULL) { |
nullsub | 0:1e3ffdfd19ec | 254 | strchr_pointer = strchr(cmdbuffer[bufindw], 'N'); |
nullsub | 0:1e3ffdfd19ec | 255 | gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10)); |
nullsub | 0:1e3ffdfd19ec | 256 | if (gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) ) { |
nullsub | 0:1e3ffdfd19ec | 257 | print_string("Serial Error: Line Number is not Last Line Number+1, Last Line:"); |
nullsub | 0:1e3ffdfd19ec | 258 | print_long(gcode_LastN); |
nullsub | 0:1e3ffdfd19ec | 259 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 260 | //print_long(gcode_N); |
nullsub | 0:1e3ffdfd19ec | 261 | FlushSerialRequestResend(); |
nullsub | 0:1e3ffdfd19ec | 262 | serial_count = 0; |
nullsub | 0:1e3ffdfd19ec | 263 | return; |
nullsub | 0:1e3ffdfd19ec | 264 | } |
nullsub | 0:1e3ffdfd19ec | 265 | |
nullsub | 0:1e3ffdfd19ec | 266 | if (strstr(cmdbuffer[bufindw], "*") != NULL) { |
nullsub | 0:1e3ffdfd19ec | 267 | int checksum = 0; |
nullsub | 0:1e3ffdfd19ec | 268 | int count = 0; |
nullsub | 0:1e3ffdfd19ec | 269 | while (cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++]; |
nullsub | 0:1e3ffdfd19ec | 270 | strchr_pointer = strchr(cmdbuffer[bufindw], '*'); |
nullsub | 0:1e3ffdfd19ec | 271 | |
nullsub | 0:1e3ffdfd19ec | 272 | if ( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) { |
nullsub | 0:1e3ffdfd19ec | 273 | print_string("Error: checksum mismatch, Last Line:"); |
nullsub | 0:1e3ffdfd19ec | 274 | print_long(gcode_LastN); |
nullsub | 0:1e3ffdfd19ec | 275 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 276 | FlushSerialRequestResend(); |
nullsub | 0:1e3ffdfd19ec | 277 | serial_count = 0; |
nullsub | 0:1e3ffdfd19ec | 278 | return; |
nullsub | 0:1e3ffdfd19ec | 279 | } |
nullsub | 0:1e3ffdfd19ec | 280 | //if no errors, continue parsing |
nullsub | 0:1e3ffdfd19ec | 281 | } else { |
nullsub | 0:1e3ffdfd19ec | 282 | print_string("Error: No Checksum with line number, Last Line:"); |
nullsub | 0:1e3ffdfd19ec | 283 | print_long(gcode_LastN); |
nullsub | 0:1e3ffdfd19ec | 284 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 285 | FlushSerialRequestResend(); |
nullsub | 0:1e3ffdfd19ec | 286 | serial_count = 0; |
nullsub | 0:1e3ffdfd19ec | 287 | return; |
nullsub | 0:1e3ffdfd19ec | 288 | } |
nullsub | 0:1e3ffdfd19ec | 289 | |
nullsub | 0:1e3ffdfd19ec | 290 | gcode_LastN = gcode_N; |
nullsub | 0:1e3ffdfd19ec | 291 | //if no errors, continue parsing |
nullsub | 0:1e3ffdfd19ec | 292 | } else { // if we don't receive 'N' but still see '*' |
nullsub | 0:1e3ffdfd19ec | 293 | if ((strstr(cmdbuffer[bufindw], "*") != NULL)) { |
nullsub | 0:1e3ffdfd19ec | 294 | print_string("Error: No Line Number with checksum, Last Line:"); |
nullsub | 0:1e3ffdfd19ec | 295 | print_long(gcode_LastN); |
nullsub | 0:1e3ffdfd19ec | 296 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 297 | serial_count = 0; |
nullsub | 0:1e3ffdfd19ec | 298 | return; |
nullsub | 0:1e3ffdfd19ec | 299 | } |
nullsub | 0:1e3ffdfd19ec | 300 | } |
nullsub | 0:1e3ffdfd19ec | 301 | if ((strstr(cmdbuffer[bufindw], "G") != NULL)) { |
nullsub | 0:1e3ffdfd19ec | 302 | strchr_pointer = strchr(cmdbuffer[bufindw], 'G'); |
nullsub | 0:1e3ffdfd19ec | 303 | switch ((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))) { |
nullsub | 0:1e3ffdfd19ec | 304 | case 0: |
nullsub | 0:1e3ffdfd19ec | 305 | case 1: |
nullsub | 0:1e3ffdfd19ec | 306 | print_string("ok\r\n"); |
nullsub | 0:1e3ffdfd19ec | 307 | break; |
nullsub | 0:1e3ffdfd19ec | 308 | default: |
nullsub | 0:1e3ffdfd19ec | 309 | break; |
nullsub | 0:1e3ffdfd19ec | 310 | } |
nullsub | 0:1e3ffdfd19ec | 311 | |
nullsub | 0:1e3ffdfd19ec | 312 | } |
nullsub | 0:1e3ffdfd19ec | 313 | bufindw = (bufindw + 1)%BUFSIZE; |
nullsub | 0:1e3ffdfd19ec | 314 | buflen += 1; |
nullsub | 0:1e3ffdfd19ec | 315 | |
nullsub | 0:1e3ffdfd19ec | 316 | comment_mode = false; //for new command |
nullsub | 0:1e3ffdfd19ec | 317 | serial_count = 0; //clear buffer |
nullsub | 0:1e3ffdfd19ec | 318 | } else { |
nullsub | 0:1e3ffdfd19ec | 319 | if (serial_char == ';') comment_mode = true; |
nullsub | 0:1e3ffdfd19ec | 320 | if (!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char; |
nullsub | 0:1e3ffdfd19ec | 321 | } |
nullsub | 0:1e3ffdfd19ec | 322 | } |
nullsub | 0:1e3ffdfd19ec | 323 | } |
nullsub | 0:1e3ffdfd19ec | 324 | |
nullsub | 0:1e3ffdfd19ec | 325 | inline float code_value() { |
nullsub | 0:1e3ffdfd19ec | 326 | return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); |
nullsub | 0:1e3ffdfd19ec | 327 | } |
nullsub | 0:1e3ffdfd19ec | 328 | inline long code_value_long() { |
nullsub | 0:1e3ffdfd19ec | 329 | return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); |
nullsub | 0:1e3ffdfd19ec | 330 | } |
nullsub | 0:1e3ffdfd19ec | 331 | inline bool code_seen(char code_string[]) { |
nullsub | 0:1e3ffdfd19ec | 332 | return (strstr(cmdbuffer[bufindr], code_string) != NULL); //Return True if the string was found |
nullsub | 0:1e3ffdfd19ec | 333 | } |
nullsub | 0:1e3ffdfd19ec | 334 | |
nullsub | 0:1e3ffdfd19ec | 335 | inline bool code_seen(char code) { |
nullsub | 0:1e3ffdfd19ec | 336 | strchr_pointer = strchr(cmdbuffer[bufindr], code); |
nullsub | 0:1e3ffdfd19ec | 337 | return (strchr_pointer != NULL); //Return True if a character was found |
nullsub | 0:1e3ffdfd19ec | 338 | } |
nullsub | 0:1e3ffdfd19ec | 339 | |
nullsub | 0:1e3ffdfd19ec | 340 | inline void process_commands() { |
nullsub | 0:1e3ffdfd19ec | 341 | unsigned long codenum; //throw away variable |
nullsub | 0:1e3ffdfd19ec | 342 | //char *starpos = NULL; |
nullsub | 0:1e3ffdfd19ec | 343 | |
nullsub | 0:1e3ffdfd19ec | 344 | if (code_seen('G')) { |
nullsub | 0:1e3ffdfd19ec | 345 | switch ((int)code_value()) { |
nullsub | 0:1e3ffdfd19ec | 346 | case 0: // G0 -> G1 |
nullsub | 0:1e3ffdfd19ec | 347 | case 1: // G1 |
nullsub | 0:1e3ffdfd19ec | 348 | #if (defined DISABLE_CHECK_DURING_ACC) || (defined DISABLE_CHECK_DURING_MOVE) || (defined DISABLE_CHECK_DURING_TRAVEL) |
nullsub | 0:1e3ffdfd19ec | 349 | manage_heater(); |
nullsub | 0:1e3ffdfd19ec | 350 | #endif |
nullsub | 0:1e3ffdfd19ec | 351 | get_coordinates(); // For X Y Z E F |
nullsub | 0:1e3ffdfd19ec | 352 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 353 | previous_millis_cmd = millis(); |
nullsub | 0:1e3ffdfd19ec | 354 | //ClearToSend(); |
nullsub | 0:1e3ffdfd19ec | 355 | return; |
nullsub | 0:1e3ffdfd19ec | 356 | //break; |
nullsub | 0:1e3ffdfd19ec | 357 | case 4: // G4 dwell |
nullsub | 0:1e3ffdfd19ec | 358 | codenum = 0; |
nullsub | 0:1e3ffdfd19ec | 359 | if (code_seen('P')) codenum = code_value(); // milliseconds to wait |
nullsub | 0:1e3ffdfd19ec | 360 | if (code_seen('S')) codenum = code_value() * 1000; // seconds to wait |
nullsub | 0:1e3ffdfd19ec | 361 | codenum += millis(); // keep track of when we started waiting |
nullsub | 0:1e3ffdfd19ec | 362 | while (millis() < codenum ) { |
nullsub | 0:1e3ffdfd19ec | 363 | manage_heater(); |
nullsub | 0:1e3ffdfd19ec | 364 | } |
nullsub | 0:1e3ffdfd19ec | 365 | break; |
nullsub | 0:1e3ffdfd19ec | 366 | case 28: //G28 Home all Axis one at a time |
nullsub | 0:1e3ffdfd19ec | 367 | saved_feedrate = feedrate; |
nullsub | 0:1e3ffdfd19ec | 368 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 369 | destination[i] = current_position[i]; |
nullsub | 0:1e3ffdfd19ec | 370 | } |
nullsub | 0:1e3ffdfd19ec | 371 | feedrate = 0; |
nullsub | 0:1e3ffdfd19ec | 372 | |
nullsub | 0:1e3ffdfd19ec | 373 | home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2]))); |
nullsub | 0:1e3ffdfd19ec | 374 | |
nullsub | 0:1e3ffdfd19ec | 375 | if ((home_all_axis) || (code_seen(axis_codes[0]))) { |
nullsub | 0:1e3ffdfd19ec | 376 | if ((X_MIN_PIN > -1 && X_HOME_DIR==-1) || (X_MAX_PIN > -1 && X_HOME_DIR==1)) { |
nullsub | 0:1e3ffdfd19ec | 377 | current_position[0] = -1.5 * X_MAX_LENGTH * X_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 378 | destination[0] = 0; |
nullsub | 0:1e3ffdfd19ec | 379 | feedrate = homing_feedrate[0]; |
nullsub | 0:1e3ffdfd19ec | 380 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 381 | |
nullsub | 0:1e3ffdfd19ec | 382 | current_position[0] = 5 * X_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 383 | destination[0] = 0; |
nullsub | 0:1e3ffdfd19ec | 384 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 385 | |
nullsub | 0:1e3ffdfd19ec | 386 | current_position[0] = -10 * X_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 387 | destination[0] = 0; |
nullsub | 0:1e3ffdfd19ec | 388 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 389 | |
nullsub | 0:1e3ffdfd19ec | 390 | current_position[0] = (X_HOME_DIR == -1) ? 0 : X_MAX_LENGTH; |
nullsub | 0:1e3ffdfd19ec | 391 | destination[0] = current_position[0]; |
nullsub | 0:1e3ffdfd19ec | 392 | feedrate = 0; |
nullsub | 0:1e3ffdfd19ec | 393 | } |
nullsub | 0:1e3ffdfd19ec | 394 | } |
nullsub | 0:1e3ffdfd19ec | 395 | |
nullsub | 0:1e3ffdfd19ec | 396 | if ((home_all_axis) || (code_seen(axis_codes[1]))) { |
nullsub | 0:1e3ffdfd19ec | 397 | if ((Y_MIN_PIN > -1 && Y_HOME_DIR==-1) || (Y_MAX_PIN > -1 && Y_HOME_DIR==1)) { |
nullsub | 0:1e3ffdfd19ec | 398 | current_position[1] = -1.5 * Y_MAX_LENGTH * Y_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 399 | destination[1] = 0; |
nullsub | 0:1e3ffdfd19ec | 400 | |
nullsub | 0:1e3ffdfd19ec | 401 | feedrate = homing_feedrate[1]; |
nullsub | 0:1e3ffdfd19ec | 402 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 403 | |
nullsub | 0:1e3ffdfd19ec | 404 | current_position[1] = 5 * Y_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 405 | destination[1] = 0; |
nullsub | 0:1e3ffdfd19ec | 406 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 407 | |
nullsub | 0:1e3ffdfd19ec | 408 | current_position[1] = -10 * Y_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 409 | destination[1] = 0; |
nullsub | 0:1e3ffdfd19ec | 410 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 411 | |
nullsub | 0:1e3ffdfd19ec | 412 | current_position[1] = (Y_HOME_DIR == -1) ? 0 : Y_MAX_LENGTH; |
nullsub | 0:1e3ffdfd19ec | 413 | destination[1] = current_position[1]; |
nullsub | 0:1e3ffdfd19ec | 414 | feedrate = 0; |
nullsub | 0:1e3ffdfd19ec | 415 | } |
nullsub | 0:1e3ffdfd19ec | 416 | } |
nullsub | 0:1e3ffdfd19ec | 417 | |
nullsub | 0:1e3ffdfd19ec | 418 | if ((home_all_axis) || (code_seen(axis_codes[2]))) { |
nullsub | 0:1e3ffdfd19ec | 419 | if ((Z_MIN_PIN > -1 && Z_HOME_DIR==-1) || (Z_MAX_PIN > -1 && Z_HOME_DIR==1)) { |
nullsub | 0:1e3ffdfd19ec | 420 | current_position[2] = -1.5 * Z_MAX_LENGTH * Z_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 421 | destination[2] = 0; |
nullsub | 0:1e3ffdfd19ec | 422 | feedrate = homing_feedrate[2]; |
nullsub | 0:1e3ffdfd19ec | 423 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 424 | |
nullsub | 0:1e3ffdfd19ec | 425 | current_position[2] = 2 * Z_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 426 | destination[2] = 0; |
nullsub | 0:1e3ffdfd19ec | 427 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 428 | |
nullsub | 0:1e3ffdfd19ec | 429 | current_position[2] = -5 * Z_HOME_DIR; |
nullsub | 0:1e3ffdfd19ec | 430 | destination[2] = 0; |
nullsub | 0:1e3ffdfd19ec | 431 | prepare_move(); |
nullsub | 0:1e3ffdfd19ec | 432 | |
nullsub | 0:1e3ffdfd19ec | 433 | current_position[2] = (Z_HOME_DIR == -1) ? 0 : Z_MAX_LENGTH; |
nullsub | 0:1e3ffdfd19ec | 434 | destination[2] = current_position[2]; |
nullsub | 0:1e3ffdfd19ec | 435 | feedrate = 0; |
nullsub | 0:1e3ffdfd19ec | 436 | } |
nullsub | 0:1e3ffdfd19ec | 437 | } |
nullsub | 0:1e3ffdfd19ec | 438 | feedrate = saved_feedrate; |
nullsub | 0:1e3ffdfd19ec | 439 | previous_millis_cmd = millis(); |
nullsub | 0:1e3ffdfd19ec | 440 | break; |
nullsub | 0:1e3ffdfd19ec | 441 | case 90: // G90 |
nullsub | 0:1e3ffdfd19ec | 442 | relative_mode = false; |
nullsub | 0:1e3ffdfd19ec | 443 | break; |
nullsub | 0:1e3ffdfd19ec | 444 | case 91: // G91 |
nullsub | 0:1e3ffdfd19ec | 445 | relative_mode = true; |
nullsub | 0:1e3ffdfd19ec | 446 | break; |
nullsub | 0:1e3ffdfd19ec | 447 | case 92: // G92 |
nullsub | 0:1e3ffdfd19ec | 448 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 449 | if (code_seen(axis_codes[i])) current_position[i] = code_value(); |
nullsub | 0:1e3ffdfd19ec | 450 | } |
nullsub | 0:1e3ffdfd19ec | 451 | break; |
nullsub | 0:1e3ffdfd19ec | 452 | } |
nullsub | 0:1e3ffdfd19ec | 453 | } |
nullsub | 0:1e3ffdfd19ec | 454 | |
nullsub | 0:1e3ffdfd19ec | 455 | else if (code_seen('M')) { |
nullsub | 0:1e3ffdfd19ec | 456 | switch ( (int)code_value() ) { |
nullsub | 0:1e3ffdfd19ec | 457 | case 42: //M42 -Change pin status via gcode |
nullsub | 0:1e3ffdfd19ec | 458 | print_string("not supported!\n"); |
nullsub | 0:1e3ffdfd19ec | 459 | /* if (code_seen('S')) { |
nullsub | 0:1e3ffdfd19ec | 460 | int pin_status = code_value(); |
nullsub | 0:1e3ffdfd19ec | 461 | if (code_seen('P') && pin_status >= 0 && pin_status <= 255) { |
nullsub | 0:1e3ffdfd19ec | 462 | int pin_number = code_value(); |
nullsub | 0:1e3ffdfd19ec | 463 | for (int i = 0; i < sizeof(sensitive_pins); i++) { |
nullsub | 0:1e3ffdfd19ec | 464 | if (sensitive_pins[i] == pin_number) { |
nullsub | 0:1e3ffdfd19ec | 465 | pin_number = -1; |
nullsub | 0:1e3ffdfd19ec | 466 | break; |
nullsub | 0:1e3ffdfd19ec | 467 | } |
nullsub | 0:1e3ffdfd19ec | 468 | } |
nullsub | 0:1e3ffdfd19ec | 469 | |
nullsub | 0:1e3ffdfd19ec | 470 | if (pin_number > -1) { |
nullsub | 0:1e3ffdfd19ec | 471 | pinMode(pin_number, OUTPUT); |
nullsub | 0:1e3ffdfd19ec | 472 | digitalWrite(pin_number, pin_status); |
nullsub | 0:1e3ffdfd19ec | 473 | analogWrite(pin_number, pin_status); |
nullsub | 0:1e3ffdfd19ec | 474 | } |
nullsub | 0:1e3ffdfd19ec | 475 | } |
nullsub | 0:1e3ffdfd19ec | 476 | }*/ |
nullsub | 0:1e3ffdfd19ec | 477 | break; |
nullsub | 0:1e3ffdfd19ec | 478 | case 104: // M104 |
nullsub | 0:1e3ffdfd19ec | 479 | if (code_seen('S')) target_raw = temp2analogh(target_temp = code_value()); |
nullsub | 0:1e3ffdfd19ec | 480 | #ifdef WATCHPERIOD |
nullsub | 0:1e3ffdfd19ec | 481 | if (target_raw > current_raw) { |
nullsub | 0:1e3ffdfd19ec | 482 | watchmillis = max(1,millis()); |
nullsub | 0:1e3ffdfd19ec | 483 | watch_raw = current_raw; |
nullsub | 0:1e3ffdfd19ec | 484 | } else { |
nullsub | 0:1e3ffdfd19ec | 485 | watchmillis = 0; |
nullsub | 0:1e3ffdfd19ec | 486 | } |
nullsub | 0:1e3ffdfd19ec | 487 | #endif |
nullsub | 0:1e3ffdfd19ec | 488 | break; |
nullsub | 0:1e3ffdfd19ec | 489 | case 140: // M140 set bed temp |
nullsub | 0:1e3ffdfd19ec | 490 | #if TEMP_1_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 491 | if (code_seen('S')) target_bed_raw = temp2analogBed(code_value()); |
nullsub | 0:1e3ffdfd19ec | 492 | #endif |
nullsub | 0:1e3ffdfd19ec | 493 | break; |
nullsub | 0:1e3ffdfd19ec | 494 | case 105: // M105 |
nullsub | 0:1e3ffdfd19ec | 495 | #if (TEMP_0_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 496 | tt = analog2temp(current_raw); |
nullsub | 0:1e3ffdfd19ec | 497 | #endif |
nullsub | 0:1e3ffdfd19ec | 498 | #if TEMP_1_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 499 | bt = analog2tempBed(current_bed_raw); |
nullsub | 0:1e3ffdfd19ec | 500 | #endif |
nullsub | 0:1e3ffdfd19ec | 501 | #if (TEMP_0_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 502 | print_string("ok T:"); |
nullsub | 0:1e3ffdfd19ec | 503 | print_int(tt); |
nullsub | 0:1e3ffdfd19ec | 504 | #ifdef PIDTEMP |
nullsub | 0:1e3ffdfd19ec | 505 | print_string(" @:"); |
nullsub | 0:1e3ffdfd19ec | 506 | print_int(heater_duty); |
nullsub | 0:1e3ffdfd19ec | 507 | print_string("\r\n,"); |
nullsub | 0:1e3ffdfd19ec | 508 | print_int(iTerm); |
nullsub | 0:1e3ffdfd19ec | 509 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 510 | #endif |
nullsub | 0:1e3ffdfd19ec | 511 | #if TEMP_1_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 512 | print_string(" B:"); |
nullsub | 0:1e3ffdfd19ec | 513 | print_int(bt); |
nullsub | 0:1e3ffdfd19ec | 514 | #else |
nullsub | 0:1e3ffdfd19ec | 515 | #endif |
nullsub | 0:1e3ffdfd19ec | 516 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 517 | |
nullsub | 0:1e3ffdfd19ec | 518 | #else |
nullsub | 0:1e3ffdfd19ec | 519 | #error No temperature source available |
nullsub | 0:1e3ffdfd19ec | 520 | #endif |
nullsub | 0:1e3ffdfd19ec | 521 | return; |
nullsub | 0:1e3ffdfd19ec | 522 | //break; |
nullsub | 0:1e3ffdfd19ec | 523 | case 109: { // M109 - Wait for extruder heater to reach target. |
nullsub | 0:1e3ffdfd19ec | 524 | if (code_seen('S')) target_raw = temp2analogh(target_temp = code_value()); |
nullsub | 0:1e3ffdfd19ec | 525 | #ifdef WATCHPERIOD |
nullsub | 0:1e3ffdfd19ec | 526 | if (target_raw>current_raw) { |
nullsub | 0:1e3ffdfd19ec | 527 | watchmillis = max(1,millis()); |
nullsub | 0:1e3ffdfd19ec | 528 | watch_raw = current_raw; |
nullsub | 0:1e3ffdfd19ec | 529 | } else { |
nullsub | 0:1e3ffdfd19ec | 530 | watchmillis = 0; |
nullsub | 0:1e3ffdfd19ec | 531 | } |
nullsub | 0:1e3ffdfd19ec | 532 | #endif |
nullsub | 0:1e3ffdfd19ec | 533 | codenum = millis(); |
nullsub | 0:1e3ffdfd19ec | 534 | |
nullsub | 0:1e3ffdfd19ec | 535 | /* See if we are heating up or cooling down */ |
nullsub | 0:1e3ffdfd19ec | 536 | bool target_direction = (current_raw < target_raw); // true if heating, false if cooling |
nullsub | 0:1e3ffdfd19ec | 537 | |
nullsub | 0:1e3ffdfd19ec | 538 | #ifdef TEMP_RESIDENCY_TIME |
nullsub | 0:1e3ffdfd19ec | 539 | long residencyStart; |
nullsub | 0:1e3ffdfd19ec | 540 | residencyStart = -1; |
nullsub | 0:1e3ffdfd19ec | 541 | /* continue to loop until we have reached the target temp |
nullsub | 0:1e3ffdfd19ec | 542 | _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */ |
nullsub | 0:1e3ffdfd19ec | 543 | while ( (target_direction ? (current_raw < target_raw) : (current_raw > target_raw)) |
nullsub | 0:1e3ffdfd19ec | 544 | || (residencyStart > -1 && (millis() - residencyStart) < TEMP_RESIDENCY_TIME*1000) ) { |
nullsub | 0:1e3ffdfd19ec | 545 | #else |
nullsub | 0:1e3ffdfd19ec | 546 | while ( target_direction ? (current_raw < target_raw) : (current_raw > target_raw) ) { |
nullsub | 0:1e3ffdfd19ec | 547 | #endif |
nullsub | 0:1e3ffdfd19ec | 548 | if ( (millis() - codenum) > 1000 ) { //Print Temp Reading every 1 second while heating up/cooling down |
nullsub | 0:1e3ffdfd19ec | 549 | print_string("T:"); |
nullsub | 0:1e3ffdfd19ec | 550 | print_float(analog2temp(current_raw) ); |
nullsub | 0:1e3ffdfd19ec | 551 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 552 | codenum = millis(); |
nullsub | 0:1e3ffdfd19ec | 553 | } |
nullsub | 0:1e3ffdfd19ec | 554 | manage_heater(); |
nullsub | 0:1e3ffdfd19ec | 555 | #ifdef TEMP_RESIDENCY_TIME |
nullsub | 0:1e3ffdfd19ec | 556 | /* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time |
nullsub | 0:1e3ffdfd19ec | 557 | or when current temp falls outside the hysteresis after target temp was reached */ |
nullsub | 0:1e3ffdfd19ec | 558 | if ( (residencyStart == -1 && target_direction && current_raw >= target_raw) |
nullsub | 0:1e3ffdfd19ec | 559 | || (residencyStart == -1 && !target_direction && current_raw <= target_raw) |
nullsub | 0:1e3ffdfd19ec | 560 | || (residencyStart > -1 && labs(analog2temp(current_raw) - analog2temp(target_raw)) > TEMP_HYSTERESIS) ) { |
nullsub | 0:1e3ffdfd19ec | 561 | residencyStart = millis(); |
nullsub | 0:1e3ffdfd19ec | 562 | } |
nullsub | 0:1e3ffdfd19ec | 563 | #endif |
nullsub | 0:1e3ffdfd19ec | 564 | } |
nullsub | 0:1e3ffdfd19ec | 565 | } |
nullsub | 0:1e3ffdfd19ec | 566 | break; |
nullsub | 0:1e3ffdfd19ec | 567 | case 190: // M190 - Wait bed for heater to reach target. |
nullsub | 0:1e3ffdfd19ec | 568 | #if TEMP_1_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 569 | if (code_seen('S')) target_bed_raw = temp2analogh(code_value()); |
nullsub | 0:1e3ffdfd19ec | 570 | codenum = millis(); |
nullsub | 0:1e3ffdfd19ec | 571 | while (current_bed_raw < target_bed_raw) { |
nullsub | 0:1e3ffdfd19ec | 572 | if ( (millis()-codenum) > 1000 ) { //Print Temp Reading every 1 second while heating up. |
nullsub | 0:1e3ffdfd19ec | 573 | tt=analog2temp(current_raw); |
nullsub | 0:1e3ffdfd19ec | 574 | print_string("T:"); |
nullsub | 0:1e3ffdfd19ec | 575 | print_int(tt); |
nullsub | 0:1e3ffdfd19ec | 576 | print_string("\r\n B:"); |
nullsub | 0:1e3ffdfd19ec | 577 | print_int(analog2temp(current_bed_raw)); |
nullsub | 0:1e3ffdfd19ec | 578 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 579 | codenum = millis(); |
nullsub | 0:1e3ffdfd19ec | 580 | } |
nullsub | 0:1e3ffdfd19ec | 581 | manage_heater(); |
nullsub | 0:1e3ffdfd19ec | 582 | } |
nullsub | 0:1e3ffdfd19ec | 583 | #endif |
nullsub | 0:1e3ffdfd19ec | 584 | break; |
nullsub | 0:1e3ffdfd19ec | 585 | #if FAN_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 586 | case 106: //M106 Fan On |
nullsub | 0:1e3ffdfd19ec | 587 | if (code_seen('S')) { |
nullsub | 0:1e3ffdfd19ec | 588 | p_fan = 1; //WRITE(FAN_PIN, HIGH); |
nullsub | 0:1e3ffdfd19ec | 589 | // analogWrite(FAN_PIN, constrain(code_value(),0,255) ); |
nullsub | 0:1e3ffdfd19ec | 590 | } else { |
nullsub | 0:1e3ffdfd19ec | 591 | p_fan = 1; //WRITE(FAN_PIN, HIGH); |
nullsub | 0:1e3ffdfd19ec | 592 | //analogWrite(FAN_PIN, 255 ); |
nullsub | 0:1e3ffdfd19ec | 593 | } |
nullsub | 0:1e3ffdfd19ec | 594 | break; |
nullsub | 0:1e3ffdfd19ec | 595 | case 107: //M107 Fan Off |
nullsub | 0:1e3ffdfd19ec | 596 | //analogWrite(FAN_PIN, 0); |
nullsub | 0:1e3ffdfd19ec | 597 | p_fan = 0; //WRITE(FAN_PIN, LOW); |
nullsub | 0:1e3ffdfd19ec | 598 | break; |
nullsub | 0:1e3ffdfd19ec | 599 | #endif |
nullsub | 0:1e3ffdfd19ec | 600 | #if (PS_ON_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 601 | case 80: // M81 - ATX Power On |
nullsub | 0:1e3ffdfd19ec | 602 | SET_OUTPUT(PS_ON_PIN); //GND |
nullsub | 0:1e3ffdfd19ec | 603 | break; |
nullsub | 0:1e3ffdfd19ec | 604 | case 81: // M81 - ATX Power Off |
nullsub | 0:1e3ffdfd19ec | 605 | SET_INPUT(PS_ON_PIN); //Floating |
nullsub | 0:1e3ffdfd19ec | 606 | break; |
nullsub | 0:1e3ffdfd19ec | 607 | #endif |
nullsub | 0:1e3ffdfd19ec | 608 | case 82: |
nullsub | 0:1e3ffdfd19ec | 609 | axis_relative_modes[3] = false; |
nullsub | 0:1e3ffdfd19ec | 610 | break; |
nullsub | 0:1e3ffdfd19ec | 611 | case 83: |
nullsub | 0:1e3ffdfd19ec | 612 | axis_relative_modes[3] = true; |
nullsub | 0:1e3ffdfd19ec | 613 | break; |
nullsub | 0:1e3ffdfd19ec | 614 | case 84: |
nullsub | 0:1e3ffdfd19ec | 615 | if (code_seen('S')) { |
nullsub | 0:1e3ffdfd19ec | 616 | stepper_inactive_time = code_value() * 1000; |
nullsub | 0:1e3ffdfd19ec | 617 | } else { |
nullsub | 0:1e3ffdfd19ec | 618 | disable_x(); |
nullsub | 0:1e3ffdfd19ec | 619 | disable_y(); |
nullsub | 0:1e3ffdfd19ec | 620 | disable_z(); |
nullsub | 0:1e3ffdfd19ec | 621 | disable_e(); |
nullsub | 0:1e3ffdfd19ec | 622 | } |
nullsub | 0:1e3ffdfd19ec | 623 | break; |
nullsub | 0:1e3ffdfd19ec | 624 | case 85: // M85 |
nullsub | 0:1e3ffdfd19ec | 625 | code_seen('S'); |
nullsub | 0:1e3ffdfd19ec | 626 | max_inactive_time = code_value() * 1000; |
nullsub | 0:1e3ffdfd19ec | 627 | break; |
nullsub | 0:1e3ffdfd19ec | 628 | case 92: // M92 |
nullsub | 0:1e3ffdfd19ec | 629 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 630 | if (code_seen(axis_codes[i])) axis_steps_per_unit[i] = code_value(); |
nullsub | 0:1e3ffdfd19ec | 631 | } |
nullsub | 0:1e3ffdfd19ec | 632 | |
nullsub | 0:1e3ffdfd19ec | 633 | #ifdef RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 634 | setup_acceleration(); |
nullsub | 0:1e3ffdfd19ec | 635 | #endif |
nullsub | 0:1e3ffdfd19ec | 636 | |
nullsub | 0:1e3ffdfd19ec | 637 | break; |
nullsub | 0:1e3ffdfd19ec | 638 | case 115: // M115 |
nullsub | 0:1e3ffdfd19ec | 639 | print_string("FIRMWARE_NAME:Sprinter FIRMWARE_URL:http%%3A/github.com/kliment/Sprinter/ PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1 UUID:"); |
nullsub | 0:1e3ffdfd19ec | 640 | print_string(uuid); |
nullsub | 0:1e3ffdfd19ec | 641 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 642 | break; |
nullsub | 0:1e3ffdfd19ec | 643 | case 114: // M114 |
nullsub | 0:1e3ffdfd19ec | 644 | print_string("ok C: X:"); |
nullsub | 0:1e3ffdfd19ec | 645 | print_float(current_position[0]); |
nullsub | 0:1e3ffdfd19ec | 646 | print_string(" Y:"); |
nullsub | 0:1e3ffdfd19ec | 647 | print_float(current_position[1]); |
nullsub | 0:1e3ffdfd19ec | 648 | print_string(" Z:"); |
nullsub | 0:1e3ffdfd19ec | 649 | print_float(current_position[2]); |
nullsub | 0:1e3ffdfd19ec | 650 | print_string(" E:"); |
nullsub | 0:1e3ffdfd19ec | 651 | print_float(current_position[3]); |
nullsub | 0:1e3ffdfd19ec | 652 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 653 | return; |
nullsub | 0:1e3ffdfd19ec | 654 | case 119: // M119 |
nullsub | 0:1e3ffdfd19ec | 655 | #if (X_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 656 | print_string("x_min:"); |
nullsub | 0:1e3ffdfd19ec | 657 | pc.printf((p_x_min.read()^X_ENDSTOP_INVERT)?"H \r\n":"L \r\n"); |
nullsub | 0:1e3ffdfd19ec | 658 | #endif |
nullsub | 0:1e3ffdfd19ec | 659 | #if (X_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 660 | print_string("x_max:"); |
nullsub | 0:1e3ffdfd19ec | 661 | pc.printf((p_x_max.read()^X_ENDSTOP_INVERT)?"H \r\n":"L \r\n"); |
nullsub | 0:1e3ffdfd19ec | 662 | #endif |
nullsub | 0:1e3ffdfd19ec | 663 | #if (Y_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 664 | print_string("y_min:"); |
nullsub | 0:1e3ffdfd19ec | 665 | pc.printf((p_y_min.read()^Y_ENDSTOP_INVERT)?"H \r\n":"L \r\n"); |
nullsub | 0:1e3ffdfd19ec | 666 | #endif |
nullsub | 0:1e3ffdfd19ec | 667 | #if (Y_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 668 | print_string("y_max:"); |
nullsub | 0:1e3ffdfd19ec | 669 | pc.printf((p_y_max.read()^Y_ENDSTOP_INVERT)?"H \r\n":"L \r\n"); |
nullsub | 0:1e3ffdfd19ec | 670 | #endif |
nullsub | 0:1e3ffdfd19ec | 671 | #if (Z_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 672 | print_string("z_min:"); |
nullsub | 0:1e3ffdfd19ec | 673 | pc.printf((p_z_min.read()^Z_ENDSTOP_INVERT)?"H \r\n":"L \r\n"); |
nullsub | 0:1e3ffdfd19ec | 674 | #endif |
nullsub | 0:1e3ffdfd19ec | 675 | #if (Z_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 676 | print_string("z_max:"); |
nullsub | 0:1e3ffdfd19ec | 677 | pc.printf((p_z_max.read()^Z_ENDSTOP_INVERT)?"H \r\n":"L \r\n"); |
nullsub | 0:1e3ffdfd19ec | 678 | #endif |
nullsub | 0:1e3ffdfd19ec | 679 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 680 | break; |
nullsub | 0:1e3ffdfd19ec | 681 | #ifdef RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 682 | //TODO: update for all axis, use for loop |
nullsub | 0:1e3ffdfd19ec | 683 | case 201: // M201 |
nullsub | 0:1e3ffdfd19ec | 684 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 685 | if (code_seen(axis_codes[i])) axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i]; |
nullsub | 0:1e3ffdfd19ec | 686 | } |
nullsub | 0:1e3ffdfd19ec | 687 | break; |
nullsub | 0:1e3ffdfd19ec | 688 | case 202: // M202 |
nullsub | 0:1e3ffdfd19ec | 689 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 690 | if (code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i]; |
nullsub | 0:1e3ffdfd19ec | 691 | } |
nullsub | 0:1e3ffdfd19ec | 692 | break; |
nullsub | 0:1e3ffdfd19ec | 693 | #endif |
nullsub | 0:1e3ffdfd19ec | 694 | } |
nullsub | 0:1e3ffdfd19ec | 695 | } |
nullsub | 0:1e3ffdfd19ec | 696 | else { |
nullsub | 0:1e3ffdfd19ec | 697 | print_string("Unknown command:\r\n"); |
nullsub | 0:1e3ffdfd19ec | 698 | print_string(cmdbuffer[bufindr]); |
nullsub | 0:1e3ffdfd19ec | 699 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 700 | } |
nullsub | 0:1e3ffdfd19ec | 701 | ClearToSend(); |
nullsub | 0:1e3ffdfd19ec | 702 | } |
nullsub | 0:1e3ffdfd19ec | 703 | |
nullsub | 0:1e3ffdfd19ec | 704 | void FlushSerialRequestResend() { |
nullsub | 0:1e3ffdfd19ec | 705 | //char cmdbuffer[bufindr][100]="Resend:"; |
nullsub | 0:1e3ffdfd19ec | 706 | //while (pc.txIsBusy()); //FLUSH!//pc.flush(); |
nullsub | 0:1e3ffdfd19ec | 707 | wait_ms(200); //dont know |
nullsub | 0:1e3ffdfd19ec | 708 | print_string("Resend:"); |
nullsub | 0:1e3ffdfd19ec | 709 | print_long(gcode_LastN + 1); |
nullsub | 0:1e3ffdfd19ec | 710 | print_string("\r\n"); |
nullsub | 0:1e3ffdfd19ec | 711 | ClearToSend(); |
nullsub | 0:1e3ffdfd19ec | 712 | } |
nullsub | 0:1e3ffdfd19ec | 713 | |
nullsub | 0:1e3ffdfd19ec | 714 | void ClearToSend() { |
nullsub | 0:1e3ffdfd19ec | 715 | previous_millis_cmd = millis(); |
nullsub | 0:1e3ffdfd19ec | 716 | print_string("ok\r\n"); |
nullsub | 0:1e3ffdfd19ec | 717 | wait_ms(10); //ACHTUNG |
nullsub | 0:1e3ffdfd19ec | 718 | } |
nullsub | 0:1e3ffdfd19ec | 719 | |
nullsub | 0:1e3ffdfd19ec | 720 | inline void get_coordinates() { |
nullsub | 0:1e3ffdfd19ec | 721 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 722 | if (code_seen(axis_codes[i])) destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i]; |
nullsub | 0:1e3ffdfd19ec | 723 | else destination[i] = current_position[i]; //Are these else lines really needed? |
nullsub | 0:1e3ffdfd19ec | 724 | } |
nullsub | 0:1e3ffdfd19ec | 725 | if (code_seen('F')) { |
nullsub | 0:1e3ffdfd19ec | 726 | next_feedrate = code_value(); |
nullsub | 0:1e3ffdfd19ec | 727 | if (next_feedrate > 0.0) feedrate = next_feedrate; |
nullsub | 0:1e3ffdfd19ec | 728 | } |
nullsub | 0:1e3ffdfd19ec | 729 | } |
nullsub | 0:1e3ffdfd19ec | 730 | |
nullsub | 0:1e3ffdfd19ec | 731 | void prepare_move() { |
nullsub | 0:1e3ffdfd19ec | 732 | //Find direction |
nullsub | 0:1e3ffdfd19ec | 733 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 734 | if (destination[i] >= current_position[i]) move_direction[i] = 1; |
nullsub | 0:1e3ffdfd19ec | 735 | else move_direction[i] = 0; |
nullsub | 0:1e3ffdfd19ec | 736 | } |
nullsub | 0:1e3ffdfd19ec | 737 | |
nullsub | 0:1e3ffdfd19ec | 738 | if (min_software_endstops) { |
nullsub | 0:1e3ffdfd19ec | 739 | if (destination[0] < 0) destination[0] = 0.0; |
nullsub | 0:1e3ffdfd19ec | 740 | if (destination[1] < 0) destination[1] = 0.0; |
nullsub | 0:1e3ffdfd19ec | 741 | if (destination[2] < 0) destination[2] = 0.0; |
nullsub | 0:1e3ffdfd19ec | 742 | } |
nullsub | 0:1e3ffdfd19ec | 743 | |
nullsub | 0:1e3ffdfd19ec | 744 | if (max_software_endstops) { |
nullsub | 0:1e3ffdfd19ec | 745 | if (destination[0] > X_MAX_LENGTH) destination[0] = X_MAX_LENGTH; |
nullsub | 0:1e3ffdfd19ec | 746 | if (destination[1] > Y_MAX_LENGTH) destination[1] = Y_MAX_LENGTH; |
nullsub | 0:1e3ffdfd19ec | 747 | if (destination[2] > Z_MAX_LENGTH) destination[2] = Z_MAX_LENGTH; |
nullsub | 0:1e3ffdfd19ec | 748 | } |
nullsub | 0:1e3ffdfd19ec | 749 | |
nullsub | 0:1e3ffdfd19ec | 750 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 751 | axis_diff[i] = destination[i] - current_position[i]; |
nullsub | 0:1e3ffdfd19ec | 752 | move_steps_to_take[i] = abs(axis_diff[i]) * axis_steps_per_unit[i]; |
nullsub | 0:1e3ffdfd19ec | 753 | } |
nullsub | 0:1e3ffdfd19ec | 754 | if (feedrate < 10) |
nullsub | 0:1e3ffdfd19ec | 755 | feedrate = 10; |
nullsub | 0:1e3ffdfd19ec | 756 | |
nullsub | 0:1e3ffdfd19ec | 757 | //Feedrate calc based on XYZ travel distance |
nullsub | 0:1e3ffdfd19ec | 758 | float xy_d; |
nullsub | 0:1e3ffdfd19ec | 759 | //Check for cases where only one axis is moving - handle those without float sqrt |
nullsub | 0:1e3ffdfd19ec | 760 | if (abs(axis_diff[0]) > 0 && abs(axis_diff[1]) == 0 && abs(axis_diff[2])==0) |
nullsub | 0:1e3ffdfd19ec | 761 | d=abs(axis_diff[0]); |
nullsub | 0:1e3ffdfd19ec | 762 | else if (abs(axis_diff[0]) == 0 && abs(axis_diff[1]) > 0 && abs(axis_diff[2])==0) |
nullsub | 0:1e3ffdfd19ec | 763 | d=abs(axis_diff[1]); |
nullsub | 0:1e3ffdfd19ec | 764 | else if (abs(axis_diff[0]) == 0 && abs(axis_diff[1]) == 0 && abs(axis_diff[2])>0) |
nullsub | 0:1e3ffdfd19ec | 765 | d=abs(axis_diff[2]); |
nullsub | 0:1e3ffdfd19ec | 766 | //two or three XYZ axes moving |
nullsub | 0:1e3ffdfd19ec | 767 | else if (abs(axis_diff[0]) > 0 || abs(axis_diff[1]) > 0) { //X or Y or both |
nullsub | 0:1e3ffdfd19ec | 768 | xy_d = sqrt(axis_diff[0] * axis_diff[0] + axis_diff[1] * axis_diff[1]); |
nullsub | 0:1e3ffdfd19ec | 769 | //check if Z involved - if so interpolate that too |
nullsub | 0:1e3ffdfd19ec | 770 | d = (abs(axis_diff[2])>0)?sqrt(xy_d * xy_d + axis_diff[2] * axis_diff[2]):xy_d; |
nullsub | 0:1e3ffdfd19ec | 771 | } else if (abs(axis_diff[3]) > 0) |
nullsub | 0:1e3ffdfd19ec | 772 | d = abs(axis_diff[3]); |
nullsub | 0:1e3ffdfd19ec | 773 | else { //zero length move |
nullsub | 0:1e3ffdfd19ec | 774 | #ifdef DEBUG_PREPARE_MOVE |
nullsub | 0:1e3ffdfd19ec | 775 | log_message("_PREPARE_MOVE - No steps to take!"); |
nullsub | 0:1e3ffdfd19ec | 776 | #endif |
nullsub | 0:1e3ffdfd19ec | 777 | return; |
nullsub | 0:1e3ffdfd19ec | 778 | } |
nullsub | 0:1e3ffdfd19ec | 779 | time_for_move = (d / (feedrate / 60000000.0) ); |
nullsub | 0:1e3ffdfd19ec | 780 | //Check max feedrate for each axis is not violated, update time_for_move if necessary |
nullsub | 0:1e3ffdfd19ec | 781 | for (int i = 0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 782 | if (move_steps_to_take[i] && abs(axis_diff[i]) / (time_for_move / 60000000.0) > max_feedrate[i]) { |
nullsub | 0:1e3ffdfd19ec | 783 | time_for_move = time_for_move / max_feedrate[i] * (abs(axis_diff[i]) / (time_for_move / 60000000.0)); |
nullsub | 0:1e3ffdfd19ec | 784 | } |
nullsub | 0:1e3ffdfd19ec | 785 | } |
nullsub | 0:1e3ffdfd19ec | 786 | //Calculate the full speed stepper interval for each axis |
nullsub | 0:1e3ffdfd19ec | 787 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 788 | if (move_steps_to_take[i]) axis_interval[i] = time_for_move / move_steps_to_take[i] * 100; |
nullsub | 0:1e3ffdfd19ec | 789 | } |
nullsub | 0:1e3ffdfd19ec | 790 | |
nullsub | 0:1e3ffdfd19ec | 791 | #ifdef DEBUG_PREPARE_MOVE |
nullsub | 0:1e3ffdfd19ec | 792 | log_float("_PREPARE_MOVE - Move distance on the XY plane", xy_d); |
nullsub | 0:1e3ffdfd19ec | 793 | log_float("_PREPARE_MOVE - Move distance on the XYZ space", d); |
nullsub | 0:1e3ffdfd19ec | 794 | log_int("_PREPARE_MOVE - Commanded feedrate", feedrate); |
nullsub | 0:1e3ffdfd19ec | 795 | log_float("_PREPARE_MOVE - Constant full speed move time", time_for_move); |
nullsub | 0:1e3ffdfd19ec | 796 | log_float_array("_PREPARE_MOVE - Destination", destination, NUM_AXIS); |
nullsub | 0:1e3ffdfd19ec | 797 | log_float_array("_PREPARE_MOVE - Current position", current_position, NUM_AXIS); |
nullsub | 0:1e3ffdfd19ec | 798 | log_ulong_array("_PREPARE_MOVE - Steps to take", move_steps_to_take, NUM_AXIS); |
nullsub | 0:1e3ffdfd19ec | 799 | log_long_array("_PREPARE_MOVE - Axes full speed intervals", axis_interval, NUM_AXIS); |
nullsub | 0:1e3ffdfd19ec | 800 | #endif |
nullsub | 0:1e3ffdfd19ec | 801 | |
nullsub | 0:1e3ffdfd19ec | 802 | unsigned long move_steps[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 803 | for (int i=0; i < NUM_AXIS; i++) |
nullsub | 0:1e3ffdfd19ec | 804 | move_steps[i] = move_steps_to_take[i]; |
nullsub | 0:1e3ffdfd19ec | 805 | linear_move(move_steps); // make the move |
nullsub | 0:1e3ffdfd19ec | 806 | } |
nullsub | 0:1e3ffdfd19ec | 807 | |
nullsub | 0:1e3ffdfd19ec | 808 | int max(int a, int b) { |
nullsub | 0:1e3ffdfd19ec | 809 | if (a > b) |
nullsub | 0:1e3ffdfd19ec | 810 | return a; |
nullsub | 0:1e3ffdfd19ec | 811 | return b; |
nullsub | 0:1e3ffdfd19ec | 812 | } |
nullsub | 0:1e3ffdfd19ec | 813 | |
nullsub | 0:1e3ffdfd19ec | 814 | inline void linear_move(unsigned long axis_steps_remaining[]) { // make linear move with preset speeds and destinations, see G0 and G1 |
nullsub | 0:1e3ffdfd19ec | 815 | //Determine direction of movement |
nullsub | 0:1e3ffdfd19ec | 816 | if (destination[0] > current_position[0]) p_x_dir =!INVERT_X_DIR; //WRITE(X_DIR_PIN,!INVERT_X_DIR); |
nullsub | 0:1e3ffdfd19ec | 817 | else p_x_dir = INVERT_X_DIR; //WRITE(X_DIR_PIN,INVERT_X_DIR); |
nullsub | 0:1e3ffdfd19ec | 818 | if (destination[1] > current_position[1]) p_y_dir =!INVERT_Y_DIR; // WRITE(Y_DIR_PIN,!INVERT_Y_DIR); |
nullsub | 0:1e3ffdfd19ec | 819 | else p_y_dir = INVERT_Y_DIR; // WRITE(Y_DIR_PIN,INVERT_Y_DIR); |
nullsub | 0:1e3ffdfd19ec | 820 | if (destination[2] > current_position[2]) p_z_dir =!INVERT_Z_DIR; //WRITE(Z_DIR_PIN,!INVERT_Z_DIR); |
nullsub | 0:1e3ffdfd19ec | 821 | else p_z_dir = INVERT_Z_DIR; //WRITE(Z_DIR_PIN,INVERT_Z_DIR); |
nullsub | 0:1e3ffdfd19ec | 822 | if (destination[3] > current_position[3]) p_e_dir =!INVERT_E_DIR; //WRITE(E_DIR_PIN,!INVERT_E_DIR); |
nullsub | 0:1e3ffdfd19ec | 823 | else p_e_dir = INVERT_E_DIR; //WRITE(E_DIR_PIN,INVERT_E_DIR); |
nullsub | 0:1e3ffdfd19ec | 824 | |
nullsub | 0:1e3ffdfd19ec | 825 | #if (X_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 826 | if (!move_direction[0]) if (p_x_min.read() != X_ENDSTOP_INVERT) axis_steps_remaining[0]=0; |
nullsub | 0:1e3ffdfd19ec | 827 | #endif |
nullsub | 0:1e3ffdfd19ec | 828 | #if (Y_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 829 | if (!move_direction[1]) if (p_y_min.read() != Y_ENDSTOP_INVERT) axis_steps_remaining[1]=0; |
nullsub | 0:1e3ffdfd19ec | 830 | #endif |
nullsub | 0:1e3ffdfd19ec | 831 | #if (Z_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 832 | if (!move_direction[2]) if (p_z_min.read() != Z_ENDSTOP_INVERT) axis_steps_remaining[2]=0; |
nullsub | 0:1e3ffdfd19ec | 833 | #endif |
nullsub | 0:1e3ffdfd19ec | 834 | #if (X_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 835 | if (move_direction[0]) if (p_x_max.read() != X_ENDSTOP_INVERT) axis_steps_remaining[0]=0; |
nullsub | 0:1e3ffdfd19ec | 836 | #endif |
nullsub | 0:1e3ffdfd19ec | 837 | #if (Y_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 838 | if (move_direction[1]) if (p_y_max.read() != Y_ENDSTOP_INVERT) axis_steps_remaining[1]=0; |
nullsub | 0:1e3ffdfd19ec | 839 | #endif |
nullsub | 0:1e3ffdfd19ec | 840 | # if(Z_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 841 | if (move_direction[2]) if (p_z_max.read() != Z_ENDSTOP_INVERT) axis_steps_remaining[2]=0; |
nullsub | 0:1e3ffdfd19ec | 842 | #endif |
nullsub | 0:1e3ffdfd19ec | 843 | |
nullsub | 0:1e3ffdfd19ec | 844 | |
nullsub | 0:1e3ffdfd19ec | 845 | //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration. |
nullsub | 0:1e3ffdfd19ec | 846 | // TODO: maybe it's better to refactor into a generic enable(int axis) function, that will probably take more ram, |
nullsub | 0:1e3ffdfd19ec | 847 | // but will reduce code size |
nullsub | 0:1e3ffdfd19ec | 848 | if (axis_steps_remaining[0]) enable_x(); |
nullsub | 0:1e3ffdfd19ec | 849 | if (axis_steps_remaining[1]) enable_y(); |
nullsub | 0:1e3ffdfd19ec | 850 | if (axis_steps_remaining[2]) enable_z(); |
nullsub | 0:1e3ffdfd19ec | 851 | if (axis_steps_remaining[3]) enable_e(); |
nullsub | 0:1e3ffdfd19ec | 852 | |
nullsub | 0:1e3ffdfd19ec | 853 | //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. |
nullsub | 0:1e3ffdfd19ec | 854 | unsigned long delta[] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes |
nullsub | 0:1e3ffdfd19ec | 855 | long axis_error[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 856 | int primary_axis; |
nullsub | 0:1e3ffdfd19ec | 857 | if (delta[1] > delta[0] && delta[1] > delta[2] && delta[1] > delta[3]) primary_axis = 1; |
nullsub | 0:1e3ffdfd19ec | 858 | else if (delta[0] >= delta[1] && delta[0] > delta[2] && delta[0] > delta[3]) primary_axis = 0; |
nullsub | 0:1e3ffdfd19ec | 859 | else if (delta[2] >= delta[0] && delta[2] >= delta[1] && delta[2] > delta[3]) primary_axis = 2; |
nullsub | 0:1e3ffdfd19ec | 860 | else primary_axis = 3; |
nullsub | 0:1e3ffdfd19ec | 861 | unsigned long steps_remaining = delta[primary_axis]; |
nullsub | 0:1e3ffdfd19ec | 862 | unsigned long steps_to_take = steps_remaining; |
nullsub | 0:1e3ffdfd19ec | 863 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 864 | if (i != primary_axis) axis_error[i] = delta[primary_axis] / 2; |
nullsub | 0:1e3ffdfd19ec | 865 | steps_taken[i]=0; |
nullsub | 0:1e3ffdfd19ec | 866 | } |
nullsub | 0:1e3ffdfd19ec | 867 | interval = axis_interval[primary_axis]; |
nullsub | 0:1e3ffdfd19ec | 868 | bool is_print_move = delta[3] > 0; |
nullsub | 0:1e3ffdfd19ec | 869 | #ifdef DEBUG_BRESENHAM |
nullsub | 0:1e3ffdfd19ec | 870 | log_int("_BRESENHAM - Primary axis", primary_axis); |
nullsub | 0:1e3ffdfd19ec | 871 | log_int("_BRESENHAM - Primary axis full speed interval", interval); |
nullsub | 0:1e3ffdfd19ec | 872 | log_ulong_array("_BRESENHAM - Deltas", delta, NUM_AXIS); |
nullsub | 0:1e3ffdfd19ec | 873 | log_long_array("_BRESENHAM - Errors", axis_error, NUM_AXIS); |
nullsub | 0:1e3ffdfd19ec | 874 | #endif |
nullsub | 0:1e3ffdfd19ec | 875 | |
nullsub | 0:1e3ffdfd19ec | 876 | //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it. |
nullsub | 0:1e3ffdfd19ec | 877 | #ifdef RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 878 | long max_speed_steps_per_second; |
nullsub | 0:1e3ffdfd19ec | 879 | long min_speed_steps_per_second; |
nullsub | 0:1e3ffdfd19ec | 880 | max_interval = axis_max_interval[primary_axis]; |
nullsub | 0:1e3ffdfd19ec | 881 | #ifdef DEBUG_RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 882 | log_ulong_array("_RAMP_ACCELERATION - Teoric step intervals at move start", axis_max_interval, NUM_AXIS); |
nullsub | 0:1e3ffdfd19ec | 883 | #endif |
nullsub | 0:1e3ffdfd19ec | 884 | unsigned long new_axis_max_intervals[NUM_AXIS]; |
nullsub | 0:1e3ffdfd19ec | 885 | max_speed_steps_per_second = 100000000 / interval; |
nullsub | 0:1e3ffdfd19ec | 886 | min_speed_steps_per_second = 100000000 / max_interval; //TODO: can this be deleted? |
nullsub | 0:1e3ffdfd19ec | 887 | //Calculate start speeds based on moving axes max start speed constraints. |
nullsub | 0:1e3ffdfd19ec | 888 | int slowest_start_axis = primary_axis; |
nullsub | 0:1e3ffdfd19ec | 889 | unsigned long slowest_start_axis_max_interval = max_interval; |
nullsub | 0:1e3ffdfd19ec | 890 | for (int i = 0; i < NUM_AXIS; i++) |
nullsub | 0:1e3ffdfd19ec | 891 | if (axis_steps_remaining[i] >0 && |
nullsub | 0:1e3ffdfd19ec | 892 | i != primary_axis && |
nullsub | 0:1e3ffdfd19ec | 893 | axis_max_interval[i] * axis_steps_remaining[i]/ axis_steps_remaining[slowest_start_axis] > slowest_start_axis_max_interval) { |
nullsub | 0:1e3ffdfd19ec | 894 | slowest_start_axis = i; |
nullsub | 0:1e3ffdfd19ec | 895 | slowest_start_axis_max_interval = axis_max_interval[i]; |
nullsub | 0:1e3ffdfd19ec | 896 | } |
nullsub | 0:1e3ffdfd19ec | 897 | for (int i = 0; i < NUM_AXIS; i++) |
nullsub | 0:1e3ffdfd19ec | 898 | if (axis_steps_remaining[i] >0) { |
nullsub | 0:1e3ffdfd19ec | 899 | // multiplying slowest_start_axis_max_interval by axis_steps_remaining[slowest_start_axis] |
nullsub | 0:1e3ffdfd19ec | 900 | // could lead to overflows when we have long distance moves (say, 390625*390625 > sizeof(unsigned long)) |
nullsub | 0:1e3ffdfd19ec | 901 | float steps_remaining_ratio = (float) axis_steps_remaining[slowest_start_axis] / axis_steps_remaining[i]; |
nullsub | 0:1e3ffdfd19ec | 902 | new_axis_max_intervals[i] = slowest_start_axis_max_interval * steps_remaining_ratio; |
nullsub | 0:1e3ffdfd19ec | 903 | |
nullsub | 0:1e3ffdfd19ec | 904 | if (i == primary_axis) { |
nullsub | 0:1e3ffdfd19ec | 905 | max_interval = new_axis_max_intervals[i]; |
nullsub | 0:1e3ffdfd19ec | 906 | min_speed_steps_per_second = 100000000 / max_interval; |
nullsub | 0:1e3ffdfd19ec | 907 | } |
nullsub | 0:1e3ffdfd19ec | 908 | } |
nullsub | 0:1e3ffdfd19ec | 909 | //Calculate slowest axis plateau time |
nullsub | 0:1e3ffdfd19ec | 910 | float slowest_axis_plateau_time = 0; |
nullsub | 0:1e3ffdfd19ec | 911 | for (int i=0; i < NUM_AXIS ; i++) { |
nullsub | 0:1e3ffdfd19ec | 912 | if (axis_steps_remaining[i] > 0) { |
nullsub | 0:1e3ffdfd19ec | 913 | if (is_print_move && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, |
nullsub | 0:1e3ffdfd19ec | 914 | (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_steps_per_sqr_second[i]); |
nullsub | 0:1e3ffdfd19ec | 915 | else if (axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, |
nullsub | 0:1e3ffdfd19ec | 916 | (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_travel_steps_per_sqr_second[i]); |
nullsub | 0:1e3ffdfd19ec | 917 | } |
nullsub | 0:1e3ffdfd19ec | 918 | } |
nullsub | 0:1e3ffdfd19ec | 919 | //Now we can calculate the new primary axis acceleration, so that the slowest axis max acceleration is not violated |
nullsub | 0:1e3ffdfd19ec | 920 | steps_per_sqr_second = (100000000.0 / axis_interval[primary_axis] - 100000000.0 / new_axis_max_intervals[primary_axis]) / slowest_axis_plateau_time; |
nullsub | 0:1e3ffdfd19ec | 921 | plateau_steps = (long) ((steps_per_sqr_second / 2.0 * slowest_axis_plateau_time + min_speed_steps_per_second) * slowest_axis_plateau_time); |
nullsub | 0:1e3ffdfd19ec | 922 | #ifdef DEBUG_RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 923 | log_int("_RAMP_ACCELERATION - Start speed limiting axis", slowest_start_axis); |
nullsub | 0:1e3ffdfd19ec | 924 | log_ulong("_RAMP_ACCELERATION - Limiting axis start interval", slowest_start_axis_max_interval); |
nullsub | 0:1e3ffdfd19ec | 925 | log_ulong_array("_RAMP_ACCELERATION - Actual step intervals at move start", new_axis_max_intervals, NUM_AXIS); |
nullsub | 0:1e3ffdfd19ec | 926 | #endif |
nullsub | 0:1e3ffdfd19ec | 927 | #endif |
nullsub | 0:1e3ffdfd19ec | 928 | |
nullsub | 0:1e3ffdfd19ec | 929 | unsigned long steps_done = 0; |
nullsub | 0:1e3ffdfd19ec | 930 | #ifdef RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 931 | plateau_steps *= 1.01; // This is to compensate we use discrete intervals |
nullsub | 0:1e3ffdfd19ec | 932 | acceleration_enabled = true; |
nullsub | 0:1e3ffdfd19ec | 933 | unsigned long full_interval = interval; |
nullsub | 0:1e3ffdfd19ec | 934 | if (interval > max_interval) acceleration_enabled = false; |
nullsub | 0:1e3ffdfd19ec | 935 | bool decelerating = false; |
nullsub | 0:1e3ffdfd19ec | 936 | #endif |
nullsub | 0:1e3ffdfd19ec | 937 | |
nullsub | 0:1e3ffdfd19ec | 938 | unsigned long start_move_micros = micros(); |
nullsub | 0:1e3ffdfd19ec | 939 | for (int i = 0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 940 | axis_previous_micros[i] = start_move_micros * 100; |
nullsub | 0:1e3ffdfd19ec | 941 | } |
nullsub | 0:1e3ffdfd19ec | 942 | |
nullsub | 0:1e3ffdfd19ec | 943 | #ifdef DISABLE_CHECK_DURING_TRAVEL |
nullsub | 0:1e3ffdfd19ec | 944 | //If the move time is more than allowed in DISABLE_CHECK_DURING_TRAVEL, let's |
nullsub | 0:1e3ffdfd19ec | 945 | // consider this a print move and perform heat management during it |
nullsub | 0:1e3ffdfd19ec | 946 | if (time_for_move / 1000 > DISABLE_CHECK_DURING_TRAVEL) is_print_move = true; |
nullsub | 0:1e3ffdfd19ec | 947 | //else, if the move is a retract, consider it as a travel move for the sake of this feature |
nullsub | 0:1e3ffdfd19ec | 948 | else if (delta[3]>0 && delta[0] + delta[1] + delta[2] == 0) is_print_move = false; |
nullsub | 0:1e3ffdfd19ec | 949 | #ifdef DEBUG_DISABLE_CHECK_DURING_TRAVEL |
nullsub | 0:1e3ffdfd19ec | 950 | log_bool("_DISABLE_CHECK_DURING_TRAVEL - is_print_move", is_print_move); |
nullsub | 0:1e3ffdfd19ec | 951 | #endif |
nullsub | 0:1e3ffdfd19ec | 952 | #endif |
nullsub | 0:1e3ffdfd19ec | 953 | |
nullsub | 0:1e3ffdfd19ec | 954 | #ifdef DEBUG_MOVE_TIME |
nullsub | 0:1e3ffdfd19ec | 955 | unsigned long startmove = micros(); |
nullsub | 0:1e3ffdfd19ec | 956 | #endif |
nullsub | 0:1e3ffdfd19ec | 957 | |
nullsub | 0:1e3ffdfd19ec | 958 | //move until no more steps remain |
nullsub | 0:1e3ffdfd19ec | 959 | while (axis_steps_remaining[0] + axis_steps_remaining[1] + axis_steps_remaining[2] + axis_steps_remaining[3] > 0) { |
nullsub | 0:1e3ffdfd19ec | 960 | #if defined RAMP_ACCELERATION && defined DISABLE_CHECK_DURING_ACC |
nullsub | 0:1e3ffdfd19ec | 961 | if (!accelerating && !decelerating) { |
nullsub | 0:1e3ffdfd19ec | 962 | //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp |
nullsub | 0:1e3ffdfd19ec | 963 | #ifdef DISABLE_CHECK_DURING_TRAVEL |
nullsub | 0:1e3ffdfd19ec | 964 | if (is_print_move) |
nullsub | 0:1e3ffdfd19ec | 965 | #endif |
nullsub | 0:1e3ffdfd19ec | 966 | manage_heater(); |
nullsub | 0:1e3ffdfd19ec | 967 | } |
nullsub | 0:1e3ffdfd19ec | 968 | #else |
nullsub | 0:1e3ffdfd19ec | 969 | #ifdef DISABLE_CHECK_DURING_MOVE |
nullsub | 0:1e3ffdfd19ec | 970 | {} //Do nothing |
nullsub | 0:1e3ffdfd19ec | 971 | #else |
nullsub | 0:1e3ffdfd19ec | 972 | //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp |
nullsub | 0:1e3ffdfd19ec | 973 | #ifdef DISABLE_CHECK_DURING_TRAVEL |
nullsub | 0:1e3ffdfd19ec | 974 | if (is_print_move) |
nullsub | 0:1e3ffdfd19ec | 975 | #endif |
nullsub | 0:1e3ffdfd19ec | 976 | manage_heater(); |
nullsub | 0:1e3ffdfd19ec | 977 | #endif |
nullsub | 0:1e3ffdfd19ec | 978 | #endif |
nullsub | 0:1e3ffdfd19ec | 979 | #ifdef RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 980 | //If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval |
nullsub | 0:1e3ffdfd19ec | 981 | if (acceleration_enabled && steps_done == 0) { |
nullsub | 0:1e3ffdfd19ec | 982 | interval = max_interval; |
nullsub | 0:1e3ffdfd19ec | 983 | } else if (acceleration_enabled && steps_done <= plateau_steps) { |
nullsub | 0:1e3ffdfd19ec | 984 | long current_speed = (long) ((((long) steps_per_sqr_second) / 100) |
nullsub | 0:1e3ffdfd19ec | 985 | * ((micros() - start_move_micros) / 100)/100 + (long) min_speed_steps_per_second); |
nullsub | 0:1e3ffdfd19ec | 986 | interval = 100000000 / current_speed; |
nullsub | 0:1e3ffdfd19ec | 987 | if (interval < full_interval) { |
nullsub | 0:1e3ffdfd19ec | 988 | accelerating = false; |
nullsub | 0:1e3ffdfd19ec | 989 | interval = full_interval; |
nullsub | 0:1e3ffdfd19ec | 990 | } |
nullsub | 0:1e3ffdfd19ec | 991 | if (steps_done >= steps_to_take / 2) { |
nullsub | 0:1e3ffdfd19ec | 992 | plateau_steps = steps_done; |
nullsub | 0:1e3ffdfd19ec | 993 | max_speed_steps_per_second = 100000000 / interval; |
nullsub | 0:1e3ffdfd19ec | 994 | accelerating = false; |
nullsub | 0:1e3ffdfd19ec | 995 | } |
nullsub | 0:1e3ffdfd19ec | 996 | } else if (acceleration_enabled && steps_remaining <= plateau_steps) { //(interval > minInterval * 100) { |
nullsub | 0:1e3ffdfd19ec | 997 | if (!accelerating) { |
nullsub | 0:1e3ffdfd19ec | 998 | start_move_micros = micros(); |
nullsub | 0:1e3ffdfd19ec | 999 | accelerating = true; |
nullsub | 0:1e3ffdfd19ec | 1000 | decelerating = true; |
nullsub | 0:1e3ffdfd19ec | 1001 | } |
nullsub | 0:1e3ffdfd19ec | 1002 | long current_speed = (long) ((long) max_speed_steps_per_second - ((((long) steps_per_sqr_second) / 100) |
nullsub | 0:1e3ffdfd19ec | 1003 | * ((micros() - start_move_micros) / 100)/100)); |
nullsub | 0:1e3ffdfd19ec | 1004 | interval = 100000000 / current_speed; |
nullsub | 0:1e3ffdfd19ec | 1005 | if (interval > max_interval) |
nullsub | 0:1e3ffdfd19ec | 1006 | interval = max_interval; |
nullsub | 0:1e3ffdfd19ec | 1007 | } else { |
nullsub | 0:1e3ffdfd19ec | 1008 | //Else, we are just use the full speed interval as current interval |
nullsub | 0:1e3ffdfd19ec | 1009 | interval = full_interval; |
nullsub | 0:1e3ffdfd19ec | 1010 | accelerating = false; |
nullsub | 0:1e3ffdfd19ec | 1011 | } |
nullsub | 0:1e3ffdfd19ec | 1012 | #endif |
nullsub | 0:1e3ffdfd19ec | 1013 | |
nullsub | 0:1e3ffdfd19ec | 1014 | //If there are x or y steps remaining, perform Bresenham algorithm |
nullsub | 0:1e3ffdfd19ec | 1015 | if (axis_steps_remaining[primary_axis]) { |
nullsub | 0:1e3ffdfd19ec | 1016 | #if (X_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1017 | if (!move_direction[0]) if (p_x_min.read() != X_ENDSTOP_INVERT) if (primary_axis==0) break; |
nullsub | 0:1e3ffdfd19ec | 1018 | else if (axis_steps_remaining[0]) axis_steps_remaining[0]=0; |
nullsub | 0:1e3ffdfd19ec | 1019 | #endif |
nullsub | 0:1e3ffdfd19ec | 1020 | #if (Y_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1021 | if (!move_direction[1]) if (p_y_min.read() != Y_ENDSTOP_INVERT) if (primary_axis==1) break; |
nullsub | 0:1e3ffdfd19ec | 1022 | else if (axis_steps_remaining[1]) axis_steps_remaining[1]=0; |
nullsub | 0:1e3ffdfd19ec | 1023 | #endif |
nullsub | 0:1e3ffdfd19ec | 1024 | #if (X_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1025 | if (move_direction[0]) if (p_x_max.read() != X_ENDSTOP_INVERT) if (primary_axis==0) break; |
nullsub | 0:1e3ffdfd19ec | 1026 | else if (axis_steps_remaining[0]) axis_steps_remaining[0]=0; |
nullsub | 0:1e3ffdfd19ec | 1027 | #endif |
nullsub | 0:1e3ffdfd19ec | 1028 | #if (Y_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1029 | if (move_direction[1]) if (p_y_max.read() != Y_ENDSTOP_INVERT) if (primary_axis==1) break; |
nullsub | 0:1e3ffdfd19ec | 1030 | else if (axis_steps_remaining[1]) axis_steps_remaining[1]=0; |
nullsub | 0:1e3ffdfd19ec | 1031 | #endif |
nullsub | 0:1e3ffdfd19ec | 1032 | #if (Z_MIN_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1033 | if (!move_direction[2]) if (p_z_min.read() != Z_ENDSTOP_INVERT) if (primary_axis==2) break; |
nullsub | 0:1e3ffdfd19ec | 1034 | else if (axis_steps_remaining[2]) axis_steps_remaining[2]=0; |
nullsub | 0:1e3ffdfd19ec | 1035 | #endif |
nullsub | 0:1e3ffdfd19ec | 1036 | #if (Z_MAX_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1037 | if (move_direction[2]) if (p_z_max.read() != Z_ENDSTOP_INVERT) if (primary_axis==2) break; |
nullsub | 0:1e3ffdfd19ec | 1038 | else if (axis_steps_remaining[2]) axis_steps_remaining[2]=0; |
nullsub | 0:1e3ffdfd19ec | 1039 | #endif |
nullsub | 0:1e3ffdfd19ec | 1040 | timediff = micros() * 100 - axis_previous_micros[primary_axis]; |
nullsub | 0:1e3ffdfd19ec | 1041 | if (timediff<0) {//check for overflow |
nullsub | 0:1e3ffdfd19ec | 1042 | axis_previous_micros[primary_axis]=micros()*100; |
nullsub | 0:1e3ffdfd19ec | 1043 | timediff=interval/2; //approximation |
nullsub | 0:1e3ffdfd19ec | 1044 | } |
nullsub | 0:1e3ffdfd19ec | 1045 | while (((unsigned long)timediff) >= interval && axis_steps_remaining[primary_axis] > 0) { |
nullsub | 0:1e3ffdfd19ec | 1046 | steps_done++; |
nullsub | 0:1e3ffdfd19ec | 1047 | steps_remaining--; |
nullsub | 0:1e3ffdfd19ec | 1048 | axis_steps_remaining[primary_axis]--; |
nullsub | 0:1e3ffdfd19ec | 1049 | timediff -= interval; |
nullsub | 0:1e3ffdfd19ec | 1050 | do_step(primary_axis); |
nullsub | 0:1e3ffdfd19ec | 1051 | axis_previous_micros[primary_axis] += interval; |
nullsub | 0:1e3ffdfd19ec | 1052 | for (int i=0; i < NUM_AXIS; i++) if (i != primary_axis && axis_steps_remaining[i] > 0) { |
nullsub | 0:1e3ffdfd19ec | 1053 | axis_error[i] = axis_error[i] - delta[i]; |
nullsub | 0:1e3ffdfd19ec | 1054 | if (axis_error[i] < 0) { |
nullsub | 0:1e3ffdfd19ec | 1055 | do_step(i); |
nullsub | 0:1e3ffdfd19ec | 1056 | axis_steps_remaining[i]--; |
nullsub | 0:1e3ffdfd19ec | 1057 | axis_error[i] = axis_error[i] + delta[primary_axis]; |
nullsub | 0:1e3ffdfd19ec | 1058 | } |
nullsub | 0:1e3ffdfd19ec | 1059 | } |
nullsub | 0:1e3ffdfd19ec | 1060 | #ifdef STEP_DELAY_RATIO |
nullsub | 0:1e3ffdfd19ec | 1061 | if (timediff >= interval) delayMicroseconds(long_step_delay_ratio * interval / 10000); |
nullsub | 0:1e3ffdfd19ec | 1062 | #endif |
nullsub | 0:1e3ffdfd19ec | 1063 | #ifdef STEP_DELAY_MICROS |
nullsub | 0:1e3ffdfd19ec | 1064 | if (timediff >= interval) delayMicroseconds(STEP_DELAY_MICROS); |
nullsub | 0:1e3ffdfd19ec | 1065 | #endif |
nullsub | 0:1e3ffdfd19ec | 1066 | } |
nullsub | 0:1e3ffdfd19ec | 1067 | } |
nullsub | 0:1e3ffdfd19ec | 1068 | } |
nullsub | 0:1e3ffdfd19ec | 1069 | #ifdef DEBUG_MOVE_TIME |
nullsub | 0:1e3ffdfd19ec | 1070 | log_ulong("_MOVE_TIME - This move took", micros()-startmove); |
nullsub | 0:1e3ffdfd19ec | 1071 | #endif |
nullsub | 0:1e3ffdfd19ec | 1072 | |
nullsub | 0:1e3ffdfd19ec | 1073 | if (DISABLE_X) disable_x(); |
nullsub | 0:1e3ffdfd19ec | 1074 | if (DISABLE_Y) disable_y(); |
nullsub | 0:1e3ffdfd19ec | 1075 | if (DISABLE_Z) disable_z(); |
nullsub | 0:1e3ffdfd19ec | 1076 | if (DISABLE_E) disable_e(); |
nullsub | 0:1e3ffdfd19ec | 1077 | |
nullsub | 0:1e3ffdfd19ec | 1078 | // Update current position partly based on direction, we probably can combine this with the direction code above... |
nullsub | 0:1e3ffdfd19ec | 1079 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 1080 | if (destination[i] > current_position[i]) current_position[i] = current_position[i] + steps_taken[i] / axis_steps_per_unit[i]; |
nullsub | 0:1e3ffdfd19ec | 1081 | else current_position[i] = current_position[i] - steps_taken[i] / axis_steps_per_unit[i]; |
nullsub | 0:1e3ffdfd19ec | 1082 | } |
nullsub | 0:1e3ffdfd19ec | 1083 | } |
nullsub | 0:1e3ffdfd19ec | 1084 | |
nullsub | 0:1e3ffdfd19ec | 1085 | void do_step(int axis) { |
nullsub | 0:1e3ffdfd19ec | 1086 | switch (axis) { |
nullsub | 0:1e3ffdfd19ec | 1087 | case 0: |
nullsub | 0:1e3ffdfd19ec | 1088 | p_x_step = 1; //WRITE(X_STEP_PIN, HIGH); |
nullsub | 0:1e3ffdfd19ec | 1089 | break; |
nullsub | 0:1e3ffdfd19ec | 1090 | case 1: |
nullsub | 0:1e3ffdfd19ec | 1091 | p_y_step = 1; //WRITE(Y_STEP_PIN, HIGH); |
nullsub | 0:1e3ffdfd19ec | 1092 | break; |
nullsub | 0:1e3ffdfd19ec | 1093 | case 2: |
nullsub | 0:1e3ffdfd19ec | 1094 | p_z_step = 1; //WRITE(Z_STEP_PIN, HIGH); |
nullsub | 0:1e3ffdfd19ec | 1095 | break; |
nullsub | 0:1e3ffdfd19ec | 1096 | case 3: |
nullsub | 0:1e3ffdfd19ec | 1097 | p_e_step = 1; //WRITE(E_STEP_PIN, HIGH); |
nullsub | 0:1e3ffdfd19ec | 1098 | break; |
nullsub | 0:1e3ffdfd19ec | 1099 | } |
nullsub | 0:1e3ffdfd19ec | 1100 | steps_taken[axis]+=1; |
nullsub | 0:1e3ffdfd19ec | 1101 | p_x_step = 0; //WRITE(X_STEP_PIN, LOW); |
nullsub | 0:1e3ffdfd19ec | 1102 | p_y_step = 0; //WRITE(Y_STEP_PIN, LOW); |
nullsub | 0:1e3ffdfd19ec | 1103 | p_z_step = 0; //WRITE(Z_STEP_PIN, LOW); |
nullsub | 0:1e3ffdfd19ec | 1104 | p_e_step = 0; //WRITE(E_STEP_PIN, LOW); |
nullsub | 0:1e3ffdfd19ec | 1105 | } |
nullsub | 0:1e3ffdfd19ec | 1106 | |
nullsub | 0:1e3ffdfd19ec | 1107 | #define HEAT_INTERVAL 250 |
nullsub | 0:1e3ffdfd19ec | 1108 | |
nullsub | 0:1e3ffdfd19ec | 1109 | #ifdef CONTROLLERFAN_PIN |
nullsub | 0:1e3ffdfd19ec | 1110 | unsigned long lastMotor = 0; //Save the time for when a motor was turned on last |
nullsub | 0:1e3ffdfd19ec | 1111 | unsigned long lastMotorCheck = 0; |
nullsub | 0:1e3ffdfd19ec | 1112 | |
nullsub | 0:1e3ffdfd19ec | 1113 | void controllerFan() { |
nullsub | 0:1e3ffdfd19ec | 1114 | if ((millis() - lastMotorCheck) >= 2500) { //Not a time critical function, so we only check every 2500ms |
nullsub | 0:1e3ffdfd19ec | 1115 | lastMotorCheck = millis(); |
nullsub | 0:1e3ffdfd19ec | 1116 | |
nullsub | 0:1e3ffdfd19ec | 1117 | if (!READ(X_ENABLE_PIN) || !READ(Y_ENABLE_PIN) || !READ(Z_ENABLE_PIN) || !READ(E_ENABLE_PIN)) { //If any of the drivers are enabled... |
nullsub | 0:1e3ffdfd19ec | 1118 | lastMotor = millis(); //... set time to NOW so the fan will turn on |
nullsub | 0:1e3ffdfd19ec | 1119 | } |
nullsub | 0:1e3ffdfd19ec | 1120 | |
nullsub | 0:1e3ffdfd19ec | 1121 | if ((millis() - lastMotor) >= (CONTROLLERFAN_SEC*1000UL) || lastMotor == 0) { //If the last time any driver was enabled, is longer since than CONTROLLERSEC... |
nullsub | 0:1e3ffdfd19ec | 1122 | WRITE(CONTROLLERFAN_PIN, LOW); //... turn the fan off |
nullsub | 0:1e3ffdfd19ec | 1123 | } else { |
nullsub | 0:1e3ffdfd19ec | 1124 | WRITE(CONTROLLERFAN_PIN, HIGH); //... turn the fan on |
nullsub | 0:1e3ffdfd19ec | 1125 | } |
nullsub | 0:1e3ffdfd19ec | 1126 | } |
nullsub | 0:1e3ffdfd19ec | 1127 | } |
nullsub | 0:1e3ffdfd19ec | 1128 | #endif |
nullsub | 0:1e3ffdfd19ec | 1129 | |
nullsub | 0:1e3ffdfd19ec | 1130 | void manage_heater() { |
nullsub | 0:1e3ffdfd19ec | 1131 | if ((millis() - previous_millis_heater) < HEATER_CHECK_INTERVAL ) |
nullsub | 0:1e3ffdfd19ec | 1132 | return; |
nullsub | 0:1e3ffdfd19ec | 1133 | previous_millis_heater = millis(); |
nullsub | 0:1e3ffdfd19ec | 1134 | #ifdef HEATER_USES_THERMISTOR |
nullsub | 0:1e3ffdfd19ec | 1135 | current_raw = (int) (p_temp0.read()*1023.0f) ; ///analogRead(TEMP_0_PIN); |
nullsub | 0:1e3ffdfd19ec | 1136 | //printf("temp0 = %f, temp1 = %f",p_temp0.read(), p_temp1.read()); |
nullsub | 0:1e3ffdfd19ec | 1137 | // printf("current_raw == %i\r\n", current_raw); |
nullsub | 0:1e3ffdfd19ec | 1138 | |
nullsub | 0:1e3ffdfd19ec | 1139 | #ifdef DEBUG_HEAT_MGMT |
nullsub | 0:1e3ffdfd19ec | 1140 | log_int("_HEAT_MGMT - analogRead(TEMP_0_PIN)", current_raw); |
nullsub | 0:1e3ffdfd19ec | 1141 | log_int("_HEAT_MGMT - NUMTEMPS", NUMTEMPS); |
nullsub | 0:1e3ffdfd19ec | 1142 | #endif |
nullsub | 0:1e3ffdfd19ec | 1143 | // When using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target, |
nullsub | 0:1e3ffdfd19ec | 1144 | // this switches it up so that the reading appears lower than target for the control logic. |
nullsub | 0:1e3ffdfd19ec | 1145 | current_raw = 1023 - current_raw; |
nullsub | 0:1e3ffdfd19ec | 1146 | #endif |
nullsub | 0:1e3ffdfd19ec | 1147 | #ifdef SMOOTHING |
nullsub | 0:1e3ffdfd19ec | 1148 | if (!nma) nma = SMOOTHFACTOR * current_raw; |
nullsub | 0:1e3ffdfd19ec | 1149 | nma = (nma + current_raw) - (nma / SMOOTHFACTOR); |
nullsub | 0:1e3ffdfd19ec | 1150 | current_raw = nma / SMOOTHFACTOR; |
nullsub | 0:1e3ffdfd19ec | 1151 | #endif |
nullsub | 0:1e3ffdfd19ec | 1152 | #ifdef WATCHPERIOD |
nullsub | 0:1e3ffdfd19ec | 1153 | if (watchmillis && millis() - watchmillis > WATCHPERIOD) { |
nullsub | 0:1e3ffdfd19ec | 1154 | if (watch_raw + 1 >= current_raw) { |
nullsub | 0:1e3ffdfd19ec | 1155 | target_temp = target_raw = 0; |
nullsub | 0:1e3ffdfd19ec | 1156 | WRITE(HEATER_0_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 1157 | analogWrite(HEATER_0_PIN, 0); |
nullsub | 0:1e3ffdfd19ec | 1158 | #if LED_PIN >- 1 |
nullsub | 0:1e3ffdfd19ec | 1159 | p_led = 0;//WRITE(LED_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 1160 | #endif |
nullsub | 0:1e3ffdfd19ec | 1161 | } else { |
nullsub | 0:1e3ffdfd19ec | 1162 | watchmillis = 0; |
nullsub | 0:1e3ffdfd19ec | 1163 | } |
nullsub | 0:1e3ffdfd19ec | 1164 | } |
nullsub | 0:1e3ffdfd19ec | 1165 | #endif |
nullsub | 0:1e3ffdfd19ec | 1166 | #ifdef MINTEMP |
nullsub | 0:1e3ffdfd19ec | 1167 | if (current_raw <= minttemp) |
nullsub | 0:1e3ffdfd19ec | 1168 | target_temp = target_raw = 0; |
nullsub | 0:1e3ffdfd19ec | 1169 | #endif |
nullsub | 0:1e3ffdfd19ec | 1170 | #ifdef MAXTEMP |
nullsub | 0:1e3ffdfd19ec | 1171 | if (current_raw >= maxttemp) { |
nullsub | 0:1e3ffdfd19ec | 1172 | target_temp = target_raw = 0; |
nullsub | 0:1e3ffdfd19ec | 1173 | #if (ALARM_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1174 | WRITE(ALARM_PIN,HIGH); |
nullsub | 0:1e3ffdfd19ec | 1175 | #endif |
nullsub | 0:1e3ffdfd19ec | 1176 | } |
nullsub | 0:1e3ffdfd19ec | 1177 | #endif |
nullsub | 0:1e3ffdfd19ec | 1178 | #if (TEMP_0_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1179 | #ifdef PIDTEMP |
nullsub | 0:1e3ffdfd19ec | 1180 | int current_temp = analog2temp(current_raw); |
nullsub | 0:1e3ffdfd19ec | 1181 | error = target_temp - current_temp; |
nullsub | 0:1e3ffdfd19ec | 1182 | int delta_temp = current_temp - prev_temp; |
nullsub | 0:1e3ffdfd19ec | 1183 | prev_temp = current_temp; |
nullsub | 0:1e3ffdfd19ec | 1184 | pTerm = ((long)PID_PGAIN * error) / 256; |
nullsub | 0:1e3ffdfd19ec | 1185 | const int H0 = min(HEATER_DUTY_FOR_SETPOINT(target_temp),HEATER_CURRENT); |
nullsub | 0:1e3ffdfd19ec | 1186 | heater_duty = H0 + pTerm; |
nullsub | 0:1e3ffdfd19ec | 1187 | if (error < 20) { |
nullsub | 0:1e3ffdfd19ec | 1188 | temp_iState += error; |
nullsub | 0:1e3ffdfd19ec | 1189 | temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max); |
nullsub | 0:1e3ffdfd19ec | 1190 | iTerm = ((long)PID_IGAIN * temp_iState) / 256; |
nullsub | 0:1e3ffdfd19ec | 1191 | heater_duty += iTerm; |
nullsub | 0:1e3ffdfd19ec | 1192 | } |
nullsub | 0:1e3ffdfd19ec | 1193 | int prev_error = abs(target_temp - prev_temp); |
nullsub | 0:1e3ffdfd19ec | 1194 | int log3 = 1; // discrete logarithm base 3, plus 1 |
nullsub | 0:1e3ffdfd19ec | 1195 | if (prev_error > 81) { |
nullsub | 0:1e3ffdfd19ec | 1196 | prev_error /= 81; |
nullsub | 0:1e3ffdfd19ec | 1197 | log3 += 4; |
nullsub | 0:1e3ffdfd19ec | 1198 | } |
nullsub | 0:1e3ffdfd19ec | 1199 | if (prev_error > 9) { |
nullsub | 0:1e3ffdfd19ec | 1200 | prev_error /= 9; |
nullsub | 0:1e3ffdfd19ec | 1201 | log3 += 2; |
nullsub | 0:1e3ffdfd19ec | 1202 | } |
nullsub | 0:1e3ffdfd19ec | 1203 | if (prev_error > 3) { |
nullsub | 0:1e3ffdfd19ec | 1204 | prev_error /= 3; |
nullsub | 0:1e3ffdfd19ec | 1205 | log3 ++; |
nullsub | 0:1e3ffdfd19ec | 1206 | } |
nullsub | 0:1e3ffdfd19ec | 1207 | dTerm = ((long)PID_DGAIN * delta_temp) / (256*log3); |
nullsub | 0:1e3ffdfd19ec | 1208 | heater_duty += dTerm; |
nullsub | 0:1e3ffdfd19ec | 1209 | heater_duty = constrain(heater_duty, 0, HEATER_CURRENT); |
nullsub | 0:1e3ffdfd19ec | 1210 | analogWrite(HEATER_0_PIN, heater_duty); |
nullsub | 0:1e3ffdfd19ec | 1211 | #if LED_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 1212 | p_led = 1;//analogWrite(LED_PIN, constrain(LED_PWM_FOR_BRIGHTNESS(heater_duty),0,255)); |
nullsub | 0:1e3ffdfd19ec | 1213 | #endif |
nullsub | 0:1e3ffdfd19ec | 1214 | #else |
nullsub | 0:1e3ffdfd19ec | 1215 | if (current_raw >= target_raw) { |
nullsub | 0:1e3ffdfd19ec | 1216 | p_heater0 = 0; //WRITE(HEATER_0_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 1217 | heat0_led = 0; |
nullsub | 0:1e3ffdfd19ec | 1218 | //analogWrite(HEATER_0_PIN, 0); |
nullsub | 0:1e3ffdfd19ec | 1219 | #if LED_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 1220 | p_led = 0; //WRITE(LED_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 1221 | #endif |
nullsub | 0:1e3ffdfd19ec | 1222 | } else { |
nullsub | 0:1e3ffdfd19ec | 1223 | p_heater0 = 1; //WRITE(HEATER_0_PIN,HIGH); |
nullsub | 0:1e3ffdfd19ec | 1224 | heat0_led = 1; |
nullsub | 0:1e3ffdfd19ec | 1225 | // analogWrite(HEATER_0_PIN, HEATER_CURRENT); |
nullsub | 0:1e3ffdfd19ec | 1226 | #if LED_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 1227 | p_led = 1; //WRITE(LED_PIN,HIGH); |
nullsub | 0:1e3ffdfd19ec | 1228 | #endif |
nullsub | 0:1e3ffdfd19ec | 1229 | } |
nullsub | 0:1e3ffdfd19ec | 1230 | #endif |
nullsub | 0:1e3ffdfd19ec | 1231 | #endif |
nullsub | 0:1e3ffdfd19ec | 1232 | if (millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL) |
nullsub | 0:1e3ffdfd19ec | 1233 | return; |
nullsub | 0:1e3ffdfd19ec | 1234 | previous_millis_bed_heater = millis(); |
nullsub | 0:1e3ffdfd19ec | 1235 | #ifndef TEMP_1_PIN |
nullsub | 0:1e3ffdfd19ec | 1236 | return; |
nullsub | 0:1e3ffdfd19ec | 1237 | #endif |
nullsub | 0:1e3ffdfd19ec | 1238 | #if TEMP_1_PIN == -1 |
nullsub | 0:1e3ffdfd19ec | 1239 | return; |
nullsub | 0:1e3ffdfd19ec | 1240 | #else |
nullsub | 0:1e3ffdfd19ec | 1241 | |
nullsub | 0:1e3ffdfd19ec | 1242 | #ifdef BED_USES_THERMISTOR |
nullsub | 0:1e3ffdfd19ec | 1243 | current_bed_raw = (int)(p_temp1.read()*1023.0f);///analogRead(TEMP_0_PIN); |
nullsub | 0:1e3ffdfd19ec | 1244 | |
nullsub | 0:1e3ffdfd19ec | 1245 | //analogRead(TEMP_1_PIN); |
nullsub | 0:1e3ffdfd19ec | 1246 | #ifdef DEBUG_HEAT_MGMT |
nullsub | 0:1e3ffdfd19ec | 1247 | log_int("_HEAT_MGMT - analogRead(TEMP_1_PIN)", current_bed_raw); |
nullsub | 0:1e3ffdfd19ec | 1248 | log_int("_HEAT_MGMT - BNUMTEMPS", BNUMTEMPS); |
nullsub | 0:1e3ffdfd19ec | 1249 | #endif |
nullsub | 0:1e3ffdfd19ec | 1250 | |
nullsub | 0:1e3ffdfd19ec | 1251 | // If using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target, |
nullsub | 0:1e3ffdfd19ec | 1252 | // this switches it up so that the reading appears lower than target for the control logic. |
nullsub | 0:1e3ffdfd19ec | 1253 | current_bed_raw = 1023 - current_bed_raw; |
nullsub | 0:1e3ffdfd19ec | 1254 | // printf("current_bed_raw == %i\r\n", current_bed_raw); |
nullsub | 0:1e3ffdfd19ec | 1255 | #endif |
nullsub | 0:1e3ffdfd19ec | 1256 | |
nullsub | 0:1e3ffdfd19ec | 1257 | #ifdef MINTEMP |
nullsub | 0:1e3ffdfd19ec | 1258 | if (current_bed_raw >= target_bed_raw || current_bed_raw < minttemp) |
nullsub | 0:1e3ffdfd19ec | 1259 | #else |
nullsub | 0:1e3ffdfd19ec | 1260 | if (current_bed_raw >= target_bed_raw) |
nullsub | 0:1e3ffdfd19ec | 1261 | #endif |
nullsub | 0:1e3ffdfd19ec | 1262 | { |
nullsub | 0:1e3ffdfd19ec | 1263 | #if HEATER_1_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 1264 | p_heater1 = 0; //WRITE(HEATER_1_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 1265 | heat1_led = 0; |
nullsub | 0:1e3ffdfd19ec | 1266 | #endif |
nullsub | 0:1e3ffdfd19ec | 1267 | } else { |
nullsub | 0:1e3ffdfd19ec | 1268 | #if HEATER_1_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 1269 | p_heater1 = 1; //WRITE(HEATER_1_PIN,HIGH); |
nullsub | 0:1e3ffdfd19ec | 1270 | heat1_led = 1; |
nullsub | 0:1e3ffdfd19ec | 1271 | #endif |
nullsub | 0:1e3ffdfd19ec | 1272 | } |
nullsub | 0:1e3ffdfd19ec | 1273 | #endif |
nullsub | 0:1e3ffdfd19ec | 1274 | |
nullsub | 0:1e3ffdfd19ec | 1275 | #ifdef CONTROLLERFAN_PIN |
nullsub | 0:1e3ffdfd19ec | 1276 | controllerFan(); //Check if fan should be turned on to cool stepper drivers down |
nullsub | 0:1e3ffdfd19ec | 1277 | #endif |
nullsub | 0:1e3ffdfd19ec | 1278 | } |
nullsub | 0:1e3ffdfd19ec | 1279 | |
nullsub | 0:1e3ffdfd19ec | 1280 | #if defined (HEATER_USES_THERMISTOR) || defined (BED_USES_THERMISTOR) |
nullsub | 0:1e3ffdfd19ec | 1281 | int temp2analog_thermistor(int celsius, const short table[][2], int numtemps) { |
nullsub | 0:1e3ffdfd19ec | 1282 | int raw = 0; |
nullsub | 0:1e3ffdfd19ec | 1283 | int i; |
nullsub | 0:1e3ffdfd19ec | 1284 | |
nullsub | 0:1e3ffdfd19ec | 1285 | for (i=1; i<numtemps; i++) { |
nullsub | 0:1e3ffdfd19ec | 1286 | if (table[i][1] < celsius) { |
nullsub | 0:1e3ffdfd19ec | 1287 | raw = table[i-1][0] + |
nullsub | 0:1e3ffdfd19ec | 1288 | (celsius - table[i-1][1]) * |
nullsub | 0:1e3ffdfd19ec | 1289 | (table[i][0] - table[i-1][0]) / |
nullsub | 0:1e3ffdfd19ec | 1290 | (table[i][1] - table[i-1][1]); |
nullsub | 0:1e3ffdfd19ec | 1291 | |
nullsub | 0:1e3ffdfd19ec | 1292 | break; |
nullsub | 0:1e3ffdfd19ec | 1293 | } |
nullsub | 0:1e3ffdfd19ec | 1294 | } |
nullsub | 0:1e3ffdfd19ec | 1295 | |
nullsub | 0:1e3ffdfd19ec | 1296 | // Overflow: Set to last value in the table |
nullsub | 0:1e3ffdfd19ec | 1297 | if (i == numtemps) raw = table[i-1][0]; |
nullsub | 0:1e3ffdfd19ec | 1298 | |
nullsub | 0:1e3ffdfd19ec | 1299 | return 1023 - raw; |
nullsub | 0:1e3ffdfd19ec | 1300 | } |
nullsub | 0:1e3ffdfd19ec | 1301 | #endif |
nullsub | 0:1e3ffdfd19ec | 1302 | |
nullsub | 0:1e3ffdfd19ec | 1303 | #if defined (HEATER_USES_THERMISTOR) || defined (BED_USES_THERMISTOR) |
nullsub | 0:1e3ffdfd19ec | 1304 | int analog2temp_thermistor(int raw,const short table[][2], int numtemps) { |
nullsub | 0:1e3ffdfd19ec | 1305 | int celsius = 0; |
nullsub | 0:1e3ffdfd19ec | 1306 | int i; |
nullsub | 0:1e3ffdfd19ec | 1307 | |
nullsub | 0:1e3ffdfd19ec | 1308 | raw = 1023 - raw; |
nullsub | 0:1e3ffdfd19ec | 1309 | |
nullsub | 0:1e3ffdfd19ec | 1310 | for (i=1; i<numtemps; i++) { |
nullsub | 0:1e3ffdfd19ec | 1311 | if (table[i][0] > raw) { |
nullsub | 0:1e3ffdfd19ec | 1312 | celsius = table[i-1][1] + |
nullsub | 0:1e3ffdfd19ec | 1313 | (raw - table[i-1][0]) * |
nullsub | 0:1e3ffdfd19ec | 1314 | (table[i][1] - table[i-1][1]) / |
nullsub | 0:1e3ffdfd19ec | 1315 | (table[i][0] - table[i-1][0]); |
nullsub | 0:1e3ffdfd19ec | 1316 | break; |
nullsub | 0:1e3ffdfd19ec | 1317 | } |
nullsub | 0:1e3ffdfd19ec | 1318 | } |
nullsub | 0:1e3ffdfd19ec | 1319 | |
nullsub | 0:1e3ffdfd19ec | 1320 | // Overflow: Set to last value in the table |
nullsub | 0:1e3ffdfd19ec | 1321 | if (i == numtemps) celsius = table[i-1][1]; |
nullsub | 0:1e3ffdfd19ec | 1322 | |
nullsub | 0:1e3ffdfd19ec | 1323 | return celsius; |
nullsub | 0:1e3ffdfd19ec | 1324 | } |
nullsub | 0:1e3ffdfd19ec | 1325 | #endif |
nullsub | 0:1e3ffdfd19ec | 1326 | |
nullsub | 0:1e3ffdfd19ec | 1327 | inline void kill() { |
nullsub | 0:1e3ffdfd19ec | 1328 | #if TEMP_0_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 1329 | target_raw=0; |
nullsub | 0:1e3ffdfd19ec | 1330 | p_heater0 = 0; //WRITE(HEATER_0_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 1331 | heat0_led = 0; |
nullsub | 0:1e3ffdfd19ec | 1332 | #endif |
nullsub | 0:1e3ffdfd19ec | 1333 | #if TEMP_1_PIN > -1 |
nullsub | 0:1e3ffdfd19ec | 1334 | target_bed_raw=0; |
nullsub | 0:1e3ffdfd19ec | 1335 | #if (HEATER_1_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1336 | p_heater1 = 0; // WRITE(HEATER_1_PIN,LOW); |
nullsub | 0:1e3ffdfd19ec | 1337 | heat1_led = 0; |
nullsub | 0:1e3ffdfd19ec | 1338 | |
nullsub | 0:1e3ffdfd19ec | 1339 | #endif |
nullsub | 0:1e3ffdfd19ec | 1340 | #endif |
nullsub | 0:1e3ffdfd19ec | 1341 | disable_x(); |
nullsub | 0:1e3ffdfd19ec | 1342 | disable_y(); |
nullsub | 0:1e3ffdfd19ec | 1343 | disable_z(); |
nullsub | 0:1e3ffdfd19ec | 1344 | disable_e(); |
nullsub | 0:1e3ffdfd19ec | 1345 | |
nullsub | 0:1e3ffdfd19ec | 1346 | #if (PS_ON_PIN > -1) |
nullsub | 0:1e3ffdfd19ec | 1347 | pinMode(PS_ON_PIN,INPUT); |
nullsub | 0:1e3ffdfd19ec | 1348 | #endif |
nullsub | 0:1e3ffdfd19ec | 1349 | } |
nullsub | 0:1e3ffdfd19ec | 1350 | |
nullsub | 0:1e3ffdfd19ec | 1351 | inline void manage_inactivity(int debug) { |
nullsub | 0:1e3ffdfd19ec | 1352 | if ( (millis()-previous_millis_cmd) > max_inactive_time ) if (max_inactive_time) kill(); |
nullsub | 0:1e3ffdfd19ec | 1353 | if ( (millis()-previous_millis_cmd) > stepper_inactive_time ) if (stepper_inactive_time) { |
nullsub | 0:1e3ffdfd19ec | 1354 | disable_x(); |
nullsub | 0:1e3ffdfd19ec | 1355 | disable_y(); |
nullsub | 0:1e3ffdfd19ec | 1356 | disable_z(); |
nullsub | 0:1e3ffdfd19ec | 1357 | disable_e(); |
nullsub | 0:1e3ffdfd19ec | 1358 | } |
nullsub | 0:1e3ffdfd19ec | 1359 | } |
nullsub | 0:1e3ffdfd19ec | 1360 | |
nullsub | 0:1e3ffdfd19ec | 1361 | #ifdef RAMP_ACCELERATION |
nullsub | 0:1e3ffdfd19ec | 1362 | void setup_acceleration() { |
nullsub | 0:1e3ffdfd19ec | 1363 | for (int i=0; i < NUM_AXIS; i++) { |
nullsub | 0:1e3ffdfd19ec | 1364 | axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]); |
nullsub | 0:1e3ffdfd19ec | 1365 | axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; |
nullsub | 0:1e3ffdfd19ec | 1366 | axis_travel_steps_per_sqr_second[i] = max_travel_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; |
nullsub | 0:1e3ffdfd19ec | 1367 | } |
nullsub | 0:1e3ffdfd19ec | 1368 | } |
nullsub | 0:1e3ffdfd19ec | 1369 | #endif |
nullsub | 0:1e3ffdfd19ec | 1370 | |
nullsub | 0:1e3ffdfd19ec | 1371 | #ifdef DEBUG |
nullsub | 0:1e3ffdfd19ec | 1372 | void log_message(char* message) { |
nullsub | 0:1e3ffdfd19ec | 1373 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1374 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1375 | } |
nullsub | 0:1e3ffdfd19ec | 1376 | |
nullsub | 0:1e3ffdfd19ec | 1377 | void log_bool(char* message, int value) { |
nullsub | 0:1e3ffdfd19ec | 1378 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1379 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1380 | print_string(": %i", value); |
nullsub | 0:1e3ffdfd19ec | 1381 | } |
nullsub | 0:1e3ffdfd19ec | 1382 | |
nullsub | 0:1e3ffdfd19ec | 1383 | void log_int(char* message, int value) { |
nullsub | 0:1e3ffdfd19ec | 1384 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1385 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1386 | print_string(": %i", value); |
nullsub | 0:1e3ffdfd19ec | 1387 | } |
nullsub | 0:1e3ffdfd19ec | 1388 | |
nullsub | 0:1e3ffdfd19ec | 1389 | void log_long(char* message, long value) { |
nullsub | 0:1e3ffdfd19ec | 1390 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1391 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1392 | print_string(": %l", value); |
nullsub | 0:1e3ffdfd19ec | 1393 | } |
nullsub | 0:1e3ffdfd19ec | 1394 | |
nullsub | 0:1e3ffdfd19ec | 1395 | void log_float(char* message, float value) { |
nullsub | 0:1e3ffdfd19ec | 1396 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1397 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1398 | print_string(": %f", value); |
nullsub | 0:1e3ffdfd19ec | 1399 | } |
nullsub | 0:1e3ffdfd19ec | 1400 | |
nullsub | 0:1e3ffdfd19ec | 1401 | void log_uint(char* message, unsigned int value) { |
nullsub | 0:1e3ffdfd19ec | 1402 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1403 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1404 | print_string(": %i", value); |
nullsub | 0:1e3ffdfd19ec | 1405 | } |
nullsub | 0:1e3ffdfd19ec | 1406 | |
nullsub | 0:1e3ffdfd19ec | 1407 | void log_ulong(char* message, unsigned long value) { |
nullsub | 0:1e3ffdfd19ec | 1408 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1409 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1410 | print_string(": %l", value); |
nullsub | 0:1e3ffdfd19ec | 1411 | } |
nullsub | 0:1e3ffdfd19ec | 1412 | |
nullsub | 0:1e3ffdfd19ec | 1413 | void log_int_array(char* message, int value[], int array_lenght) { |
nullsub | 0:1e3ffdfd19ec | 1414 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1415 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1416 | print_string(": {"); |
nullsub | 0:1e3ffdfd19ec | 1417 | for (int i=0; i < array_lenght; i++) { |
nullsub | 0:1e3ffdfd19ec | 1418 | print_string("%i",value[i]); |
nullsub | 0:1e3ffdfd19ec | 1419 | if (i != array_lenght-1) print_string(", "); |
nullsub | 0:1e3ffdfd19ec | 1420 | } |
nullsub | 0:1e3ffdfd19ec | 1421 | print_string("}\r\n"); |
nullsub | 0:1e3ffdfd19ec | 1422 | } |
nullsub | 0:1e3ffdfd19ec | 1423 | |
nullsub | 0:1e3ffdfd19ec | 1424 | void log_long_array(char* message, long value[], int array_lenght) { |
nullsub | 0:1e3ffdfd19ec | 1425 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1426 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1427 | print_string(": {"); |
nullsub | 0:1e3ffdfd19ec | 1428 | for (int i=0; i < array_lenght; i++) { |
nullsub | 0:1e3ffdfd19ec | 1429 | print_string("%l",value[i]); |
nullsub | 0:1e3ffdfd19ec | 1430 | if (i != array_lenght-1) print_string(", "); |
nullsub | 0:1e3ffdfd19ec | 1431 | } |
nullsub | 0:1e3ffdfd19ec | 1432 | print_string("}\r\n"); |
nullsub | 0:1e3ffdfd19ec | 1433 | } |
nullsub | 0:1e3ffdfd19ec | 1434 | |
nullsub | 0:1e3ffdfd19ec | 1435 | void log_float_array(char* message, float value[], int array_lenght) { |
nullsub | 0:1e3ffdfd19ec | 1436 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1437 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1438 | print_string(": {"); |
nullsub | 0:1e3ffdfd19ec | 1439 | for (int i=0; i < array_lenght; i++) { |
nullsub | 0:1e3ffdfd19ec | 1440 | print_string("%f",value[i]); |
nullsub | 0:1e3ffdfd19ec | 1441 | if (i != array_lenght-1) print_string(", "); |
nullsub | 0:1e3ffdfd19ec | 1442 | } |
nullsub | 0:1e3ffdfd19ec | 1443 | print_string("}\r\n"); |
nullsub | 0:1e3ffdfd19ec | 1444 | } |
nullsub | 0:1e3ffdfd19ec | 1445 | |
nullsub | 0:1e3ffdfd19ec | 1446 | void log_uint_array(char* message, unsigned int value[], int array_lenght) { |
nullsub | 0:1e3ffdfd19ec | 1447 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1448 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1449 | print_string(": {"); |
nullsub | 0:1e3ffdfd19ec | 1450 | for (int i=0; i < array_lenght; i++) { |
nullsub | 0:1e3ffdfd19ec | 1451 | print_string("%i", value[i]); |
nullsub | 0:1e3ffdfd19ec | 1452 | if (i != array_lenght-1) print_string(", "); |
nullsub | 0:1e3ffdfd19ec | 1453 | } |
nullsub | 0:1e3ffdfd19ec | 1454 | print_string("}\r\n"); |
nullsub | 0:1e3ffdfd19ec | 1455 | } |
nullsub | 0:1e3ffdfd19ec | 1456 | |
nullsub | 0:1e3ffdfd19ec | 1457 | void log_ulong_array(char* message, unsigned long value[], int array_lenght) { |
nullsub | 0:1e3ffdfd19ec | 1458 | print_string("DEBUG"); |
nullsub | 0:1e3ffdfd19ec | 1459 | print_string(message); |
nullsub | 0:1e3ffdfd19ec | 1460 | print_string(": {"); |
nullsub | 0:1e3ffdfd19ec | 1461 | for (int i=0; i < array_lenght; i++) { |
nullsub | 0:1e3ffdfd19ec | 1462 | print_string("%l",value[i]); |
nullsub | 0:1e3ffdfd19ec | 1463 | if (i != array_lenght-1) print_string(", "); |
nullsub | 0:1e3ffdfd19ec | 1464 | } |
nullsub | 0:1e3ffdfd19ec | 1465 | print_string("}\r\n"); |
nullsub | 0:1e3ffdfd19ec | 1466 | } |
nullsub | 0:1e3ffdfd19ec | 1467 | #endif |