|
@@ -113,7 +113,7 @@ static volatile char endstop_hit_bits = 0; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_
|
113
|
113
|
bool abort_on_endstop_hit = false;
|
114
|
114
|
#endif
|
115
|
115
|
|
116
|
|
-#if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
|
|
116
|
+#if HAS_MOTOR_CURRENT_PWM
|
117
|
117
|
#ifndef PWM_MOTOR_CURRENT
|
118
|
118
|
#define PWM_MOTOR_CURRENT DEFAULT_PWM_MOTOR_CURRENT
|
119
|
119
|
#endif
|
|
@@ -1238,13 +1238,19 @@ void digipot_init() {
|
1238
|
1238
|
digipot_current(i, digipot_motor_current[i]);
|
1239
|
1239
|
}
|
1240
|
1240
|
#endif
|
1241
|
|
- #ifdef MOTOR_CURRENT_PWM_XY_PIN
|
1242
|
|
- pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT);
|
1243
|
|
- pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT);
|
1244
|
|
- pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT);
|
1245
|
|
- digipot_current(0, motor_current_setting[0]);
|
1246
|
|
- digipot_current(1, motor_current_setting[1]);
|
1247
|
|
- digipot_current(2, motor_current_setting[2]);
|
|
1241
|
+ #if HAS_MOTOR_CURRENT_PWM
|
|
1242
|
+ #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
|
|
1243
|
+ pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT);
|
|
1244
|
+ digipot_current(0, motor_current_setting[0]);
|
|
1245
|
+ #endif
|
|
1246
|
+ #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
|
|
1247
|
+ pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT);
|
|
1248
|
+ digipot_current(1, motor_current_setting[1]);
|
|
1249
|
+ #endif
|
|
1250
|
+ #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
|
|
1251
|
+ pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT);
|
|
1252
|
+ digipot_current(2, motor_current_setting[2]);
|
|
1253
|
+ #endif
|
1248
|
1254
|
//Set timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
|
1249
|
1255
|
TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50);
|
1250
|
1256
|
#endif
|
|
@@ -1254,11 +1260,18 @@ void digipot_current(uint8_t driver, int current) {
|
1254
|
1260
|
#if HAS_DIGIPOTSS
|
1255
|
1261
|
const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
|
1256
|
1262
|
digitalPotWrite(digipot_ch[driver], current);
|
1257
|
|
- #elif defined(MOTOR_CURRENT_PWM_XY_PIN)
|
|
1263
|
+ #elif HAS_MOTOR_CURRENT_PWM
|
|
1264
|
+ #define _WRITE_CURRENT_PWM(P) analogWrite(P, 255L * current / (MOTOR_CURRENT_PWM_RANGE))
|
1258
|
1265
|
switch (driver) {
|
1259
|
|
- case 0: analogWrite(MOTOR_CURRENT_PWM_XY_PIN, 255L * current / (MOTOR_CURRENT_PWM_RANGE)); break;
|
1260
|
|
- case 1: analogWrite(MOTOR_CURRENT_PWM_Z_PIN, 255L * current / (MOTOR_CURRENT_PWM_RANGE)); break;
|
1261
|
|
- case 2: analogWrite(MOTOR_CURRENT_PWM_E_PIN, 255L * current / (MOTOR_CURRENT_PWM_RANGE)); break;
|
|
1266
|
+ #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
|
|
1267
|
+ case 0: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_XY_PIN); break;
|
|
1268
|
+ #endif
|
|
1269
|
+ #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
|
|
1270
|
+ case 1: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_Z_PIN); break;
|
|
1271
|
+ #endif
|
|
1272
|
+ #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
|
|
1273
|
+ case 2: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_E_PIN); break;
|
|
1274
|
+ #endif
|
1262
|
1275
|
}
|
1263
|
1276
|
#else
|
1264
|
1277
|
UNUSED(driver);
|