* CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
*
******************************************************************************/
-\r
+
#ifndef __CM3_CORE_H__
#define __CM3_CORE_H__
-\r
+
#ifdef __cplusplus
extern "C" {
#endif
-\r
+
#define __CM3_CMSIS_VERSION_MAIN (0x01) /*!< [31:16] CMSIS HAL main version */
#define __CM3_CMSIS_VERSION_SUB (0x30) /*!< [15:0] CMSIS HAL sub version */
#define __CM3_CMSIS_VERSION ((__CM3_CMSIS_VERSION_MAIN << 16) | __CM3_CMSIS_VERSION_SUB) /*!< CMSIS HAL version number */
-\r
+
#define __CORTEX_M (0x03) /*!< Cortex core */
-\r
+
/**
* Lint configuration \n
* ----------------------- \n
* Note: To re-enable a Message, insert a space before 'lint' * \n
*
*/
-\r
+
/*lint -save */
/*lint -e10 */
/*lint -e530 */
/*lint -e750 */
/*lint -e528 */
/*lint -e751 */
-\r
-\r
+
+
#include <stdint.h> /* Include standard types */
-\r
+
#if defined (__ICCARM__)
#include <intrinsics.h> /* IAR Intrinsics */
#endif
-\r
-\r
+
+
#ifndef __NVIC_PRIO_BITS
#define __NVIC_PRIO_BITS 4 /*!< standard definition for NVIC Priority Bits */
#endif
-\r
-\r
-\r
-\r
+
+
+
+
/**
* IO definitions
*
* define access restrictions to peripheral registers
*/
-\r
+
#ifdef __cplusplus
#define __I volatile /*!< defines 'read only' permissions */
#else
#endif
#define __O volatile /*!< defines 'write only' permissions */
#define __IO volatile /*!< defines 'read / write' permissions */
-\r
-\r
-\r
+
+
+
/*******************************************************************************
* Register Abstraction
******************************************************************************/
-\r
-\r
+
+
/* System Reset */
#define NVIC_VECTRESET 0 /*!< Vector Reset Bit */
#define NVIC_SYSRESETREQ 2 /*!< System Reset Request */
#define NVIC_AIRCR_VECTKEY (0x5FA << 16) /*!< AIRCR Key for write access */
#define NVIC_AIRCR_ENDIANESS 15 /*!< Endianess */
-\r
+
/* Core Debug */
#define CoreDebug_DEMCR_TRCENA (1 << 24) /*!< DEMCR TRCENA enable */
#define ITM_TCR_ITMENA 1 /*!< ITM enable */
-\r
-\r
-\r
-\r
+
+
+
+
/* memory mapping struct for Nested Vectored Interrupt Controller (NVIC) */
typedef struct
{
uint32_t RESERVED5[644];
__O uint32_t STIR; /*!< Software Trigger Interrupt Register */
} NVIC_Type;
-\r
-\r
+
+
/* memory mapping struct for System Control Block */
typedef struct
{
__I uint32_t MMFR[4]; /*!< Memory Model Feature Register */
__I uint32_t ISAR[5]; /*!< ISA Feature Register */
} SCB_Type;
-\r
-\r
+
+
/* memory mapping struct for SysTick */
typedef struct
{
__IO uint32_t VAL; /*!< SysTick Current Value Register */
__I uint32_t CALIB; /*!< SysTick Calibration Register */
} SysTick_Type;
-\r
-\r
+
+
/* memory mapping structur for ITM */
typedef struct
{
__I uint32_t CID2;
__I uint32_t CID3;
} ITM_Type;
-\r
-\r
+
+
/* memory mapped struct for Interrupt Type */
typedef struct
{
uint32_t RESERVED1;
#endif
} InterruptType_Type;
-\r
-\r
+
+
/* Memory Protection Unit */
#if defined (__MPU_PRESENT) && (__MPU_PRESENT == 1)
typedef struct
__IO uint32_t RASR_A3; /*!< MPU Alias 3 Region Attribute and Size Register */
} MPU_Type;
#endif
-\r
-\r
+
+
/* Core Debug Register */
typedef struct
{
__IO uint32_t DCRDR; /*!< Debug Core Register Data Register */
__IO uint32_t DEMCR; /*!< Debug Exception and Monitor Control Register */
} CoreDebug_Type;
-\r
-\r
+
+
/* Memory mapping of Cortex-M3 Hardware */
#define SCS_BASE (0xE000E000) /*!< System Control Space Base Address */
#define ITM_BASE (0xE0000000) /*!< ITM Base Address */
#define SysTick_BASE (SCS_BASE + 0x0010) /*!< SysTick Base Address */
#define NVIC_BASE (SCS_BASE + 0x0100) /*!< NVIC Base Address */
#define SCB_BASE (SCS_BASE + 0x0D00) /*!< System Control Block Base Address */
-\r
+
#define InterruptType ((InterruptType_Type *) SCS_BASE) /*!< Interrupt Type Register */
#define SCB ((SCB_Type *) SCB_BASE) /*!< SCB configuration struct */
#define SysTick ((SysTick_Type *) SysTick_BASE) /*!< SysTick configuration struct */
#define NVIC ((NVIC_Type *) NVIC_BASE) /*!< NVIC configuration struct */
#define ITM ((ITM_Type *) ITM_BASE) /*!< ITM configuration struct */
#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */
-\r
+
#if defined (__MPU_PRESENT) && (__MPU_PRESENT == 1)
#define MPU_BASE (SCS_BASE + 0x0D90) /*!< Memory Protection Unit */
#define MPU ((MPU_Type*) MPU_BASE) /*!< Memory Protection Unit */
#endif
-\r
-\r
+
+
/*******************************************************************************
* Hardware Abstraction Layer
******************************************************************************/
-\r
-\r
+
+
#if defined ( __CC_ARM )
#define __ASM __asm /*!< asm keyword for ARM Compiler */
#define __INLINE __inline /*!< inline keyword for ARM Compiler */
-\r
+
#elif defined ( __ICCARM__ )
#define __ASM __asm /*!< asm keyword for IAR Compiler */
#define __INLINE inline /*!< inline keyword for IAR Compiler. Only avaiable in High optimization mode! */
-\r
+
#elif defined ( __GNUC__ )
#define __ASM __asm /*!< asm keyword for GNU Compiler */
#define __INLINE inline /*!< inline keyword for GNU Compiler */
-\r
+
#elif defined ( __TASKING__ )
#define __ASM __asm /*!< asm keyword for TASKING Compiler */
#define __INLINE inline /*!< inline keyword for TASKING Compiler */
-\r
+
#endif
-\r
-\r
+
+
/* ################### Compiler specific Intrinsics ########################### */
-\r
+
#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
/* ARM armcc specific functions */
-\r
+
#define __enable_fault_irq __enable_fiq
#define __disable_fault_irq __disable_fiq
-\r
+
#define __NOP __nop
#define __WFI __wfi
#define __WFE __wfe
#define __STREXB(value, ptr) __strex(value, ptr)
#define __STREXH(value, ptr) __strex(value, ptr)
#define __STREXW(value, ptr) __strex(value, ptr)
-\r
-\r
+
+
/* intrinsic unsigned long long __ldrexd(volatile void *ptr) */
/* intrinsic int __strexd(unsigned long long val, volatile void *ptr) */
/* intrinsic void __enable_irq(); */
/* intrinsic void __disable_irq(); */
-\r
-\r
+
+
#if (__ARMCC_VERSION < 400000)
-\r
+
#else /* (__ARMCC_VERSION >= 400000) */
-\r
-\r
+
+
/**
* @brief Remove the exclusive lock created by ldrex
*
* Removes the exclusive lock which is created by ldrex.
*/
#define __CLREX __clrex
-\r
+
/**
* @brief Return the Base Priority value
*
register uint32_t __regBasePri __ASM("basepri");
return(__regBasePri);
}
-\r
+
/**
* @brief Set the Base Priority value
*
register uint32_t __regBasePri __ASM("basepri");
__regBasePri = (basePri & 0xff);
}
-\r
+
/**
* @brief Return the Priority Mask value
*
register uint32_t __regPriMask __ASM("primask");
return(__regPriMask);
}
-\r
+
/**
* @brief Set the Priority Mask value
*
register uint32_t __regPriMask __ASM("primask");
__regPriMask = (priMask);
}
-\r
+
/**
* @brief Return the Fault Mask value
*
register uint32_t __regFaultMask __ASM("faultmask");
return(__regFaultMask);
}
-\r
+
/**
* @brief Set the Fault Mask value
*
register uint32_t __regFaultMask __ASM("faultmask");
__regFaultMask = (faultMask & 1);
}
-\r
+
/**
* @brief Return the Control Register value
*
register uint32_t __regControl __ASM("control");
return(__regControl);
}
-\r
+
/**
* @brief Set the Control Register value
*
register uint32_t __regControl __ASM("control");
__regControl = control;
}
-\r
+
#endif /* __ARMCC_VERSION */
-\r
-\r
-\r
+
+
+
#elif (defined (__ICCARM__)) /*------------------ ICC Compiler -------------------*/
/* IAR iccarm specific functions */
-\r
+
#define __enable_irq __enable_interrupt /*!< global Interrupt enable */
#define __disable_irq __disable_interrupt /*!< global Interrupt disable */
-\r
+
static __INLINE void __enable_fault_irq() { __ASM ("cpsie f"); }
static __INLINE void __disable_fault_irq() { __ASM ("cpsid f"); }
-\r
+
#define __NOP __no_operation() /*!< no operation intrinsic in IAR Compiler */
static __INLINE void __WFI() { __ASM ("wfi"); }
static __INLINE void __WFE() { __ASM ("wfe"); }
static __INLINE void __SEV() { __ASM ("sev"); }
static __INLINE void __CLREX() { __ASM ("clrex"); }
-\r
+
/* intrinsic void __ISB(void) */
/* intrinsic void __DSB(void) */
/* intrinsic void __DMB(void) */
/* intrinsic uint32_t __REVSH(uint32_t value); */
/* intrinsic unsigned long __STREX(unsigned long, unsigned long); */
/* intrinsic unsigned long __LDREX(unsigned long *); */
-\r
-\r
+
+
#elif (defined (__GNUC__)) /*------------------ GNU Compiler ---------------------*/
/* GNU gcc specific functions */
-\r
+
static __INLINE void __enable_irq() { __ASM volatile ("cpsie i"); }
static __INLINE void __disable_irq() { __ASM volatile ("cpsid i"); }
-\r
+
static __INLINE void __enable_fault_irq() { __ASM volatile ("cpsie f"); }
static __INLINE void __disable_fault_irq() { __ASM volatile ("cpsid f"); }
-\r
+
static __INLINE void __NOP() { __ASM volatile ("nop"); }
static __INLINE void __WFI() { __ASM volatile ("wfi"); }
static __INLINE void __WFE() { __ASM volatile ("wfe"); }
static __INLINE void __DSB() { __ASM volatile ("dsb"); }
static __INLINE void __DMB() { __ASM volatile ("dmb"); }
static __INLINE void __CLREX() { __ASM volatile ("clrex"); }
-\r
-\r
+
+
#elif (defined (__TASKING__)) /*------------------ TASKING Compiler ---------------------*/
/* TASKING carm specific functions */
-\r
+
/*
* The CMSIS functions have been implemented as intrinsics in the compiler.
* Please use "carm -?i" to get an up to date list of all instrinsics,
* Including the CMSIS ones.
*/
-\r
+
#endif
-\r
-\r
-\r
+
+
+
/* ########################## NVIC functions #################################### */
-\r
-\r
+
+
/**
* @brief Set the Priority Grouping in NVIC Interrupt Controller
*
{
uint32_t reg_value;
uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
- \r
+
reg_value = SCB->AIRCR; /* read old register configuration */
reg_value &= ~((0xFFFFU << 16) | (0x0F << 8)); /* clear bits to change */
reg_value = ((reg_value | NVIC_AIRCR_VECTKEY | (PriorityGroupTmp << 8))); /* Insert write key and priorty group */
SCB->AIRCR = reg_value;
}
-\r
+
/**
* @brief Get the Priority Grouping from NVIC Interrupt Controller
*
{
return ((SCB->AIRCR >> 8) & 0x07); /* read priority grouping field */
}
-\r
+
/**
* @brief Enable Interrupt in NVIC Interrupt Controller
*
{
NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* enable interrupt */
}
-\r
+
/**
* @brief Disable the interrupt line for external interrupt specified
*
{
NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
}
-\r
+
/**
* @brief Read the interrupt pending bit for a device specific interrupt source
*
{
return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
}
-\r
+
/**
* @brief Set the pending bit for an external interrupt
*
{
NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
}
-\r
+
/**
* @brief Clear the pending bit for an external interrupt
*
{
NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
}
-\r
+
/**
* @brief Read the active bit for an external interrupt
*
{
return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
}
-\r
+
/**
* @brief Set the priority for an interrupt
*
else {
NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */
}
-\r
+
/**
* @brief Read the priority for an interrupt
*
*/
static __INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
{
-\r
+
if(IRQn < 0) {
return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M3 system interrupts */
else {
return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */
}
-\r
-\r
+
+
/**
* @brief Encode the priority for an interrupt
*
uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
uint32_t PreemptPriorityBits;
uint32_t SubPriorityBits;
-\r
+
PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
- \r
+
return (
((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
((SubPriority & ((1 << (SubPriorityBits )) - 1)))
);
}
-\r
-\r
+
+
/**
* @brief Decode the priority of an interrupt
*
uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
uint32_t PreemptPriorityBits;
uint32_t SubPriorityBits;
-\r
+
PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
- \r
+
*pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
*pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1);
}
-\r
-\r
-\r
+
+
+
/* ################################## SysTick function ############################################ */
-\r
+
#if (!defined (__Vendor_SysTickConfig)) || (__Vendor_SysTickConfig == 0)
-\r
+
/* SysTick constants */
#define SYSTICK_ENABLE 0 /* Config-Bit to start or stop the SysTick Timer */
#define SYSTICK_TICKINT 1 /* Config-Bit to enable or disable the SysTick interrupt */
#define SYSTICK_CLKSOURCE 2 /* Clocksource has the offset 2 in SysTick Control and Status Register */
#define SYSTICK_MAXCOUNT ((1<<24) -1) /* SysTick MaxCount */
-\r
+
/**
* @brief Initialize and start the SysTick counter and its interrupt.
*
* Initialise the system tick timer and its interrupt and start the
* system tick timer / counter in free running mode to generate
* periodical interrupts.
+ *
+ * NOTE: Modified for Smoothie by adding enable_irq argument.
+ * Currently used by Spindle module.
*/
-static __INLINE uint32_t SysTick_Config(uint32_t ticks)
+static __INLINE uint32_t SysTick_Config(uint32_t ticks, bool enable_irq)
{
if (ticks > SYSTICK_MAXCOUNT) return (1); /* Reload value impossible */
-\r
- SysTick->LOAD = (ticks & SYSTICK_MAXCOUNT) - 1; /* set reload register */
- NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Cortex-M0 System Interrupts */
+
+ SysTick->LOAD = ticks; /* set reload register */
SysTick->VAL = (0x00); /* Load the SysTick Counter Value */
- SysTick->CTRL = (1 << SYSTICK_CLKSOURCE) | (1<<SYSTICK_ENABLE) | (1<<SYSTICK_TICKINT); /* Enable SysTick IRQ and SysTick Timer */
+ SysTick->CTRL = (1 << SYSTICK_CLKSOURCE) | (1<<SYSTICK_ENABLE); /* Enable SysTick IRQ and SysTick Timer */
+
+ if (enable_irq)
+ {
+ NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Cortex-M0 System Interrupts */
+ SysTick->CTRL |= (1<<SYSTICK_TICKINT);
+ }
+
return (0); /* Function successful */
}
-\r
+
#endif
-\r
-\r
-\r
-\r
-\r
+
+
+
+
+
/* ################################## Reset function ############################################ */
-\r
+
/**
* @brief Initiate a system reset request.
*
__DSB(); /* Ensure completion of memory access */
while(1); /* wait until reset */
}
-\r
-\r
+
+
/* ##################################### Debug In/Output function ########################################### */
-\r
+
extern volatile int ITM_RxBuffer; /* variable to receive characters */
#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /* value identifying ITM_RxBuffer is ready for next character */
-\r
-\r
+
+
/**
* @brief Outputs a character via the ITM channel 0
*
}
return (ch);
}
-\r
-\r
+
+
/**
* @brief Inputs a character via variable ITM_RxBuffer
*
*/
static __INLINE int ITM_ReceiveChar (void) {
int ch = -1; /* no character available */
-\r
+
if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
ch = ITM_RxBuffer;
ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */
}
- \r
+
return (ch);
}
-\r
-\r
+
+
/**
* @brief Check if a character via variable ITM_RxBuffer is available
*
* The function returns '1' if a character is available and '0' if no character is available.
*/
static __INLINE int ITM_CheckChar (void) {
-\r
+
if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
return (0); /* no character available */
} else {
return (1); /* character available */
}
}
-\r
-\r
-\r
+
+
+
#ifdef __cplusplus
}
#endif
-\r
+
#endif /* __CM3_CORE_H__ */
-\r
+
/*lint -restore */
--- /dev/null
+/*
+ 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 "Spindle.h"
+#include "Config.h"
+#include "libs/nuts_bolts.h"
+#include "checksumm.h"
+#include "ConfigValue.h"
+#include "Gcode.h"
+#include "StreamOutputPool.h"
+#include "SlowTicker.h"
+#include "Conveyor.h"
+#include "system_LPC17xx.h"
+
+#include "libs/Pin.h"
+#include "InterruptIn.h"
+#include "PwmOut.h"
+#include "port_api.h"
+
+#define spindle_enable_checksum CHECKSUM("spindle_enable")
+#define spindle_pwm_pin_checksum CHECKSUM("spindle_pwm_pin")
+#define spindle_pwm_period_checksum CHECKSUM("spindle_pwm_period")
+#define spindle_feedback_pin_checksum CHECKSUM("spindle_feedback_pin")
+#define spindle_pulses_per_rev_checksum CHECKSUM("spindle_pulses_per_rev")
+#define spindle_default_rpm_checksum CHECKSUM("spindle_default_rpm")
+#define spindle_control_P_checksum CHECKSUM("spindle_control_P")
+#define spindle_control_I_checksum CHECKSUM("spindle_control_I")
+#define spindle_control_D_checksum CHECKSUM("spindle_control_D")
+
+#define UPDATE_FREQ 1000
+
+Spindle::Spindle()
+{
+}
+
+void Spindle::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 = true;
+
+ if (!THEKERNEL->config->value(spindle_enable_checksum)->by_default(false)->as_bool())
+ {
+ delete this; // Spindle control module is disabled
+ return;
+ }
+
+ pulses_per_rev = THEKERNEL->config->value(spindle_pulses_per_rev_checksum)->by_default(1.0f)->as_number();
+ target_rpm = THEKERNEL->config->value(spindle_default_rpm_checksum)->by_default(5000.0f)->as_number();
+ control_P_term = THEKERNEL->config->value(spindle_control_P_checksum)->by_default(0.0001f)->as_number();
+ control_I_term = THEKERNEL->config->value(spindle_control_I_checksum)->by_default(0.0001f)->as_number();
+ control_D_term = THEKERNEL->config->value(spindle_control_D_checksum)->by_default(0.0001f)->as_number();
+
+ // Get the pin for hardware pwm
+ {
+ Pin *smoothie_pin = new Pin();
+ smoothie_pin->from_string(THEKERNEL->config->value(spindle_pwm_pin_checksum)->by_default("nc")->as_string());
+ spindle_pin = smoothie_pin->as_output()->hardware_pwm();
+ output_inverted = smoothie_pin->inverting;
+ delete smoothie_pin;
+ }
+
+ if (spindle_pin == NULL)
+ {
+ THEKERNEL->streams->printf("Error: Spindle PWM pin must be P2.0-2.5 or other PWM pin\n");
+ delete this;
+ return;
+ }
+
+ int period = THEKERNEL->config->value(spindle_pwm_period_checksum)->by_default(1000)->as_int();
+ spindle_pin->period_us(period);
+ spindle_pin->write(output_inverted ? 1 : 0);
+
+ // Get the pin for interrupt
+ {
+ Pin *smoothie_pin = new Pin();
+ smoothie_pin->from_string(THEKERNEL->config->value(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, &Spindle::on_pin_rise);
+ }
+ else
+ {
+ THEKERNEL->streams->printf("Error: Spindle feedback pin has to be on P0 or P2.\n");
+ delete this;
+ return;
+ }
+ delete smoothie_pin;
+ }
+
+ SysTick_Config(SYSTICK_MAXCOUNT, false);
+
+ THEKERNEL->slow_ticker->attach(UPDATE_FREQ, this, &Spindle::on_update_speed);
+ register_for_event(ON_GCODE_RECEIVED);
+ register_for_event(ON_GCODE_EXECUTE);
+}
+
+void Spindle::on_pin_rise()
+{
+ uint32_t timestamp = SYSTICK_MAXCOUNT - SysTick->VAL;
+ last_time = (timestamp - last_edge) & SYSTICK_MAXCOUNT;
+ last_edge = timestamp;
+ irq_count++;
+}
+
+uint32_t Spindle::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
+ current_rpm = SystemCoreClock * 60.0f / (t * pulses_per_rev);
+
+ 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 = 0.5f;
+ 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;
+ }
+ else
+ {
+ current_I_value = 0;
+ current_pwm_value = 0;
+ }
+
+ if (output_inverted)
+ spindle_pin->write(1.0f - current_pwm_value);
+ else
+ spindle_pin->write(current_pwm_value);
+
+ return 0;
+}
+
+
+void Spindle::on_gcode_received(void* argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
+
+ if (gcode->has_m)
+ {
+ if (gcode->m == 957)
+ {
+ // M957: report spindle speed
+ THEKERNEL->streams->printf("Current RPM: %5.0f Target RPM: %5.0f PWM value: %5.3f\n",
+ current_rpm, target_rpm, current_pwm_value);
+ gcode->mark_as_taken();
+ }
+ else if (gcode->m == 958)
+ {
+ // M958: set spindle PID parameters
+ if (gcode->has_letter('P'))
+ control_P_term = gcode->get_value('P');
+ if (gcode->has_letter('I'))
+ control_I_term = gcode->get_value('I');
+ if (gcode->has_letter('D'))
+ control_D_term = gcode->get_value('D');
+ THEKERNEL->streams->printf("P: %0.6f I: %0.6f D: %0.6f\n",
+ control_P_term, control_I_term, control_D_term);
+ }
+ else if (gcode->m == 3 || gcode->m == 5)
+ {
+ // M3: Spindle on, M5: Spindle off
+ THEKERNEL->conveyor->append_gcode(gcode);
+ gcode->mark_as_taken();
+ }
+ }
+}
+
+void Spindle::on_gcode_execute(void* argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
+
+ if (gcode->has_m)
+ {
+ if (gcode->m == 3)
+ {
+ // M3: Spindle on
+ spindle_on = true;
+
+ if (gcode->has_letter('S'))
+ {
+ target_rpm = gcode->get_value('S');
+ }
+ }
+ else if (gcode->m == 5)
+ {
+ spindle_on = false;
+ }
+ }
+}
+
HCoop / clinton / Smoothieware.git / commitdiff