void handle_motors() { if (step_state == 1) return; last_active = seconds(); cli(); uint8_t state = step_state; uint8_t cf = current_fragment; uint8_t cs = current_sample; sei(); // Check probe. bool probed; if (settings[cf].flags & Settings::PROBING && probe_pin < NUM_DIGITAL_PINS) { if (state == 0) { if (GET(probe_pin) ^ bool(pin_flags & 2)) stopping = active_motors; probed = true; } else probed = false; } else probed = true; // If we didn't need to probe; don't block later on. if (stopping < 0) { // Check sensors. for (uint8_t m = 0; m < active_motors; ++m) { if (!(motor[m].intflags & Motor::ACTIVE)) continue; //debug("check %d", m); // Check sense pins. if (motor[m].sense_pin < NUM_DIGITAL_PINS) { if (GET(motor[m].sense_pin) ^ bool(motor[m].flags & Motor::SENSE_STATE)) { //debug("sense %d %x", m, motor[m].flags); motor[m].flags ^= Motor::SENSE_STATE; motor[m].flags |= (motor[m].flags & Motor::SENSE_STATE ? Motor::SENSE1 : Motor::SENSE0); uint8_t sense_state = motor[m].flags & Motor::SENSE_STATE ? 1 : 0; cli(); for (int mi = 0; mi < active_motors; ++mi) motor[mi].sense_pos[sense_state] = motor[mi].current_pos; sei(); } } // Check limit switches. if (stopping < 0) { int8_t value = buffer[cf][m][cs]; if (value == 0) continue; uint8_t limit_pin = value < 0 ? motor[m].limit_min_pin : motor[m].limit_max_pin; if (limit_pin < NUM_DIGITAL_PINS) { bool inverted = motor[m].flags & (value < 0 ? Motor::INVERT_LIMIT_MIN : Motor::INVERT_LIMIT_MAX); if (GET(limit_pin) ^ inverted) { debug("hit %d pos %d state %d sample %d", m, motor[m].current_pos, state, buffer[cf][m][cs]); stopping = m; motor[m].flags |= Motor::LIMIT; break; } } } } } if (stopping >= 0) { // Hit endstop or probe; disable timer interrupt. step_state = 1; //debug("hit limit %d curpos %ld cf %d ncf %d lf %d cfp %d", m, F(motor[m].current_pos), cf, notified_current_fragment, last_fragment, cs); // Notify host. limit_fragment_pos = cs; arch_set_speed(0); return; } if (homers > 0) { // Homing. if (state == 0) { probed = true; for (uint8_t m = 0; m < active_motors; ++m) { if (!(motor[m].intflags & Motor::ACTIVE)) continue; // Get twe "wrong" limit pin for the given direction. uint8_t limit_pin = (buffer[cf][m][cs] < 0 ? motor[m].limit_max_pin : motor[m].limit_min_pin); bool inverted = motor[m].flags & (buffer[cf][m][cs] < 0 ? Motor::INVERT_LIMIT_MAX : Motor::INVERT_LIMIT_MIN); if (limit_pin >= NUM_DIGITAL_PINS || GET(limit_pin) ^ inverted) { // Limit pin still triggered; continue moving. continue; } // Limit pin no longer triggered. Stop moving and possibly notify host. motor[m].intflags &= ~Motor::ACTIVE; if (!--homers) { arch_set_speed(0); return; } } } else probed = false; } if (state == 0 && probed) { if (homers > 0) current_sample = 0; // Use only the first sample for homing. step_state = homers > 0 || (settings[cf].flags & Settings::PROBING) ? 2 : 3; } }
void setup() { serial_buffer_head = serial_buffer; serial_buffer_tail = serial_buffer; serial_overflow = false; debug_value = 0x1337; arch_setup_start(); enabled_pins = NUM_DIGITAL_PINS; for (uint8_t p = 0; p < NUM_DIGITAL_PINS; ++p) { pin[p].duty = 255; // Reset state is unset, then unset the pin. pin[p].state = CTRL_UNSET << 2 | CTRL_RESET; UNSET(p); } pin_events = 0; notified_current_fragment = 0; current_fragment = notified_current_fragment; last_fragment = current_fragment; filling = 0; // Disable all adcs. for (uint8_t a = 0; a < NUM_ANALOG_INPUTS; ++a) { for (uint8_t i = 0; i < 2; ++i) { adc[a].linked[i] = ~0; adc[a].value[i] = 1 << 15; adc[a].is_on = false; } } adc_phase = INACTIVE; adc_current = ~0; adc_next = ~0; // Set up communication state. command_end = 0; had_data = false; ping = 0; out_busy = 0; ff_in = 0; ff_out = 0; reply_ready = 0; adcreply_ready = 0; timeout = false; timeout_time = 0; // Set up homing state. homers = 0; home_step_time = 0; // Set up movement state. last_len = 0; stopping = -1; arch_set_speed(0); current_len = 0; active_motors = 0; move_phase = 0; full_phase = 1; // Set up led state. led_fast = 0; led_last = millis(); led_phase = 0; led_pin = ~0; stop_pin = ~0; probe_pin = ~0; spiss_pin = ~0; audio = 0; audio_motor = 0; // Do arch-specific things. This fills printerid and uuid. arch_setup_end(); // Inform host of reset. send_id(CMD_STARTUP); }
void handle_motors() { cli(); uint8_t state = step_state; uint8_t cf = current_fragment; uint8_t cs = current_sample; sei(); if (state == STEP_STATE_STOP) return; last_active = seconds(); // Check probe. bool probed; if (settings[cf].flags & Settings::PROBING && probe_pin < NUM_DIGITAL_PINS) { if (state == STEP_STATE_RUN || state == STEP_STATE_NEXT || state == STEP_STATE_WAIT) { // Probing, but state is set to run; fix that. cli(); step_state = STEP_STATE_PROBE; // Update other variables which may have been changed by the ISR. state = step_state; cf = current_fragment; cs = current_sample; sei(); } if (state == STEP_STATE_PROBE) { if (GET(probe_pin) ^ bool(pin_flags & 2)) { step_state = STEP_STATE_STOP; stopping = active_motors; } probed = true; } else { probed = false; } } else probed = true; // If we didn't need to probe; don't block later on. if (stopping < 0) { if (stop_pin < NUM_DIGITAL_PINS && GET(stop_pin) ^ bool(pin_flags & 4)) { step_state = STEP_STATE_STOP; stopping = active_motors; } else { // Check sensors. for (uint8_t m = 0; m < active_motors; ++m) { if (!(motor[m].intflags & Motor::ACTIVE)) continue; // Check limit switches. if (stopping < 0) { int16_t value = *reinterpret_cast <volatile int16_t *>(&buffer[cf][m][cs]); if (value == 0) continue; uint8_t limit_pin = value < 0 ? motor[m].limit_min_pin : motor[m].limit_max_pin; if (limit_pin < NUM_DIGITAL_PINS) { bool inverted = motor[m].flags & (value < 0 ? Motor::INVERT_LIMIT_MIN : Motor::INVERT_LIMIT_MAX); if (GET(limit_pin) ^ inverted) { step_state = STEP_STATE_STOP; //debug("hit %d pin %d pos %d state %d sample %d", m, limit_pin, int(motor[m].current_pos), state, current_sample); stopping = m; motor[m].flags |= Motor::LIMIT; break; } } } } } } if (stopping >= 0) { // Notify host. // Use actual current sample, not the one that was used for testing. limit_fragment_pos = current_sample; arch_set_speed(0); return; } if (homers > 0) { // Homing. if (state == STEP_STATE_PROBE) { probed = true; for (uint8_t m = 0; m < active_motors; ++m) { if (!(motor[m].intflags & Motor::ACTIVE)) continue; // Get the "wrong" limit pin for the given direction. int16_t value = *reinterpret_cast <volatile int16_t *>(&buffer[cf][m][cs]); uint8_t limit_pin = (value < 0 ? motor[m].limit_max_pin : motor[m].limit_min_pin); bool inverted = motor[m].flags & (value < 0 ? Motor::INVERT_LIMIT_MAX : Motor::INVERT_LIMIT_MIN); if (limit_pin >= NUM_DIGITAL_PINS || GET(limit_pin) ^ inverted) { // Limit pin still triggered; continue moving. continue; } // Limit pin no longer triggered. Stop moving and possibly notify host. motor[m].intflags &= ~Motor::ACTIVE; if (!--homers) { arch_set_speed(0); return; } } } else probed = false; } if (state == STEP_STATE_PROBE && probed) { if (homers > 0) current_sample = 0; // Use only the first sample for homing. uint8_t new_state = homers > 0 || (settings[cf].flags & Settings::PROBING) ? STEP_STATE_SINGLE : STEP_STATE_RUN; //debug("step_state non zero %d (fragment %d)", new_state, cf); step_state = new_state; } cli(); if (step_state == STEP_STATE_NEXT || step_state == STEP_STATE_WAIT) step_state = STEP_STATE_RUN; sei(); }