heater-arm.c: implement heater_set().

Works very nicely from full off (M106 S0) to full on (M106 S255).

Test: M106 should work now as expected. M106 S0 should turn full
off, M106 S255 should turn full on, both without any spike on the
scope.
This commit is contained in:
Markus Hitter 2015-08-09 15:53:16 +02:00
parent d7b59e2d33
commit 1aeb04329c
2 changed files with 52 additions and 8 deletions

View File

@ -465,11 +465,9 @@ void process_gcode_command() {
if (temp_achieved() == 0) {
enqueue(NULL);
}
#ifndef __ARMEL_NOTYET__
#ifdef DC_EXTRUDER
heater_set(DC_EXTRUDER, DC_EXTRUDER_PWM);
#endif
#endif /* __ARMEL_NOTYET__ */
break;
// M5/M103- extruder off
@ -478,11 +476,9 @@ void process_gcode_command() {
//? --- M103: extruder off ---
//?
//? Undocumented.
#ifndef __ARMEL_NOTYET__
#ifdef DC_EXTRUDER
heater_set(DC_EXTRUDER, 0);
#endif
#endif /* __ARMEL_NOTYET__ */
break;
case 104:
@ -556,9 +552,7 @@ void process_gcode_command() {
#endif
if ( ! next_target.seen_S)
break;
#ifndef __ARMEL_NOTYET__
heater_set(next_target.P, next_target.S);
#endif /* __ARMEL_NOTYET__ */
break;
case 110:

View File

@ -5,6 +5,11 @@
#if defined TEACUP_C_INCLUDE && defined __ARMEL__
#include "cmsis-lpc11xx.h"
#include "pinio.h"
#include "sersendf.h"
#include "debug.h"
/**
Test configuration.
*/
@ -45,6 +50,25 @@
*/
#define PWM_SCALE 255
/** \struct heater_definition_t
Holds pinout data to allow changing PWM output after initialisation. Port,
pin, PWM channel if used. After inititalisation we can no longer do the
#include "config_wrapper.h" trick.
*/
typedef struct {
/// Pointer to the match register which changes PWM duty.
__IO uint32_t* match;
} heater_definition_t;
#undef DEFINE_HEATER
#define DEFINE_HEATER(name, pin, pwm) \
{ &(pin ## _TIMER->MR[pin ## _MATCH]) },
static const heater_definition_t heaters[NUM_HEATERS] = {
#include "config_wrapper.h"
};
#undef DEFINE_HEATER
/** Initialise heater subsystem.
@ -111,9 +135,8 @@ void heater_init() {
pin ## _TIMER->MCR = (1 << 10); /* Reset on Match 3. */ \
/* PWM_SCALE - 1, so match = 255 is full off. */ \
pin ## _TIMER->MR[3] = PWM_SCALE - 1; /* Match 3 at 254. */ \
/* TODO: Heaters should start at zero, of course. 10% is as demo. */ \
pin ## _TIMER->MR[pin ## _MATCH] = /* Match pin = duty. */ \
(PWM_SCALE * 0.9); \
PWM_SCALE; \
/*pin ## _TIMER->CCR = 0; ( = reset value) No pin capture. */ \
pin ## _TIMER->EMR |= ((1 << pin ## _MATCH) /* Connect to pin. */ \
| (0x03 << ((pin ## _MATCH * 2) + 4))); /* Toggle pin on match.*/ \
@ -129,4 +152,31 @@ void heater_init() {
#endif /* 0 */
}
/** Set PWM output.
\param index The heater we're setting the output for.
\param value The PWM value to write, range 0 (off) to 255 (full on).
This function is called by M106 or, if a temp sensor is connected to the
heater, every few milliseconds by its PID handler. Using M106 on an output
with a sensor changes its setting only for a short moment.
*/
void heater_set(heater_t index, uint8_t value) {
if (index < NUM_HEATERS) {
heaters_runtime[index].heater_output = value;
// Remember, we scale, and duty cycle is inverted.
*heaters[index].match = PWM_SCALE - ((uint32_t)value * (PWM_SCALE / 255));
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR("PWM %su = %lu\n"), index, *heaters[index].match);
if (value)
power_on();
}
}
#endif /* defined TEACUP_C_INCLUDE && defined __ARMEL__ */