/* servo_pen.cpp Part of Grbl_ESP32 copyright (c) 2018 - Bart Dring This file was modified for use on the ESP32 CPU. Do not use this with Grbl for atMega328P Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. Grbl is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with Grbl. If not, see . */ #include "grbl.h" #ifdef USE_PEN_SERVO static TaskHandle_t servoSyncTaskHandle = 0; // used to delay turn on bool servo_pen_enable = false; void servo_init() { grbl_send(CLIENT_SERIAL, "[MSG:Servo Pen Mode]\r\n"); // startup message //validate_servo_settings(true); // display any calibration errors // Debug stuff //grbl_sendf(CLIENT_SERIAL, "[MSG:Servo max,min pulse times %.4f sec,%.4f sec]\r\n", SERVO_MAX_PULSE_SEC, SERVO_MIN_PULSE_SEC); //grbl_sendf(CLIENT_SERIAL, "[MSG:Servo max,min pulse counts %d,%d]\r\n", SERVO_MAX_PULSE, SERVO_MIN_PULSE); validate_servo_settings(true); // will print errors // debug stuff servo_pen_enable = false; // start delay has not completed yet. // setup PWM channel ledcSetup(SERVO_PEN_CHANNEL_NUM, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); ledcAttachPin(SERVO_PEN_PIN, SERVO_PEN_CHANNEL_NUM); servo_disable(); // start it it off // setup a task that will calculate the determine and set the servo position xTaskCreatePinnedToCore( servoSyncTask, // task "servoSyncTask", // name for task 4096, // size of task stack NULL, // parameters 1, // priority &servoSyncTaskHandle, 0 // core ); } // turn off the PWM (0 duty) to prevent servo jitter when not in use. void servo_disable() { ledcWrite(SERVO_PEN_CHANNEL_NUM, 0); } // Grbl settings are used to calibrate the servo positions // They work on a percentage, so a value of 100 (100%) applies no calibration // Values outside a reasonable range can cause errors, so this function checks // that they are within a reasonable range bool validate_servo_settings(bool verbose) // make sure the settings are reasonable..otherwise reset the settings to default { bool settingsOK = true; if ( (settings.steps_per_mm[Z_AXIS] < SERVO_CAL_MIN) || (settings.steps_per_mm[Z_AXIS] > SERVO_CAL_MAX) ) { if (verbose) { grbl_sendf(CLIENT_SERIAL, "[MSG:Servo cal ($102) Error: %4.4f s/b between %.2f and %.2f]\r\n", settings.steps_per_mm[Z_AXIS], SERVO_CAL_MIN, SERVO_CAL_MAX); } settingsOK = false; } // Note: Max travel is set positive via $$, but stored as a negative number if ( (settings.max_travel[Z_AXIS] < -SERVO_CAL_MAX) || (settings.max_travel[Z_AXIS] > -SERVO_CAL_MIN) ) { if (verbose) { grbl_sendf(CLIENT_SERIAL, "[MSG:Servo cal ($132) Error: %4.4f s/b between %.2f and %.2f]\r\n", -settings.max_travel[Z_AXIS], SERVO_CAL_MIN, SERVO_CAL_MAX); } settingsOK = false; } return settingsOK; } // this is the task void servoSyncTask(void *pvParameters) { //int32_t current_position[N_AXIS]; // copy of current location //float m_pos[N_AXIS]; // machine position in mm TickType_t xLastWakeTime; const TickType_t xServoFrequency = SERVO_TIMER_INT_FREQ; // in ticks (typically ms) uint16_t servo_delay_counter = 0; float mpos_z, wpos_z; float z_offset; xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. while(true) { // don't ever return from this or the task dies if (sys.state != STATE_ALARM) { // don't move until alarm is cleared...typically homing if (!servo_pen_enable ) { servo_delay_counter++; servo_pen_enable = (servo_delay_counter > SERVO_TURNON_DELAY); } else { mpos_z = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); // get the machine Z in mm z_offset = gc_state.coord_system[Z_AXIS]+gc_state.coord_offset[Z_AXIS]; // get the current z work offset wpos_z = mpos_z - z_offset; // determine the current work Z calc_pen_servo(wpos_z); // calculate kinematics and move the servos } } vTaskDelayUntil(&xLastWakeTime, xServoFrequency); } } // calculate and set the PWM value for the servo void calc_pen_servo(float penZ) { uint32_t servo_pen_pulse_len; float servo_pen_pulse_min, servo_pen_pulse_max; if (!servo_pen_enable) { // only proceed if startup delay as expired return; } if (validate_servo_settings(false)) { // if calibration settings are OK then apply them if (bit_istrue(settings.dir_invert_mask,bit(Z_AXIS))) { // this allows the user to change the direction via settings // Apply a calibration to the minimum position servo_pen_pulse_max = SERVO_MIN_PULSE * (settings.steps_per_mm[Z_AXIS] / 100.0); // Apply a calibration to the maximum position servo_pen_pulse_min = SERVO_MAX_PULSE * (settings.max_travel[Z_AXIS] / -100.0); } else { // Apply a calibration to the minimum position servo_pen_pulse_min = SERVO_MIN_PULSE * (settings.steps_per_mm[Z_AXIS] / 100.0); // Apply a calibration to the maximum position servo_pen_pulse_max = SERVO_MAX_PULSE * (settings.max_travel[Z_AXIS] / -100.0); } } else { // use the defaults if (bit_istrue(settings.dir_invert_mask,bit(Z_AXIS))) { // this allows the user to change the direction via settings servo_pen_pulse_min = SERVO_MAX_PULSE; servo_pen_pulse_max = SERVO_MIN_PULSE; } else { servo_pen_pulse_min = SERVO_MIN_PULSE; servo_pen_pulse_max = SERVO_MAX_PULSE; } } // determine the pulse length servo_pen_pulse_len = (uint32_t)mapConstrain(penZ, SERVO_PEN_RANGE_MIN_MM, SERVO_PEN_RANGE_MAX_MM, servo_pen_pulse_min, servo_pen_pulse_max ); // skip setting value if it is unchanged if (ledcRead(SERVO_PEN_CHANNEL_NUM) == servo_pen_pulse_len) return; // update the PWM value // ledcWrite appears to have issues with interrupts, so make this a critical section portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED; portENTER_CRITICAL(&myMutex); ledcWrite(SERVO_PEN_CHANNEL_NUM, servo_pen_pulse_len); portEXIT_CRITICAL(&myMutex); } #endif