Browse Source

Move stepper DAC to a separate file

Scott Lahteine 9 years ago
parent
commit
59483221d5
4 changed files with 125 additions and 64 deletions
  1. 4
    0
      Marlin/Marlin_main.cpp
  2. 1
    64
      Marlin/stepper.cpp
  3. 87
    0
      Marlin/stepper_dac.cpp
  4. 33
    0
      Marlin/stepper_dac.h

+ 4
- 0
Marlin/Marlin_main.cpp View File

@@ -68,6 +68,10 @@
68 68
   #include <SPI.h>
69 69
 #endif
70 70
 
71
+#if ENABLED(DAC_STEPPER_CURRENT)
72
+  #include "stepper_dac.h"
73
+#endif
74
+
71 75
 /**
72 76
  * Look here for descriptions of G-codes:
73 77
  *  - http://linuxcnc.org/handbook/gcode/g-code.html

+ 1
- 64
Marlin/stepper.cpp View File

@@ -30,7 +30,7 @@
30 30
 #include "language.h"
31 31
 #include "cardreader.h"
32 32
 #include "speed_lookuptable.h"
33
-#include "dac_mcp4728.h"
33
+
34 34
 #if HAS_DIGIPOTSS
35 35
   #include <SPI.h>
36 36
 #endif
@@ -1227,69 +1227,6 @@ void digipot_current(uint8_t driver, int current) {
1227 1227
   #endif
1228 1228
 }
1229 1229
 
1230
-#if ENABLED(DAC_STEPPER_CURRENT)
1231
-
1232
-  bool dac_present = false;
1233
-  const uint8_t dac_order[NUM_AXIS] = DAC_STEPPER_ORDER;
1234
-
1235
-  int dac_init() {
1236
-    mcp4728_init();
1237
-
1238
-    if (mcp4728_simpleCommand(RESET)) return -1;
1239
-
1240
-    dac_present = true;
1241
-
1242
-    mcp4728_setVref_all(DAC_STEPPER_VREF);
1243
-    mcp4728_setGain_all(DAC_STEPPER_GAIN);
1244
-
1245
-    return 0;
1246
-  }
1247
-
1248
-  void dac_current_percent(uint8_t channel, float val) {
1249
-    if (!dac_present) return;
1250
-
1251
-    NOMORE(val, 100);
1252
-
1253
-    mcp4728_analogWrite(dac_order[channel], val * DAC_STEPPER_MAX / 100);
1254
-    mcp4728_simpleCommand(UPDATE);
1255
-  }
1256
-
1257
-  void dac_current_raw(uint8_t channel, uint16_t val) {
1258
-    if (!dac_present) return;
1259
-
1260
-    NOMORE(val, DAC_STEPPER_MAX);
1261
-
1262
-    mcp4728_analogWrite(dac_order[channel], val);
1263
-    mcp4728_simpleCommand(UPDATE);
1264
-  }
1265
-
1266
-  static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) / DAC_STEPPER_MAX; }
1267
-  static float dac_amps(int8_t n) { return ((2.048 * mcp4728_getValue(dac_order[n])) / 4096.0) / (8.0 * DAC_STEPPER_SENSE); }
1268
-
1269
-  void dac_print_values() {
1270
-    if (!dac_present) return;
1271
-
1272
-    SERIAL_ECHO_START;
1273
-    SERIAL_ECHOLNPGM("Stepper current values in % (Amps):");
1274
-    SERIAL_ECHO_START;
1275
-    SERIAL_ECHOPAIR(" X:",  dac_perc(0));
1276
-    SERIAL_ECHOPAIR(" (",   dac_amps(0));
1277
-    SERIAL_ECHOPAIR(") Y:", dac_perc(1));
1278
-    SERIAL_ECHOPAIR(" (",   dac_amps(1));
1279
-    SERIAL_ECHOPAIR(") Z:", dac_perc(2));
1280
-    SERIAL_ECHOPAIR(" (",   dac_amps(2));
1281
-    SERIAL_ECHOPAIR(") E:", dac_perc(3));
1282
-    SERIAL_ECHOPAIR(" (",   dac_amps(3));
1283
-    SERIAL_ECHOLN(")");
1284
-  }
1285
-
1286
-  void dac_commit_eeprom() {
1287
-    if (!dac_present) return;
1288
-    mcp4728_eepromWrite();
1289
-  }
1290
-
1291
-#endif // DAC_STEPPER_CURRENT
1292
-
1293 1230
 void microstep_init() {
1294 1231
   #if HAS_MICROSTEPS_E1
1295 1232
     pinMode(E1_MS1_PIN, OUTPUT);

+ 87
- 0
Marlin/stepper_dac.cpp View File

@@ -0,0 +1,87 @@
1
+/*
2
+  stepper_dac.cpp - To set stepper current via DAC
3
+
4
+  Part of Marlin
5
+
6
+  Copyright (c) 2016 MarlinFirmware
7
+
8
+  Marlin is free software: you can redistribute it and/or modify
9
+  it under the terms of the GNU General Public License as published by
10
+  the Free Software Foundation, either version 3 of the License, or
11
+  (at your option) any later version.
12
+
13
+  Marlin is distributed in the hope that it will be useful,
14
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
15
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16
+  GNU General Public License for more details.
17
+
18
+  You should have received a copy of the GNU General Public License
19
+  along with Marlin.  If not, see <http://www.gnu.org/licenses/>.
20
+*/
21
+
22
+#include "Marlin.h"
23
+
24
+#if ENABLED(DAC_STEPPER_CURRENT)
25
+
26
+  #include "stepper_dac.h"
27
+
28
+  bool dac_present = false;
29
+  const uint8_t dac_order[NUM_AXIS] = DAC_STEPPER_ORDER;
30
+
31
+  int dac_init() {
32
+    mcp4728_init();
33
+
34
+    if (mcp4728_simpleCommand(RESET)) return -1;
35
+
36
+    dac_present = true;
37
+
38
+    mcp4728_setVref_all(DAC_STEPPER_VREF);
39
+    mcp4728_setGain_all(DAC_STEPPER_GAIN);
40
+
41
+    return 0;
42
+  }
43
+
44
+  void dac_current_percent(uint8_t channel, float val) {
45
+    if (!dac_present) return;
46
+
47
+    NOMORE(val, 100);
48
+
49
+    mcp4728_analogWrite(dac_order[channel], val * DAC_STEPPER_MAX / 100);
50
+    mcp4728_simpleCommand(UPDATE);
51
+  }
52
+
53
+  void dac_current_raw(uint8_t channel, uint16_t val) {
54
+    if (!dac_present) return;
55
+
56
+    NOMORE(val, DAC_STEPPER_MAX);
57
+
58
+    mcp4728_analogWrite(dac_order[channel], val);
59
+    mcp4728_simpleCommand(UPDATE);
60
+  }
61
+
62
+  static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) / DAC_STEPPER_MAX; }
63
+  static float dac_amps(int8_t n) { return ((2.048 * mcp4728_getValue(dac_order[n])) / 4096.0) / (8.0 * DAC_STEPPER_SENSE); }
64
+
65
+  void dac_print_values() {
66
+    if (!dac_present) return;
67
+
68
+    SERIAL_ECHO_START;
69
+    SERIAL_ECHOLNPGM("Stepper current values in % (Amps):");
70
+    SERIAL_ECHO_START;
71
+    SERIAL_ECHOPAIR(" X:",  dac_perc(0));
72
+    SERIAL_ECHOPAIR(" (",   dac_amps(0));
73
+    SERIAL_ECHOPAIR(") Y:", dac_perc(1));
74
+    SERIAL_ECHOPAIR(" (",   dac_amps(1));
75
+    SERIAL_ECHOPAIR(") Z:", dac_perc(2));
76
+    SERIAL_ECHOPAIR(" (",   dac_amps(2));
77
+    SERIAL_ECHOPAIR(") E:", dac_perc(3));
78
+    SERIAL_ECHOPAIR(" (",   dac_amps(3));
79
+    SERIAL_ECHOLN(")");
80
+  }
81
+
82
+  void dac_commit_eeprom() {
83
+    if (!dac_present) return;
84
+    mcp4728_eepromWrite();
85
+  }
86
+
87
+#endif // DAC_STEPPER_CURRENT

