124 lines
3.9 KiB
C++
124 lines
3.9 KiB
C++
|
/*
|
||
|
|
solenoid_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 <http://www.gnu.org/licenses/>.
|
||
|
|
|
||
|
|
*/
|
||
|
|
#include "grbl.h"
|
||
|
|
|
||
|
|
#ifdef USE_PEN_SOLENOID
|
||
|
|
|
||
|
|
static TaskHandle_t solenoidSyncTaskHandle = 0;
|
||
|
|
|
||
|
|
// used to delay turn on
|
||
|
|
bool solenoid_pen_enable;
|
||
|
|
uint16_t solenoide_hold_count;
|
||
|
|
|
||
|
|
void solenoid_init()
|
||
|
|
{
|
||
|
|
grbl_send(CLIENT_SERIAL, "[MSG:Solenoid Mode]\r\n"); // startup message
|
||
|
|
//validate_servo_settings(true); // display any calibration errors
|
||
|
|
|
||
|
|
solenoid_pen_enable = false; // start delay has not completed yet.
|
||
|
|
solenoide_hold_count = 0; // initialize
|
||
|
|
|
||
|
|
// setup PWM channel
|
||
|
|
ledcSetup(SOLENOID_CHANNEL_NUM, SOLENOID_PWM_FREQ, SOLENOID_PWM_RES_BITS);
|
||
|
|
ledcAttachPin(SOLENOID_PEN_PIN, SOLENOID_CHANNEL_NUM);
|
||
|
|
|
||
|
|
solenoid_disable(); // start it it off
|
||
|
|
|
||
|
|
// setup a task that will calculate the determine and set the servo position
|
||
|
|
xTaskCreatePinnedToCore( solenoidSyncTask, // task
|
||
|
|
"solenoidSyncTask", // name for task
|
||
|
|
4096, // size of task stack
|
||
|
|
NULL, // parameters
|
||
|
|
1, // priority
|
||
|
|
&solenoidSyncTaskHandle,
|
||
|
|
0 // core
|
||
|
|
);
|
||
|
|
}
|
||
|
|
|
||
|
|
// turn off the PWM (0 duty)
|
||
|
|
void solenoid_disable()
|
||
|
|
{
|
||
|
|
ledcWrite(SOLENOID_CHANNEL_NUM, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
// this is the task
|
||
|
|
void solenoidSyncTask(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 xSolenoidFrequency = SOLENOID_TIMER_INT_FREQ; // in ticks (typically ms)
|
||
|
|
uint16_t solenoid_delay_counter = 0;
|
||
|
|
|
||
|
|
xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time.
|
||
|
|
while(true) { // don't ever return from this or the task dies
|
||
|
|
if (!solenoid_pen_enable) {
|
||
|
|
solenoid_delay_counter++;
|
||
|
|
solenoid_pen_enable = (solenoid_delay_counter > SOLENOID_TURNON_DELAY);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
memcpy(current_position,sys_position,sizeof(sys_position)); // get current position in step
|
||
|
|
system_convert_array_steps_to_mpos(m_pos,current_position); // convert to millimeters
|
||
|
|
calc_solenoid(m_pos[Z_AXIS]); // calculate kinematics and move the servos
|
||
|
|
}
|
||
|
|
vTaskDelayUntil(&xLastWakeTime, xSolenoidFrequency);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// calculate and set the PWM value for the servo
|
||
|
|
void calc_solenoid(float penZ)
|
||
|
|
{
|
||
|
|
uint32_t solenoid_pen_pulse_len;
|
||
|
|
|
||
|
|
if (!solenoid_pen_enable) // only proceed if startup delay as expired
|
||
|
|
return;
|
||
|
|
|
||
|
|
if (penZ < 0 && (sys.state != STATE_ALARM)) { // alarm also makes it go up
|
||
|
|
solenoide_hold_count = 0; // reset this count
|
||
|
|
solenoid_pen_pulse_len = 0; //
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (solenoide_hold_count < SOLENOID_PULSE_LEN_HOLD) {
|
||
|
|
solenoid_pen_pulse_len = SOLENOID_PULSE_LEN_UP;
|
||
|
|
solenoide_hold_count++;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
solenoid_pen_pulse_len = SOLENOID_PULSE_LEN_HOLD;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
// skip setting value if it is unchanged
|
||
|
|
if (ledcRead(SOLENOID_CHANNEL_NUM) == solenoid_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(SOLENOID_CHANNEL_NUM, solenoid_pen_pulse_len);
|
||
|
|
portEXIT_CRITICAL(&myMutex);
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|
||
|
|
|