src/modules/tools/spindle/PWMSpindleControl.cpp

   1 /*
   2       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
   3       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.
   4       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.
   5       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
   6 */
   7
   8 #include "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include "PWMSpindleControl.h"
  11 #include "Config.h"
  12 #include "checksumm.h"
  13 #include "ConfigValue.h"
  14 #include "StreamOutputPool.h"
  15 #include "SlowTicker.h"
  16 #include "Conveyor.h"
  17 #include "system_LPC17xx.h"
  18 #include "utils.h"
  19
  20 #include "libs/Pin.h"
  21 #include "InterruptIn.h"
  22 #include "PwmOut.h"
  23 #include "port_api.h"
  24 #include "us_ticker_api.h"
  25
  26 #define spindle_checksum                    CHECKSUM("spindle")
  27 #define spindle_pwm_pin_checksum            CHECKSUM("pwm_pin")
  28 #define spindle_pwm_period_checksum         CHECKSUM("pwm_period")
  29 #define spindle_max_pwm_checksum            CHECKSUM("max_pwm")
  30 #define spindle_feedback_pin_checksum       CHECKSUM("feedback_pin")
  31 #define spindle_pulses_per_rev_checksum     CHECKSUM("pulses_per_rev")
  32 #define spindle_default_rpm_checksum        CHECKSUM("default_rpm")
  33 #define spindle_control_P_checksum          CHECKSUM("control_P")
  34 #define spindle_control_I_checksum          CHECKSUM("control_I")
  35 #define spindle_control_D_checksum          CHECKSUM("control_D")
  36 #define spindle_control_smoothing_checksum  CHECKSUM("control_smoothing")
  37
  38 #define UPDATE_FREQ 1000
  39
  40 PWMSpindleControl::PWMSpindleControl()
  41 {
  42 }
  43
  44 void PWMSpindleControl::on_module_loaded()
  45 {
  46     last_time = 0;
  47     last_edge = 0;
  48     current_rpm = 0;
  49     current_I_value = 0;
  50     current_pwm_value = 0;
  51     time_since_update = 0;
  52
  53     spindle_on = false;
  54
  55     pulses_per_rev = THEKERNEL->config->value(spindle_checksum, spindle_pulses_per_rev_checksum)->by_default(1.0f)->as_number();
  56     target_rpm = THEKERNEL->config->value(spindle_checksum, spindle_default_rpm_checksum)->by_default(5000.0f)->as_number();
  57     control_P_term = THEKERNEL->config->value(spindle_checksum, spindle_control_P_checksum)->by_default(0.0001f)->as_number();
  58     control_I_term = THEKERNEL->config->value(spindle_checksum, spindle_control_I_checksum)->by_default(0.0001f)->as_number();
  59     control_D_term = THEKERNEL->config->value(spindle_checksum, spindle_control_D_checksum)->by_default(0.0001f)->as_number();
  60
  61     // Smoothing value is low pass filter time constant in seconds.
  62     float smoothing_time = THEKERNEL->config->value(spindle_checksum, spindle_control_smoothing_checksum)->by_default(0.1f)->as_number();
  63     if (smoothing_time * UPDATE_FREQ < 1.0f)
  64         smoothing_decay = 1.0f;
  65     else
  66         smoothing_decay = 1.0f / (UPDATE_FREQ * smoothing_time);
  67
  68     // Get the pin for hardware pwm
  69     {
  70         Pin *smoothie_pin = new Pin();
  71         smoothie_pin->from_string(THEKERNEL->config->value(spindle_checksum, spindle_pwm_pin_checksum)->by_default("nc")->as_string());
  72         pwm_pin = smoothie_pin->as_output()->hardware_pwm();
  73         output_inverted = smoothie_pin->is_inverting();
  74         delete smoothie_pin;
  75     }
  76
  77     if (pwm_pin == NULL)
  78     {
  79         THEKERNEL->streams->printf("Error: Spindle PWM pin must be P2.0-2.5 or other PWM pin\n");
  80         delete this;
  81         return;
  82     }
  83
  84     max_pwm = THEKERNEL->config->value(spindle_checksum, spindle_max_pwm_checksum)->by_default(1.0f)->as_number();
  85
  86     int period = THEKERNEL->config->value(spindle_checksum, spindle_pwm_period_checksum)->by_default(1000)->as_int();
  87     pwm_pin->period_us(period);
  88     pwm_pin->write(output_inverted ? 1 : 0);
  89
  90     // Get the pin for interrupt
  91     {
  92         Pin *smoothie_pin = new Pin();
  93         smoothie_pin->from_string(THEKERNEL->config->value(spindle_checksum, spindle_feedback_pin_checksum)->by_default("nc")->as_string());
  94         smoothie_pin->as_input();
  95         if (smoothie_pin->port_number == 0 || smoothie_pin->port_number == 2) {
  96             PinName pinname = port_pin((PortName)smoothie_pin->port_number, smoothie_pin->pin);
  97             feedback_pin = new mbed::InterruptIn(pinname);
  98             feedback_pin->rise(this, &PWMSpindleControl::on_pin_rise);
  99             NVIC_SetPriority(EINT3_IRQn, 16);
 100         } else {
 101             THEKERNEL->streams->printf("Error: Spindle feedback pin has to be on P0 or P2.\n");
 102             delete this;
 103             return;
 104         }
 105         delete smoothie_pin;
 106     }
 107
 108     THEKERNEL->slow_ticker->attach(UPDATE_FREQ, this, &PWMSpindleControl::on_update_speed);
 109 }
 110
 111 void PWMSpindleControl::on_pin_rise()
 112 {
 113     uint32_t timestamp = us_ticker_read();
 114     last_time = timestamp - last_edge;
 115     last_edge = timestamp;
 116     irq_count++;
 117 }
 118
 119 uint32_t PWMSpindleControl::on_update_speed(uint32_t dummy)
 120 {
 121     // If we don't get any interrupts for 1 second, set current RPM to 0
 122     uint32_t new_irq = irq_count;
 123     if (last_irq != new_irq)
 124         time_since_update = 0;
 125     else
 126         time_since_update++;
 127     last_irq = new_irq;
 128
 129     if (time_since_update > UPDATE_FREQ)
 130         last_time = 0;
 131
 132     // Calculate current RPM
 133     uint32_t t = last_time;
 134     if (t == 0) {
 135         current_rpm = 0;
 136     } else {
 137         float new_rpm = 1000000 * 60.0f / (t * pulses_per_rev);
 138         current_rpm = smoothing_decay * new_rpm + (1.0f - smoothing_decay) * current_rpm;
 139     }
 140
 141     if (spindle_on) {
 142         float error = target_rpm - current_rpm;
 143
 144         current_I_value += control_I_term * error * 1.0f / UPDATE_FREQ;
 145         current_I_value = confine(current_I_value, -1.0f, 1.0f);
 146
 147         float new_pwm = 0.5f;
 148         new_pwm += control_P_term * error;
 149         new_pwm += current_I_value;
 150         new_pwm += control_D_term * UPDATE_FREQ * (error - prev_error);
 151         new_pwm = confine(new_pwm, 0.0f, 1.0f);
 152         prev_error = error;
 153
 154         current_pwm_value = new_pwm;
 155
 156         if (current_pwm_value > max_pwm) {
 157             current_pwm_value = max_pwm;
 158         }
 159     } else {
 160         current_I_value = 0;
 161         current_pwm_value = 0;
 162     }
 163
 164     if (output_inverted)
 165         pwm_pin->write(1.0f - current_pwm_value);
 166     else
 167         pwm_pin->write(current_pwm_value);
 168
 169     return 0;
 170 }
 171
 172 void PWMSpindleControl::turn_on() {
 173     spindle_on = true;
 174 }
 175
 176 void PWMSpindleControl::turn_off() {
 177     spindle_on = false;
 178 }
 179
 180
 181 void PWMSpindleControl::set_speed(int rpm) {
 182     target_rpm = rpm;
 183 }
 184
 185
 186 void PWMSpindleControl::report_speed() {
 187     THEKERNEL->streams->printf("Current RPM: %5.0f  Target RPM: %5.0f  PWM value: %5.3f\n",
 188                                current_rpm, target_rpm, current_pwm_value);
 189 }
 190
 191
 192 void PWMSpindleControl::set_p_term(float p) {
 193     control_P_term = p;
 194 }
 195
 196
 197 void PWMSpindleControl::set_i_term(float i) {
 198     control_I_term = i;
 199 }
 200
 201
 202 void PWMSpindleControl::set_d_term(float d) {
 203     control_D_term = d;
 204 }
 205
 206
 207 void PWMSpindleControl::report_settings() {
 208     THEKERNEL->streams->printf("P: %0.6f I: %0.6f D: %0.6f\n",
 209                                control_P_term, control_I_term, control_D_term);
 210 }
 211