+ 33
- 0
Marlin/stepper_dac.h View File

@@ -0,0 +1,33 @@
1
+/*
2
+  stepper_dac.h   - To set stepper current via DAC
3
+
4
+  Part of Marlin
5
+
6
+  Copyright (c) 2016 MarlinFirmware
7
+
8
+  Marlin is free software: you can redistribute it and/or modify
9
+  it under the terms of the GNU General Public License as published by
10
+  the Free Software Foundation, either version 3 of the License, or
11
+  (at your option) any later version.
12
+
13
+  Marlin is distributed in the hope that it will be useful,
14
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
15
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16
+  GNU General Public License for more details.
17
+
18
+  You should have received a copy of the GNU General Public License
19
+  along with Marlin.  If not, see <http://www.gnu.org/licenses/>.
20
+*/
21
+
22
+#ifndef STEPPER_DAC_H
23
+#define STEPPER_DAC_H
24
+
25
+#include "dac_mcp4728.h"
26
+
27
+int dac_init();
28
+void dac_current_percent(uint8_t channel, float val);
29
+void dac_current_raw(uint8_t channel, uint16_t val);
30
+void dac_print_values();
31
+void dac_commit_eeprom();
32
+
33
+#endif // STEPPER_DAC_H

Loading…
Cancel
Save