loko u / sprinterPort

A port of the Sprinter Firmware to the mbed.

Dependencies:   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?

UserRevisionLine numberNew 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