From 5d8036e554fd0f73e9e0d4642cd54194ea2f9924 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 25 Mar 2016 04:05:07 -0700 Subject: [PATCH] Code cleanup around MOTOR_CURRENT_PWM options --- Marlin/Conditionals.h | 2 ++ Marlin/Marlin_main.cpp | 6 +++--- Marlin/servo.h | 46 +++++++++++++++++++----------------------- Marlin/stepper.cpp | 37 ++++++++++++++++++++++----------- 4 files changed, 51 insertions(+), 40 deletions(-) diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h index 376946ab7..a1ab07d89 100644 --- a/Marlin/Conditionals.h +++ b/Marlin/Conditionals.h @@ -570,6 +570,8 @@ #define HAS_E3_STEP (PIN_EXISTS(E3_STEP)) #define HAS_E4_STEP (PIN_EXISTS(E4_STEP)) + #define HAS_MOTOR_CURRENT_PWM (PIN_EXISTS(MOTOR_CURRENT_PWM_XY) || PIN_EXISTS(MOTOR_CURRENT_PWM_Z) || PIN_EXISTS(MOTOR_CURRENT_PWM_E)) + /** * Helper Macros for heaters and extruder fan */ diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index edc36914c..8baaa7cdd 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -5729,13 +5729,13 @@ inline void gcode_M907() { if (code_seen('B')) digipot_current(4, code_value()); if (code_seen('S')) for (int i = 0; i <= 4; i++) digipot_current(i, code_value()); #endif - #ifdef MOTOR_CURRENT_PWM_XY_PIN + #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY) if (code_seen('X')) digipot_current(0, code_value()); #endif - #ifdef MOTOR_CURRENT_PWM_Z_PIN + #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z) if (code_seen('Z')) digipot_current(1, code_value()); #endif - #ifdef MOTOR_CURRENT_PWM_E_PIN + #if PIN_EXISTS(MOTOR_CURRENT_PWM_E) if (code_seen('E')) digipot_current(2, code_value()); #endif #if ENABLED(DIGIPOT_I2C) diff --git a/Marlin/servo.h b/Marlin/servo.h index 13ce9f100..d764fc440 100644 --- a/Marlin/servo.h +++ b/Marlin/servo.h @@ -85,39 +85,35 @@ //#define _useTimer1 #define _useTimer3 #define _useTimer4 - #ifndef MOTOR_CURRENT_PWM_XY_PIN - //Timer 5 is used for motor current PWM and can't be used for servos. - #define _useTimer5 - //typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; - typedef enum { _timer5, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; - #else - typedef enum {_timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; + #if !HAS_MOTOR_CURRENT_PWM + #define _useTimer5 // Timer 5 is used for motor current PWM and can't be used for servos. #endif - #elif defined(__AVR_ATmega32U4__) - //#define _useTimer1 #define _useTimer3 - //typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; - typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; - #elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) #define _useTimer3 - //#define _useTimer1 - //typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; - typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; - -#elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) ||defined(__AVR_ATmega2561__) +#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__) #define _useTimer3 - //#define _useTimer1 - //typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; - typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; +#else + // everything else +#endif -#else // everything else - //#define _useTimer1 - //typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; - typedef enum { _Nbr_16timers } timer16_Sequence_t ; +typedef enum { + #if ENABLED(_useTimer1) + _timer1, + #endif + #if ENABLED(_useTimer3) + _timer3, + #endif + #if ENABLED(_useTimer4) + _timer4, + #endif + #if ENABLED(_useTimer5) + _timer5, + #endif + _Nbr_16timers +} timer16_Sequence_t; -#endif #define Servo_VERSION 2 // software version of this library diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 32ae37eed..aaf4db7e5 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -113,7 +113,7 @@ static volatile char endstop_hit_bits = 0; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_ bool abort_on_endstop_hit = false; #endif -#if PIN_EXISTS(MOTOR_CURRENT_PWM_XY) +#if HAS_MOTOR_CURRENT_PWM #ifndef PWM_MOTOR_CURRENT #define PWM_MOTOR_CURRENT DEFAULT_PWM_MOTOR_CURRENT #endif @@ -1238,13 +1238,19 @@ void digipot_init() { digipot_current(i, digipot_motor_current[i]); } #endif - #ifdef MOTOR_CURRENT_PWM_XY_PIN - pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT); - pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT); - pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT); - digipot_current(0, motor_current_setting[0]); - digipot_current(1, motor_current_setting[1]); - digipot_current(2, motor_current_setting[2]); + #if HAS_MOTOR_CURRENT_PWM + #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY) + pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT); + digipot_current(0, motor_current_setting[0]); + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z) + pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT); + digipot_current(1, motor_current_setting[1]); + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_E) + pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT); + digipot_current(2, motor_current_setting[2]); + #endif //Set timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise) TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50); #endif @@ -1254,11 +1260,18 @@ void digipot_current(uint8_t driver, int current) { #if HAS_DIGIPOTSS const uint8_t digipot_ch[] = DIGIPOT_CHANNELS; digitalPotWrite(digipot_ch[driver], current); - #elif defined(MOTOR_CURRENT_PWM_XY_PIN) + #elif HAS_MOTOR_CURRENT_PWM + #define _WRITE_CURRENT_PWM(P) analogWrite(P, 255L * current / (MOTOR_CURRENT_PWM_RANGE)) switch (driver) { - case 0: analogWrite(MOTOR_CURRENT_PWM_XY_PIN, 255L * current / (MOTOR_CURRENT_PWM_RANGE)); break; - case 1: analogWrite(MOTOR_CURRENT_PWM_Z_PIN, 255L * current / (MOTOR_CURRENT_PWM_RANGE)); break; - case 2: analogWrite(MOTOR_CURRENT_PWM_E_PIN, 255L * current / (MOTOR_CURRENT_PWM_RANGE)); break; + #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY) + case 0: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_XY_PIN); break; + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z) + case 1: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_Z_PIN); break; + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_E) + case 2: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_E_PIN); break; + #endif } #else UNUSED(driver);