Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Added "M4" for spindles #1476

Closed
wants to merge 1 commit into from
Closed

Added "M4" for spindles #1476

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
28 changes: 26 additions & 2 deletions src/modules/tools/spindle/AnalogSpindleControl.cpp
View file
Open in desktop
Expand Up @@ -21,6 +21,7 @@
#define spindle_pwm_pin_checksum CHECKSUM("pwm_pin")
#define spindle_pwm_period_checksum CHECKSUM("pwm_period")
#define spindle_switch_on_pin_checksum CHECKSUM("switch_on_pin")
#define spindle_reverse_dir_pin_checksum CHECKSUM("reverse_dir_pin")

void AnalogSpindleControl::on_module_loaded()
{
Expand Down Expand Up @@ -59,30 +60,53 @@ void AnalogSpindleControl::on_module_loaded()
switch_on = new Pin();
switch_on->from_string(switch_on_pin)->as_output()->set(false);
}

// Get digital out pin for reverse direction
std::string reverse_dir_pin = THEKERNEL->config->value(spindle_checksum, spindle_reverse_dir_pin_checksum)->by_default("nc")->as_string();
reverse_dir = NULL;
if(reverse_dir_pin.compare("nc") != 0) {
reverse_dir = new Pin();
reverse_dir->from_string(reverse_dir_pin)->as_output()->set(false);
}
}

void AnalogSpindleControl::turn_on()
{
// set the output for switching the VFD on
if(switch_on != NULL)
switch_on->set(true);
switch_on->set(true);
if(reverse_dir != NULL)
reverse_dir->set(false);
spindle_on = true;

}

void AnalogSpindleControl::turn_on_rev()
{
if(reverse_dir != NULL) {
// set the output for switching the VFD on
if(switch_on != NULL)
switch_on->set(true);
// set output for reverse direction
reverse_dir->set(true);
spindle_on = true;
}
}

void AnalogSpindleControl::turn_off()
{
// clear the output for switching the VFD on
if(switch_on != NULL)
switch_on->set(false);
// clear the output for reverse direction
if(reverse_dir != NULL)
reverse_dir->set(false);
spindle_on = false;
// set the PWM value to 0 to make sure it stops
update_pwm(0);

}


