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