void AnalogSpindleControl::set_speed(int rpm)
{
// limit the requested RPM value
Expand Down
6 changes: 4 additions & 2 deletions src/modules/tools/spindle/AnalogSpindleControl.h
View file
Open in desktop
Expand Up @@ -26,15 +26,17 @@ class AnalogSpindleControl: public SpindleControl {

private:

Pin *switch_on; // digital output for switching the VFD on
Pin *switch_on; // digital output for switching the VFD on
Pin *reverse_dir; // digital output pin for reverse
mbed::PwmOut *pwm_pin; // PWM output for spindle speed control
bool output_inverted;

int target_rpm;
int min_rpm;
int max_rpm;

void turn_on(void);
void turn_on_rev(void);
void turn_off(void);
void set_speed(int);
void report_speed(void);
Expand Down
257 changes: 257 additions & 0 deletions src/modules/tools/spindle/EncoderSpindleControl.cpp
View file
Open in desktop
@@ -0,0 +1,257 @@
/*
This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
Smoothie 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.
Smoothie 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 Smoothie. If not, see <http://www.gnu.org/licenses/>.
*/

#include "libs/Module.h"
#include "libs/Kernel.h"
#include "EncoderSpindleControl.h"
#include "Config.h"
#include "checksumm.h"
#include "ConfigValue.h"
#include "StreamOutputPool.h"
#include "SlowTicker.h"
#include "Conveyor.h"
#include "system_LPC17xx.h"
#include "utils.h"

#include "libs/Pin.h"
#include "InterruptIn.h"
#include "PwmOut.h"
#include "port_api.h"
#include "us_ticker_api.h"

#define spindle_checksum CHECKSUM("spindle")
#define spindle_pwm_pin_checksum CHECKSUM("pwm_pin")
#define spindle_pwm_period_checksum CHECKSUM("pwm_period")
#define spindle_max_pwm_checksum CHECKSUM("max_pwm")
#define spindle_feedback_pin_checksum CHECKSUM("feedback_pin")
#define spindle_pulses_per_rev_checksum CHECKSUM("pulses_per_rev")
#define spindle_default_rpm_checksum CHECKSUM("default_rpm")
#define spindle_max_rpm_checksum CHECKSUM("max_rpm")
#define spindle_control_P_checksum CHECKSUM("control_P")
#define spindle_control_I_checksum CHECKSUM("control_I")
#define spindle_control_D_checksum CHECKSUM("control_D")
#define spindle_control_smoothing_checksum CHECKSUM("control_smoothing")
#define spindle_reverse_dir_pin_checksum CHECKSUM("reverse_dir_pin")
#define spindle_switch_on_pin_checksum CHECKSUM("switch_on_pin")

#define UPDATE_FREQ 1000

EncoderSpindleControl::EncoderSpindleControl()
{
}

void EncoderSpindleControl::on_module_loaded()
{
last_time = 0;
last_edge = 0;
current_rpm = 0;
current_I_value = 0;
current_pwm_value = 0;
time_since_update = 0;

spindle_on = false;

pulses_per_rev = THEKERNEL->config->value(spindle_checksum, spindle_pulses_per_rev_checksum)->by_default(1.0f)->as_number();
max_rpm = THEKERNEL->config->value(spindle_checksum, spindle_max_rpm_checksum)->by_default(10000.0f)->as_number();
default_rpm = THEKERNEL->config->value(spindle_checksum, spindle_default_rpm_checksum)->by_default(max_rpm * .1f)->as_number();
control_P_term = THEKERNEL->config->value(spindle_checksum, spindle_control_P_checksum)->by_default(0.0001f)->as_number();
control_I_term = THEKERNEL->config->value(spindle_checksum, spindle_control_I_checksum)->by_default(0.0001f)->as_number();
control_D_term = THEKERNEL->config->value(spindle_checksum, spindle_control_D_checksum)->by_default(0.0001f)->as_number();

// Smoothing value is low pass filter time constant in seconds.
float smoothing_time = THEKERNEL->config->value(spindle_checksum, spindle_control_smoothing_checksum)->by_default(0.1f)->as_number();
if (smoothing_time * UPDATE_FREQ < 1.0f)
smoothing_decay = 1.0f;
else
smoothing_decay = 1.0f / (UPDATE_FREQ * smoothing_time);


// Get the pin for hardware pwm
{
Pin *smoothie_pin = new Pin();
smoothie_pin->from_string(THEKERNEL->config->value(spindle_checksum, spindle_pwm_pin_checksum)->by_default("nc")->as_string());
pwm_pin = smoothie_pin->as_output()->hardware_pwm();
output_inverted = smoothie_pin->is_inverting();
delete smoothie_pin;
}

if (pwm_pin == NULL)
{
THEKERNEL->streams->printf("Error: Spindle PWM pin must be P2.0-2.5 or other PWM pin\n");
delete this;
return;
}

max_pwm = THEKERNEL->config->value(spindle_checksum, spindle_max_pwm_checksum)->by_default(1.0f)->as_number();

int period = THEKERNEL->config->value(spindle_checksum, spindle_pwm_period_checksum)->by_default(1000)->as_int();
pwm_pin->period_us(period);
pwm_pin->write(output_inverted ? 1 : 0);

// Get the pin for interrupt
{
Pin *smoothie_pin = new Pin();
smoothie_pin->from_string(THEKERNEL->config->value(spindle_checksum, spindle_feedback_pin_checksum)->by_default("nc")->as_string());
smoothie_pin->as_input();
if (smoothie_pin->port_number == 0 || smoothie_pin->port_number == 2) {
PinName pinname = port_pin((PortName)smoothie_pin->port_number, smoothie_pin->pin);
feedback_pin = new mbed::InterruptIn(pinname);
feedback_pin->rise(this, &EncoderSpindleControl::on_pin_rise);
NVIC_SetPriority(EINT3_IRQn, 16);
} else {
THEKERNEL->streams->printf("Error: Spindle feedback pin has to be on P0 or P2.\n");
delete this;
return;
}
delete smoothie_pin;
}

// Get digital out pin for reverse direction
reverse_dir_pin = THEKERNEL->config->value(spindle_checksum, spindle_reverse_dir_pin_checksum)->by_default("nc")->as_string();
reverse_dir = NULL;
if(reverse_dir_pin.compare("nc") != 0) {
reverse_dir = new Pin();
reverse_dir->from_string(reverse_dir_pin)->as_output()->set(false);
}

// Get digital out pin for switch on
switch_on_pin = THEKERNEL->config->value(spindle_checksum, spindle_switch_on_pin_checksum)->by_default("nc")->as_string();
switch_on = NULL;
if(switch_on_pin.compare("nc") != 0) {
switch_on = new Pin();
switch_on->from_string(switch_on_pin)->as_output()->set(false);
}

THEKERNEL->slow_ticker->attach(UPDATE_FREQ, this, &EncoderSpindleControl::on_update_speed);
}

void EncoderSpindleControl::on_pin_rise()
{
uint32_t timestamp = us_ticker_read();
last_time = timestamp - last_edge;
last_edge = timestamp;
irq_count++;
}

uint32_t EncoderSpindleControl::on_update_speed(uint32_t dummy)
{
// If we don't get any interrupts for 1 second, set current RPM to 0
uint32_t new_irq = irq_count;
if (last_irq != new_irq)
time_since_update = 0;
else
time_since_update++;
last_irq = new_irq;

if (time_since_update > UPDATE_FREQ)
last_time = 0;

// Calculate current RPM
uint32_t t = last_time;
if (t == 0) {
current_rpm = 0;
} else {
float new_rpm = 1000000 * 60.0f / (t * pulses_per_rev);
current_rpm = smoothing_decay * new_rpm + (1.0f - smoothing_decay) * current_rpm;
}

if (spindle_on) {

float error = target_rpm - current_rpm;

current_I_value += control_I_term * error * 1.0f / UPDATE_FREQ;
current_I_value = confine(current_I_value, -1.0f, 1.0f);

float new_pwm = default_rpm / max_rpm;
new_pwm += control_P_term * error;
new_pwm += current_I_value;
new_pwm += control_D_term * UPDATE_FREQ * (error - prev_error);
new_pwm = confine(new_pwm, 0.0f, 1.0f);
prev_error = error;

current_pwm_value = new_pwm;

if (current_pwm_value > max_pwm) {
current_pwm_value = max_pwm;
}
} else {
current_I_value = 0;
current_pwm_value = 0;
}

if (output_inverted)
pwm_pin->write(1.0f - current_pwm_value);
else
pwm_pin->write(current_pwm_value);

return 0;
}

void EncoderSpindleControl::turn_on() {
// clear output for reverse direction
if(reverse_dir != NULL)
reverse_dir->set(false);
// set output for switch on
if(switch_on != NULL)
switch_on->set(true);
spindle_on = true;
}

void EncoderSpindleControl::turn_on_rev() {
// check if reverse is being used
if(reverse_dir != NULL) {
// set output for switch on
if(switch_on != NULL)
switch_on->set(true);
// set output for reverse direction
reverse_dir->set(true);
spindle_on = true;
}
}

void EncoderSpindleControl::turn_off() {
// clear output for reverse direction
if(reverse_dir != NULL)
reverse_dir->set(false);
// clear output for switch on
if(switch_on != NULL)
switch_on->set(false);
spindle_on = false;
}


void EncoderSpindleControl::set_speed(int rpm) {
target_rpm = rpm;
}


void EncoderSpindleControl::report_speed() {
THEKERNEL->streams->printf("Current RPM: %5.0f Target RPM: %5.0f PWM value: %5.3f \n",
current_rpm, target_rpm, current_pwm_value);
}


void EncoderSpindleControl::set_p_term(float p) {
control_P_term = p;
}


void EncoderSpindleControl::set_i_term(float i) {
control_I_term = i;
}


void EncoderSpindleControl::set_d_term(float d) {
control_D_term = d;
}


void EncoderSpindleControl::report_settings() {
THEKERNEL->streams->printf("P: %0.6f I: %0.6f D: %0.6f\n",
control_P_term, control_I_term, control_D_term);
}