Browse Source

updated code for use with first prototype of xy table

Thomas Buck 2 years ago
parent
commit
6d5dc579da
15 changed files with 494 additions and 84 deletions
  1. 3
    0
      include/common.h
  2. 15
    7
      include/config.h
  3. 2
    2
      include/config_pins.h
  4. 1
    0
      include/encoder.h
  5. 8
    2
      include/statemachine.h
  6. 2
    0
      include/states.h
  7. 6
    0
      include/steppers.h
  8. 2
    0
      platformio.ini
  9. 7
    0
      src/debounce.cpp
  10. 4
    0
      src/encoder.cpp
  11. 8
    0
      src/lcd.cpp
  12. 52
    31
      src/main.cpp
  13. 31
    7
      src/statemachine.cpp
  14. 125
    13
      src/states.cpp
  15. 228
    22
      src/steppers.cpp

+ 3
- 0
include/common.h View File

@@ -1,6 +1,9 @@
1 1
 #ifndef _COMMON_H_
2 2
 #define _COMMON_H_
3 3
 
4
+#define CLEAR_STORE_INTERRUPTS() uint8_t prev_int_reg = SREG; cli()
5
+#define RESTORE_INTERRUPTS() SREG = prev_int_reg
6
+
4 7
 void async_beep(int time, int freq);
5 8
 void blocking_beep(int time, int freq, int repeat = 0);
6 9
 void common_run(unsigned long t);

+ 15
- 7
include/config.h View File

@@ -72,12 +72,12 @@
72 72
 #define E_MAX_SPEED 50.0 // in percent/s
73 73
 
74 74
 // homing speeds
75
-#define XY_FAST_HOME_SPEED 2.5 // in mm/s
76
-#define XY_SLOW_HOME_SPEED 1.0 // in mm/s
77
-#define Z_FAST_HOME_SPEED 2.0 // in mm/s
75
+#define XY_FAST_HOME_SPEED 25.0 // in mm/s
76
+#define XY_SLOW_HOME_SPEED 5.0 // in mm/s
77
+#define Z_FAST_HOME_SPEED 5.0 // in mm/s
78 78
 #define Z_SLOW_HOME_SPEED 1.0 // in mm/s
79 79
 #define E_FAST_HOME_SPEED 10.0 // in percent/s
80
-#define E_SLOW_HOME_SPEED 5.0 // in percent/s
80
+#define E_SLOW_HOME_SPEED 2.0 // in percent/s
81 81
 
82 82
 // accelerations
83 83
 #define XY_MAX_ACCEL 100.0 // in mm/s^2
@@ -91,12 +91,20 @@
91 91
 #define E_AXIS_MOVEMENT_DIR 1.0
92 92
 
93 93
 // homing back-off
94
-#define XY_BACK_OFF_DISTANCE 10.0 // in mm
95
-#define Z_BACK_OFF_DISTANCE 5.0 // in mm
96
-#define E_BACK_OFF_DISTANCE 2.0 // in mm
94
+#define XY_BACK_OFF_DISTANCE 5.0 // in mm
95
+#define Z_BACK_OFF_DISTANCE 2.0 // in mm
96
+#define E_BACK_OFF_DISTANCE 1.0 // in mm
97 97
 
98 98
 #define XY_HOME_BACK_OFF_TIME (XY_BACK_OFF_DISTANCE / XY_FAST_HOME_SPEED * 1000)
99 99
 #define Z_HOME_BACK_OFF_TIME (Z_BACK_OFF_DISTANCE / Z_FAST_HOME_SPEED * 1000)
100 100
 #define E_HOME_BACK_OFF_TIME (E_BACK_OFF_DISTANCE / E_FAST_HOME_SPEED * 1000)
101 101
 
102
+// epsilon around expected endstop position where we dont abort on hits
103
+#define X_AXIS_EPSILON 20.0 // TODO
104
+#define Y_AXIS_EPSILON 20.0 // TODO
105
+#define Z_AXIS_EPSILON 1.0
106
+#define E_AXIS_EPSILON 1.0
107
+
108
+#define ENCODER_RPM_VALUE_FACTOR 10.0
109
+
102 110
 #endif // _CONFIG_H_

+ 2
- 2
include/config_pins.h View File

@@ -41,12 +41,12 @@
41 41
 
42 42
 #define LED_PIN            13
43 43
 
44
-#define FAN_PIN            9
44
+#define FAN_PIN            8
45 45
 
46 46
 #define PS_ON_PIN          12	//ATX , awake=LOW, SLEEP=High
47 47
 
48 48
 #define HEATER_0_PIN	10  // Extruder Heater
49
-#define HEATER_1_PIN	8
49
+#define HEATER_1_PIN	9
50 50
 
51 51
 #define TEMP_0_PIN		13   // ANALOG NUMBERING
52 52
 #define TEMP_1_PIN		14   // ANALOG NUMBERING

+ 1
- 0
include/encoder.h View File

@@ -5,6 +5,7 @@ void encoder_init(void);
5 5
 void encoder_run(void);
6 6
 
7 7
 int encoder_change(void);
8
+int encoder_rpm(void);
8 9
 int encoder_click(void);
9 10
 int kill_switch(void);
10 11
 

+ 8
- 2
include/statemachine.h View File

@@ -12,11 +12,12 @@ template <typename T, size_t N>
12 12
 void array_insert_at_pos(Array<T, N> *arr, T value, size_t pos);
13 13
 
14 14
 struct StateMachineInput {
15
-    StateMachineInput(int c, int e, int k, int m)
16
-            : click(c), encoder(e), kill(k), motors_done(m) { };
15
+    StateMachineInput(int c, int e, int r, int k, int m)
16
+            : click(c), encoder(e), rpm(r), kill(k), motors_done(m) { };
17 17
 
18 18
     int click;
19 19
     int encoder;
20
+    int rpm;
20 21
     int kill;
21 22
     int motors_done;
22 23
 };
@@ -54,6 +55,8 @@ public:
54 55
     void onEnter(EnterFuncPtr func);
55 56
     void whenIn(InFuncPtr func);
56 57
 
58
+    void updateText(void);
59
+
57 60
     virtual void enterState(void);
58 61
     virtual void inState(StateMachineInput smi);
59 62
 
@@ -151,8 +154,10 @@ public:
151 154
     void setHeading(const char *_heading);
152 155
     void setText(const char *_text);
153 156
 
157
+    typedef void(*EnterFuncPtr)(void);
154 158
     typedef void(*UpdateFuncPtr)(size_t index, T value);
155 159
 
160
+    void onEnter(EnterFuncPtr func);
156 161
     void onLiveUpdate(UpdateFuncPtr func);
157 162
     void onUpdate(UpdateFuncPtr func);
158 163
 
@@ -168,6 +173,7 @@ private:
168 173
     const char *heading;
169 174
     size_t pos;
170 175
     bool editing;
176
+    EnterFuncPtr onEnterFunc;
171 177
     UpdateFuncPtr updateFunc, updateLiveFunc;
172 178
 };
173 179
 

+ 2
- 0
include/states.h View File

@@ -9,4 +9,6 @@ void states_run(struct StateMachineInput smi);
9 9
 State *states_get(void);
10 10
 void states_go_to(State *state);
11 11
 
12
+extern StateText sm_error;
13
+
12 14
 #endif // _STATES_H_

+ 6
- 0
include/steppers.h View File

@@ -5,8 +5,12 @@ void steppers_init(void);
5 5
 bool steppers_run(void);
6 6
 
7 7
 bool steppers_homed(void);
8
+char steppers_homing_axis(void);
8 9
 void steppers_start_homing(void);
9 10
 
11
+float steppers_current_pos(int axis);
12
+
13
+// target pos
10 14
 float steppers_pos_x(void);
11 15
 float steppers_pos_y(void);
12 16
 float steppers_pos_z(void);
@@ -29,4 +33,6 @@ void steppers_set_accel_y(float accel);
29 33
 void steppers_set_accel_z(float accel);
30 34
 void steppers_set_accel_e(float accel);
31 35
 
36
+bool steppers_home_switch(int axis);
37
+
32 38
 #endif // _STEPPERS_H_

+ 2
- 0
platformio.ini View File

@@ -24,6 +24,7 @@ lib_deps =
24 24
     https://github.com/waspinator/AccelStepper
25 25
     https://github.com/mathertel/RotaryEncoder
26 26
     https://github.com/janelia-arduino/Array
27
+    https://github.com/PaulStoffregen/TimerOne
27 28
 
28 29
 [env:ramps_lcd12864]
29 30
 platform = atmelavr
@@ -38,3 +39,4 @@ lib_deps =
38 39
     https://github.com/waspinator/AccelStepper
39 40
     https://github.com/mathertel/RotaryEncoder
40 41
     https://github.com/janelia-arduino/Array
42
+    https://github.com/PaulStoffregen/TimerOne

+ 7
- 0
src/debounce.cpp View File

@@ -19,6 +19,13 @@ int Debouncer::poll() {
19 19
             if (currentState == LOW) {
20 20
                 ret = 1;
21 21
             }
22
+
23
+            /*
24
+            Serial.print("Debounce: ");
25
+            Serial.print(pin);
26
+            Serial.print(" = ");
27
+            Serial.println(state);
28
+            */
22 29
         }
23 30
     }
24 31
 

+ 4
- 0
src/encoder.cpp View File

@@ -63,6 +63,10 @@ int encoder_change(void) {
63 63
     return diff;
64 64
 }
65 65
 
66
+int encoder_rpm(void) {
67
+    return encoder.getRPM();
68
+}
69
+
66 70
 int encoder_click(void) {
67 71
     int r = click_state;
68 72
 

+ 8
- 0
src/lcd.cpp View File

@@ -25,6 +25,8 @@ LiquidCrystal lcd(LCD_PINS_RS, LCD_PINS_ENABLE, LCD_PINS_D4, LCD_PINS_D5, LCD_PI
25 25
 
26 26
 #include <U8g2lib.h>
27 27
 
28
+//#define ENABLE_DYNAMIC_SCROLLING
29
+
28 30
 #define LCD_HEADING_LINES 1
29 31
 #define LCD_HEADING_WIDTH 15
30 32
 #define LCD_TEXT_LINES 5
@@ -111,6 +113,8 @@ void lcd_shorten_multiline(String s[], int n, int w, String *remainder) {
111 113
     last_scroll_time = millis();
112 114
 }
113 115
 
116
+#ifdef ENABLE_DYNAMIC_SCROLLING
117
+
114 118
 void lcd_scroll_multiline(String s[], int n, int w, String &pre, String &post, int &dir) {
115 119
     if (dir == 0) {
116 120
         // switch directions if done scrolling in current direction
@@ -180,6 +184,8 @@ void lcd_scrolling(void) {
180 184
     }
181 185
 }
182 186
 
187
+#endif // ENABLE_DYNAMIC_SCROLLING
188
+
183 189
 int lcd_text_lines(void) {
184 190
     return LCD_TEXT_LINES;
185 191
 }
@@ -221,10 +227,12 @@ void lcd_init(void) {
221 227
 }
222 228
 
223 229
 void lcd_loop(void) {
230
+#ifdef ENABLE_DYNAMIC_SCROLLING
224 231
     if ((millis() - last_scroll_time) >= SCROLL_DELAY) {
225 232
         last_scroll_time = millis();
226 233
         lcd_scrolling();
227 234
     }
235
+#endif // ENABLE_DYNAMIC_SCROLLING
228 236
 
229 237
 #ifdef USE_FULL_GRAPHIC_LCD
230 238
     if (redraw) {

+ 52
- 31
src/main.cpp View File

@@ -1,4 +1,5 @@
1 1
 #include <Arduino.h>
2
+#include <TimerOne.h>
2 3
 
3 4
 #include "config.h"
4 5
 #include "config_pins.h"
@@ -9,32 +10,9 @@
9 10
 #include "steppers.h"
10 11
 #include "states.h"
11 12
 
12
-void setup() {
13
-    pinMode(LED_PIN, OUTPUT);
14
-    digitalWrite(LED_PIN, HIGH);
15
-
16
-    pinMode(BEEPER, OUTPUT);
17
-    blocking_beep(100, 1000);
18
-    
19
-    Serial.begin(115200);
20
-    Serial.println(F("Initializing Fuellfix v2"));
21
-    Serial.print(F("Version: "));
22
-    Serial.println(FIRMWARE_VERSION);
23
-
24
-    Serial.println(F("Init data"));
25
-    data_init();
26
-
27
-    Serial.println(F("Init encoder"));
28
-    encoder_init();
29
-
30
-    Serial.println(F("Init LCD"));
31
-    lcd_init();
32
-
33
-    Serial.println(F("Init stepper motors"));
34
-    steppers_init();
35
-
36
-    // ----------------------------------
13
+static volatile bool still_running = false;
37 14
 
15
+void print_data(void) {
38 16
     Serial.println(F("XY:"));
39 17
 
40 18
     Serial.print(F("\t X: "));
@@ -110,8 +88,42 @@ void setup() {
110 88
     Serial.println(F(" percent/s^2"));
111 89
 
112 90
     Serial.println();
91
+}
113 92
 
114
-    // ----------------------------------
93
+void fast_loop(void) {
94
+    still_running = steppers_run();
95
+    encoder_run();
96
+}
97
+
98
+void setup() {
99
+    pinMode(LED_PIN, OUTPUT);
100
+    digitalWrite(LED_PIN, HIGH);
101
+
102
+    pinMode(BEEPER, OUTPUT);
103
+    blocking_beep(100, 1000);
104
+
105
+    // turn on fan connected to D8 for electronics
106
+    pinMode(FAN_PIN, OUTPUT);
107
+    digitalWrite(FAN_PIN, HIGH);
108
+
109
+    Serial.begin(115200);
110
+    Serial.println(F("Initializing Fuellfix v2"));
111
+    Serial.print(F("Version: "));
112
+    Serial.println(FIRMWARE_VERSION);
113
+
114
+    Serial.println(F("Init data"));
115
+    data_init();
116
+
117
+    Serial.println(F("Init encoder"));
118
+    encoder_init();
119
+
120
+    Serial.println(F("Init LCD"));
121
+    lcd_init();
122
+
123
+    Serial.println(F("Init stepper motors"));
124
+    steppers_init();
125
+
126
+    print_data();
115 127
 
116 128
     Serial.println(F("ready, showing splash screen"));
117 129
     digitalWrite(LED_PIN, LOW);
@@ -125,19 +137,28 @@ void setup() {
125 137
 
126 138
     blocking_beep(100, 2000);
127 139
     Serial.println(F("starting main loop"));
140
+
141
+    Timer1.initialize(1000); // 1000us -> 1kHz
142
+    Timer1.attachInterrupt(fast_loop);
128 143
 }
129 144
 
130 145
 void loop() {
131
-    unsigned long t = millis();
132
-    common_run(t);
133
-    bool still_running = steppers_run();
134
-    encoder_run();
146
+    common_run(millis());
147
+
148
+    CLEAR_STORE_INTERRUPTS();
135 149
 
136 150
     int click = encoder_click();
151
+    int change = encoder_change();
152
+    int rpm = encoder_rpm();
137 153
     int kill = kill_switch();
154
+    bool motors_done = !still_running;
155
+
156
+    RESTORE_INTERRUPTS();
138 157
 
139
-    struct StateMachineInput smi(click, encoder_change(), kill, !still_running);
158
+    struct StateMachineInput smi(click, change, rpm, kill, motors_done);
140 159
     states_run(smi);
141 160
 
142 161
     lcd_loop();
162
+
163
+    delay(10);
143 164
 }

+ 31
- 7
src/statemachine.cpp View File

@@ -45,11 +45,7 @@ void StateText::whenIn(InFuncPtr func) {
45 45
     whenInFunc = func;
46 46
 }
47 47
 
48
-void StateText::enterState(void) {
49
-    if (onEnterFunc != NULL) {
50
-        onEnterFunc();
51
-    }
52
-
48
+void StateText::updateText(void) {
53 49
     lcd_clear();
54 50
     if (heading != NULL) {
55 51
         lcd_set_heading(heading);
@@ -61,6 +57,14 @@ void StateText::enterState(void) {
61 57
     }
62 58
 }
63 59
 
60
+void StateText::enterState(void) {
61
+    if (onEnterFunc != NULL) {
62
+        onEnterFunc();
63
+    }
64
+
65
+    updateText();
66
+}
67
+
64 68
 void StateText::inState(struct StateMachineInput smi) {
65 69
     if (whenInFunc != NULL) {
66 70
         whenInFunc(smi);
@@ -325,7 +329,10 @@ void StateValue<T>::enterState(void) {
325 329
 template <typename T>
326 330
 void StateValue<T>::inState(StateMachineInput smi) {
327 331
     if (smi.encoder != 0) {
328
-        value -= smi.encoder;
332
+        float vf = smi.encoder;
333
+        vf *= 1.0 + ((float)smi.rpm / ENCODER_RPM_VALUE_FACTOR);
334
+        int v = vf;
335
+        value -= v;
329 336
         if (value < min) {
330 337
             value = min;
331 338
         }
@@ -353,6 +360,7 @@ template class StateValue<float>;
353 360
 template <typename T, size_t N>
354 361
 StateValues<T, N>::StateValues(State *_parent) : State(_parent) {
355 362
     heading = NULL;
363
+    onEnterFunc = NULL;
356 364
     updateFunc = NULL;
357 365
     updateLiveFunc = NULL;
358 366
     pos = 0;
@@ -376,6 +384,11 @@ void StateValues<T, N>::setHeading(const char *_heading) {
376 384
 }
377 385
 
378 386
 template <typename T, size_t N>
387
+void StateValues<T, N>::onEnter(EnterFuncPtr func) {
388
+    onEnterFunc = func;
389
+}
390
+
391
+template <typename T, size_t N>
379 392
 void StateValues<T, N>::onUpdate(UpdateFuncPtr func) {
380 393
     updateFunc = func;
381 394
 }
@@ -425,6 +438,9 @@ void StateValues<T, N>::display(void) {
425 438
 template <typename T, size_t N>
426 439
 void StateValues<T, N>::enterState(void) {
427 440
     pos = 0;
441
+    if (onEnterFunc != NULL) {
442
+        onEnterFunc();
443
+    }
428 444
     display();
429 445
 }
430 446
 
@@ -432,16 +448,23 @@ template <typename T, size_t N>
432 448
 void StateValues<T, N>::inState(StateMachineInput smi) {
433 449
     if (editing) {
434 450
         if (smi.encoder != 0) {
435
-            *(values[pos]) -= smi.encoder;
451
+            float vf = smi.encoder;
452
+            vf *= 1.0 + ((float)smi.rpm / ENCODER_RPM_VALUE_FACTOR);
453
+            int v = vf;
454
+            *(values[pos]) -= v;
455
+
436 456
             if (*(values[pos]) < mins[pos]) {
437 457
                 *(values[pos]) = mins[pos];
438 458
             }
459
+
439 460
             if (*(values[pos]) > maxs[pos]) {
440 461
                 *(values[pos]) = maxs[pos];
441 462
             }
463
+
442 464
             if (updateLiveFunc != NULL) {
443 465
                 updateLiveFunc(pos, *(values[pos]));
444 466
             }
467
+
445 468
             display();
446 469
         }
447 470
         if (smi.click) {
@@ -475,6 +498,7 @@ void StateValues<T, N>::inState(StateMachineInput smi) {
475 498
                         updateFunc(i, *(values[i]));
476 499
                     }
477 500
                 }
501
+
478 502
                 if (getChild() != NULL) {
479 503
                     states_go_to(getChild());
480 504
                 } else if (getParent() != NULL) {

+ 125
- 13
src/states.cpp View File

@@ -9,6 +9,10 @@
9 9
 #include "statemachine.h"
10 10
 #include "states.h"
11 11
 
12
+//#define DISABLE_HOMING_STATE
13
+#define ENABLE_MOVEMENT_TEST_STATE
14
+#define ENABLE_INPUT_TEST_STATE
15
+
12 16
 // --------------------------------------
13 17
 
14 18
 StateText sm_ask_homing = StateText();
@@ -52,6 +56,18 @@ StateValue<float> sm_conf_accel_xy = StateValue<float>(&sm_config, data_options(
52 56
 StateValue<float> sm_conf_accel_z = StateValue<float>(&sm_config, data_options()->accel_z, 1.0, Z_MAX_ACCEL);
53 57
 StateValue<float> sm_conf_accel_e = StateValue<float>(&sm_config, data_options()->accel_e, 1.0, E_MAX_ACCEL);
54 58
 
59
+StateText sm_error = StateText(NULL);
60
+
61
+#ifdef ENABLE_MOVEMENT_TEST_STATE
62
+StateMenu sm_movement_test = StateMenu(&sm_menu);
63
+StateText sm_move_x_test = StateText(&sm_movement_test);
64
+StateText sm_move_y_test = StateText(&sm_movement_test);
65
+#endif // ENABLE_MOVEMENT_TEST_STATE
66
+
67
+#ifdef ENABLE_INPUT_TEST_STATE
68
+StateText sm_input_test = StateText(&sm_menu);
69
+#endif // ENABLE_INPUT_TEST_STATE
70
+
55 71
 // --------------------------------------
56 72
 
57 73
 State *current_state = NULL;
@@ -73,18 +89,33 @@ void states_init(void) {
73 89
     sm_do_homing.setHeading("Homing...");
74 90
 
75 91
     sm_do_homing.onEnter([]() {
76
-        //steppers_start_homing();
92
+#ifndef DISABLE_HOMING_STATE
93
+        steppers_start_homing();
94
+#endif // DISABLE_HOMING_STATE
77 95
     });
78 96
 
79 97
     sm_do_homing.whenIn([](StateMachineInput smi) {
80
-        //if (smi.motors_done) {
81
-        //    if (steppers_homed()) {
98
+#ifndef DISABLE_HOMING_STATE
99
+        if (smi.motors_done) {
100
+            if (steppers_homed()) {
101
+#endif // DISABLE_HOMING_STATE
102
+
82 103
                 async_beep(HOMING_BEEP_TIME, HOMING_BEEP_FREQ);
83 104
                 states_go_to(&sm_menu);
84
-        //    }
85
-        //}
86 105
 
87
-        // TODO update text with current axis
106
+#ifndef DISABLE_HOMING_STATE
107
+            }
108
+        }
109
+#endif // DISABLE_HOMING_STATE
110
+
111
+        static char last_axis = ' ';
112
+        char axis = steppers_homing_axis();
113
+        if ((axis != ' ') && (axis != last_axis)) {
114
+            last_axis = axis;
115
+            strbuf = String(F("Currently homing ")) + axis + String(F(" axis"));
116
+            sm_do_homing.setText(strbuf.c_str());
117
+            sm_do_homing.updateText();
118
+        }
88 119
     });
89 120
 
90 121
     // ----------------------------------
@@ -132,6 +163,7 @@ void states_init(void) {
132 163
 
133 164
     sm_auto.setTitle("Filling Menu");
134 165
 
166
+    // TODO
135 167
     sm_presets.setTitle("Use Preset");
136 168
     sm_presets.setPrefix("Preset ");
137 169
     sm_presets.dataCount([]() {
@@ -144,16 +176,27 @@ void states_init(void) {
144 176
 
145 177
     sm_wiz_move.setTitle("Initial Movement");
146 178
     sm_wiz_move.onEnter([]() {
147
-        //steppers_start_homing();
179
+        steppers_start_homing();
148 180
     });
149 181
     sm_wiz_move.whenIn([](StateMachineInput smi) {
150
-        //if (smi.motors_done) {
151
-        //    if (steppers_homed()) {
182
+#ifndef DISABLE_HOMING_STATE
183
+        if (smi.motors_done) {
184
+            if (steppers_homed()) {
185
+#endif // DISABLE_HOMING_STATE
152 186
                 states_go_to(&sm_wiz_count);
153
-        //    }
154
-        //}
155
-
156
-        // TODO update text with current axis
187
+#ifndef DISABLE_HOMING_STATE
188
+            }
189
+        }
190
+#endif // DISABLE_HOMING_STATE
191
+
192
+        static char last_axis = ' ';
193
+        char axis = steppers_homing_axis();
194
+        if ((axis != ' ') && (axis != last_axis)) {
195
+            last_axis = axis;
196
+            strbuf = String(F("Currently homing ")) + axis + String(F(" axis"));
197
+            sm_wiz_move.setText(strbuf.c_str());
198
+            sm_wiz_move.updateText();
199
+        }
157 200
     });
158 201
 
159 202
     sm_wiz_count.setTitle("Container Count");
@@ -163,6 +206,10 @@ void states_init(void) {
163 206
     sm_wiz_first_pos.setTitle("First Container Position");
164 207
     sm_wiz_first_pos.setData(0, "X: ", &wizard_first_x, X_AXIS_MIN, X_AXIS_MAX);
165 208
     sm_wiz_first_pos.setData(1, "Y: ", &wizard_first_y, Y_AXIS_MIN, Y_AXIS_MAX);
209
+    sm_wiz_first_pos.onEnter([]() {
210
+        wizard_first_x = steppers_current_pos(0);
211
+        wizard_first_y = steppers_current_pos(1);
212
+    });
166 213
     sm_wiz_first_pos.onLiveUpdate([](size_t i, float v) {
167 214
         if (i == 0) {
168 215
             steppers_move_x(v);
@@ -181,6 +228,10 @@ void states_init(void) {
181 228
     sm_wiz_last_pos.setTitle("Last Container Position");
182 229
     sm_wiz_last_pos.setData(0, "X: ", &wizard_last_x, X_AXIS_MIN, X_AXIS_MAX);
183 230
     sm_wiz_last_pos.setData(1, "Y: ", &wizard_last_y, Y_AXIS_MIN, Y_AXIS_MAX);
231
+    sm_wiz_last_pos.onEnter([]() {
232
+        wizard_last_x = steppers_current_pos(0);
233
+        wizard_last_y = steppers_current_pos(1);
234
+    });
184 235
     sm_wiz_last_pos.onLiveUpdate([](size_t i, float v) {
185 236
         if (i == 0) {
186 237
             steppers_move_x(v);
@@ -215,12 +266,14 @@ void states_init(void) {
215 266
         }
216 267
     });
217 268
 
269
+    // TODO
218 270
     sm_mod_preset.setTitle("Modify Preset");
219 271
     sm_mod_preset.setPrefix("Preset ");
220 272
     sm_mod_preset.dataCount([]() {
221 273
         return (int)data_preset_count();
222 274
     });
223 275
 
276
+    // TODO
224 277
     sm_del_preset.setTitle("Delete Preset");
225 278
     sm_del_preset.setPrefix("Preset ");
226 279
     sm_del_preset.dataCount([]() {
@@ -290,6 +343,65 @@ void states_init(void) {
290 343
         steppers_set_accel_e(v);
291 344
     });
292 345
 
346
+    sm_error.setChild(&sm_ask_homing);
347
+    sm_error.setTitle("Axis Error");
348
+    sm_error.setHeading("Axis Error");
349
+    sm_error.setText("Endstop has been hit!");
350
+
351
+#ifdef ENABLE_MOVEMENT_TEST_STATE
352
+    sm_movement_test.setTitle("Movement Test");
353
+
354
+    sm_move_x_test.setTitle("X Axis");
355
+    sm_move_x_test.setHeading("X Move Test");
356
+    sm_move_x_test.whenIn([](StateMachineInput smi) {
357
+        static bool s = false;
358
+        if (smi.motors_done) {
359
+            s = !s;
360
+            steppers_move_x(s ? (X_AXIS_MAX - 10.0) : (X_AXIS_MIN + 10.0));
361
+            if (smi.click) {
362
+                states_go_to(&sm_movement_test);
363
+            }
364
+        }
365
+    });
366
+
367
+    sm_move_y_test.setTitle("Y Axis");
368
+    sm_move_y_test.setHeading("Y Move Test");
369
+    sm_move_y_test.whenIn([](StateMachineInput smi) {
370
+        static bool s = false;
371
+        if (smi.motors_done) {
372
+            s = !s;
373
+            steppers_move_y(s ? (Y_AXIS_MAX - 10.0) : (Y_AXIS_MIN + 10.0));
374
+            if (smi.click) {
375
+                states_go_to(&sm_movement_test);
376
+            }
377
+        }
378
+    });
379
+#endif // ENABLE_MOVEMENT_TEST_STATE
380
+
381
+#ifdef ENABLE_INPUT_TEST_STATE
382
+    sm_input_test.setTitle("Input Test");
383
+    sm_input_test.setHeading("Input Test");
384
+
385
+    sm_input_test.whenIn([](StateMachineInput smi) {
386
+        String s = "Endstops: ";
387
+        for (int i = 0; i < 4; i++) {
388
+            if (steppers_home_switch(i)) {
389
+                s += '1';
390
+            } else {
391
+                s += '0';
392
+            }
393
+            if (i < 3) {
394
+                s += ' ';
395
+            }
396
+        }
397
+        if (s != strbuf) {
398
+            strbuf = s;
399
+            sm_input_test.setText(strbuf.c_str());
400
+            sm_input_test.updateText();
401
+        }
402
+    });
403
+#endif // ENABLE_INPUT_TEST_STATE
404
+
293 405
     // ----------------------------------
294 406
 
295 407
     states_go_to(&sm_ask_homing);

+ 228
- 22
src/steppers.cpp View File

@@ -3,8 +3,10 @@
3 3
 
4 4
 #include "config.h"
5 5
 #include "config_pins.h"
6
+#include "common.h"
6 7
 #include "lcd.h"
7 8
 #include "data.h"
9
+#include "states.h"
8 10
 #include "steppers.h"
9 11
 
10 12
 static AccelStepper stepper_x(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN, 0, 0, false);
@@ -30,8 +32,8 @@ enum stepper_states {
30 32
     step_homed
31 33
 };
32 34
 
33
-static stepper_states state = step_disabled;
34
-static unsigned long steppers_home_move_start_time = 0;
35
+static volatile stepper_states state = step_disabled;
36
+static volatile unsigned long steppers_home_move_start_time = 0;
35 37
 
36 38
 void steppers_init(void) {
37 39
     pinMode(X_ENDSTOP_PIN, INPUT_PULLUP);
@@ -53,24 +55,45 @@ void steppers_init(void) {
53 55
     pinMode(E0_DIR_PIN, OUTPUT);
54 56
 
55 57
     stepper_x.setEnablePin(X_ENABLE_PIN);
56
-    stepper_x.setMaxSpeed(XY_MAX_SPEED * XY_STEPS_PER_MM);
58
+    stepper_x.setPinsInverted(false, false, true);
57 59
     steppers_set_speed_x(data_options()->speed_x);
58 60
     steppers_set_accel_x(data_options()->accel_x);
59 61
 
60 62
     stepper_y.setEnablePin(Y_ENABLE_PIN);
63
+    stepper_y.setPinsInverted(false, false, true);
61 64
     steppers_set_speed_y(data_options()->speed_y);
62 65
     steppers_set_accel_y(data_options()->accel_y);
63 66
 
64 67
     stepper_z.setEnablePin(Z_ENABLE_PIN);
68
+    stepper_z.setPinsInverted(false, false, true);
65 69
     steppers_set_speed_z(data_options()->speed_z);
66 70
     steppers_set_accel_z(data_options()->accel_z);
67 71
 
68 72
     stepper_e.setEnablePin(E0_ENABLE_PIN);
73
+    stepper_e.setPinsInverted(false, false, true);
69 74
     steppers_set_speed_e(data_options()->speed_e);
70 75
     steppers_set_accel_e(data_options()->accel_e);
71 76
 }
72 77
 
73 78
 static void steppers_initiate_home(int axis, int phase) {
79
+    Serial.print(F("steppers_initiate_home("));
80
+    if (axis == 0) {
81
+        Serial.print('X');
82
+    } else if (axis == 1) {
83
+        Serial.print('Y');
84
+    } else if (axis == 2) {
85
+        Serial.print('Z');
86
+    } else if (axis == 3) {
87
+        Serial.print('E');
88
+    } else {
89
+        Serial.print(axis);
90
+    }
91
+    Serial.print(F(", "));
92
+    Serial.print(phase);
93
+    Serial.println(F(")"));
94
+
95
+    CLEAR_STORE_INTERRUPTS();
96
+
74 97
     steppers_home_move_start_time = millis();
75 98
 
76 99
     if (axis == 0) {
@@ -124,21 +147,52 @@ static void steppers_initiate_home(int axis, int phase) {
124 147
     } else {
125 148
         Serial.println(F("home_init error: invalid axis"));
126 149
     }
150
+
151
+    RESTORE_INTERRUPTS();
127 152
 }
128 153
 
129
-static bool steppers_home_switch(int axis) {
154
+bool steppers_home_switch(int axis) {
155
+    bool r = true;
156
+    CLEAR_STORE_INTERRUPTS();
157
+
130 158
     if (axis == 0) {
131
-        return digitalRead(X_ENDSTOP_PIN) == LOW;
159
+        r = digitalRead(X_ENDSTOP_PIN) == LOW;
132 160
     } else if (axis == 1) {
133
-        return digitalRead(Y_ENDSTOP_PIN) == LOW;
161
+        r = digitalRead(Y_ENDSTOP_PIN) == LOW;
134 162
     } else if (axis == 2) {
135
-        return digitalRead(Z_ENDSTOP_PIN) == LOW;
163
+        r = digitalRead(Z_ENDSTOP_PIN) == LOW;
136 164
     } else if (axis == 3) {
137
-        return digitalRead(E_ENDSTOP_PIN) == LOW;
165
+        r = digitalRead(E_ENDSTOP_PIN) == LOW;
138 166
     } else {
139 167
         Serial.println(F("home_switch error: invalid axis"));
140 168
     }
141
-    return true;
169
+
170
+    RESTORE_INTERRUPTS();
171
+    return r;
172
+}
173
+
174
+float steppers_current_pos(int axis) {
175
+    float r = 0.0;
176
+    CLEAR_STORE_INTERRUPTS();
177
+
178
+    if (axis == 0) {
179
+        float v = stepper_x.currentPosition();
180
+        r = v / XY_STEPS_PER_MM / X_AXIS_MOVEMENT_DIR;
181
+    } else if (axis == 1) {
182
+        float v = stepper_y.currentPosition();
183
+        r = v / XY_STEPS_PER_MM / Y_AXIS_MOVEMENT_DIR;
184
+    } else if (axis == 2) {
185
+        float v = stepper_z.currentPosition();
186
+        r = v / Z_STEPS_PER_MM / Z_AXIS_MOVEMENT_DIR;
187
+    } else if (axis == 3) {
188
+        float v = stepper_e.currentPosition();
189
+        r = E_PERCENT_TO_MM(v / E_STEPS_PER_PERCENT / E_AXIS_MOVEMENT_DIR);
190
+    } else {
191
+        Serial.println(F("steppers_pos error: invalid axis"));
192
+    }
193
+
194
+    RESTORE_INTERRUPTS();
195
+    return r;
142 196
 }
143 197
 
144 198
 bool steppers_run(void) {
@@ -158,6 +212,13 @@ bool steppers_run(void) {
158 212
     } else if (state == step_homing_x_slow) {
159 213
         if (steppers_home_switch(0)) {
160 214
             stepper_x.setSpeed(0);
215
+
216
+#if (X_MIN_PIN == -1)
217
+            stepper_x.setCurrentPosition(X_AXIS_MAX * XY_STEPS_PER_MM * X_AXIS_MOVEMENT_DIR);
218
+#else
219
+            stepper_x.setCurrentPosition(X_AXIS_MIN * XY_STEPS_PER_MM * X_AXIS_MOVEMENT_DIR);
220
+#endif
221
+
161 222
             steppers_initiate_home(1, 0);
162 223
         } else {
163 224
             stepper_x.runSpeed();
@@ -178,7 +239,17 @@ bool steppers_run(void) {
178 239
     } else if (state == step_homing_y_slow) {
179 240
         if (steppers_home_switch(1)) {
180 241
             stepper_y.setSpeed(0);
181
-            steppers_initiate_home(3, 0);
242
+
243
+#if (Y_MIN_PIN == -1)
244
+            stepper_y.setCurrentPosition(Y_AXIS_MAX * XY_STEPS_PER_MM * Y_AXIS_MOVEMENT_DIR);
245
+#else
246
+            stepper_y.setCurrentPosition(Y_AXIS_MIN * XY_STEPS_PER_MM * Y_AXIS_MOVEMENT_DIR);
247
+#endif
248
+
249
+            //steppers_initiate_home(3, 0);
250
+            // TODO
251
+            state = step_homed;
252
+            return false;
182 253
         } else {
183 254
             stepper_y.runSpeed();
184 255
         }
@@ -198,6 +269,13 @@ bool steppers_run(void) {
198 269
     } else if (state == step_homing_z_slow) {
199 270
         if (steppers_home_switch(2)) {
200 271
             stepper_z.setSpeed(0);
272
+
273
+#if (Z_MIN_PIN == -1)
274
+            stepper_z.setCurrentPosition(Z_AXIS_MAX * Z_STEPS_PER_MM * Z_AXIS_MOVEMENT_DIR);
275
+#else
276
+            stepper_z.setCurrentPosition(Z_AXIS_MIN * Z_STEPS_PER_MM * Z_AXIS_MOVEMENT_DIR);
277
+#endif
278
+
201 279
             steppers_initiate_home(0, 0);
202 280
         } else {
203 281
             stepper_z.runSpeed();
@@ -218,24 +296,80 @@ bool steppers_run(void) {
218 296
     } else if (state == step_homing_e_slow) {
219 297
         if (steppers_home_switch(3)) {
220 298
             stepper_e.setSpeed(0);
299
+
300
+#if (E_MIN_PIN == -1)
301
+            stepper_e.setCurrentPosition(E_MM_TO_PERCENT(E_AXIS_MAX) * E_STEPS_PER_PERCENT * E_AXIS_MOVEMENT_DIR);
302
+#else
303
+            stepper_e.setCurrentPosition(E_MM_TO_PERCENT(E_AXIS_MIN) * E_STEPS_PER_PERCENT * E_AXIS_MOVEMENT_DIR);
304
+#endif
305
+
221 306
             state = step_homed;
222 307
             return false;
223 308
         } else {
224 309
             stepper_e.runSpeed();
225 310
         }
226 311
     } else if (state != step_disabled) {
227
-        for (int i = 0; i < 4; i++) {
228
-            if (steppers_home_switch(i)) {
229
-                Serial.print(F("ERROR: endstop hit on "));
230
-                Serial.println(i);
231
-
232
-                // TODO proper error handling?
233
-                lcd_clear();
234
-                lcd_set_heading("ERROR");
235
-                lcd_set_text("Endstop hit. Aborting!");
236
-                while (1) { }
312
+        if (state == step_homed) {
313
+            for (int i = 0; i < 4; i++) {
314
+                Serial.print(i);
315
+                Serial.print(": ");
316
+                Serial.println(steppers_current_pos(i));
317
+
318
+                float compare = 0.0, epsilon = 0.0;
319
+                if (i == 0) {
320
+                    epsilon = X_AXIS_EPSILON;
321
+#if (X_MIN_PIN == -1)
322
+                    compare = X_AXIS_MAX;
323
+#else
324
+                    compare = X_AXIS_MIN;
325
+#endif
326
+                } else if (i == 1) {
327
+                    epsilon = Y_AXIS_EPSILON;
328
+#if (Y_MIN_PIN == -1)
329
+                    compare = Y_AXIS_MAX;
330
+#else
331
+                    compare = Y_AXIS_MIN;
332
+#endif
333
+                } else if (i == 2) {
334
+                    epsilon = Z_AXIS_EPSILON;
335
+#if (Z_MIN_PIN == -1)
336
+                    compare = Z_AXIS_MAX;
337
+#else
338
+                    compare = Z_AXIS_MIN;
339
+#endif
340
+                } else if (i == 3) {
341
+                    epsilon = E_AXIS_EPSILON;
342
+#if (E_MIN_PIN == -1)
343
+                    compare = E_AXIS_MAX;
344
+#else
345
+                    compare = E_AXIS_MIN;
346
+#endif
347
+                }
348
+
349
+                if (fabs(steppers_current_pos(i) - compare) > epsilon) {
350
+                    if (steppers_home_switch(i)) {
351
+                        Serial.print(F("Endstop hit at "));
352
+                        Serial.println(steppers_current_pos(i));
353
+
354
+                        steppers_kill();
355
+
356
+                        if (i == 0) {
357
+                            sm_error.setText("Enstop hit on X axis");
358
+                        } else if (i == 1) {
359
+                            sm_error.setText("Enstop hit on Y axis");
360
+                        } else if (i == 2) {
361
+                            sm_error.setText("Enstop hit on Z axis");
362
+                        } else if (i == 3) {
363
+                            sm_error.setText("Enstop hit on E axis");
364
+                        } else {
365
+                            sm_error.setText("Enstop hit on unknown axis");
366
+                        }
367
+                        states_go_to(&sm_error);
368
+                    }
369
+                }
237 370
             }
238 371
         }
372
+
239 373
         boolean x = stepper_x.run();
240 374
         boolean y = stepper_y.run();
241 375
         boolean z = stepper_z.run();
@@ -248,14 +382,48 @@ bool steppers_run(void) {
248 382
 }
249 383
 
250 384
 bool steppers_homed(void) {
251
-    return (state == step_homed);
385
+    CLEAR_STORE_INTERRUPTS();
386
+    bool r = (state == step_homed);
387
+    RESTORE_INTERRUPTS();
388
+    return r;
389
+}
390
+
391
+char steppers_homing_axis(void) {
392
+    CLEAR_STORE_INTERRUPTS();
393
+
394
+    char r = ' ';
395
+    if ((state == step_homing_x_fast) || (state == step_homing_x_back) || (state == step_homing_x_slow)) {
396
+        r = 'X';
397
+    } else if ((state == step_homing_y_fast) || (state == step_homing_y_back) || (state == step_homing_y_slow)) {
398
+        r = 'Y';
399
+    } else if ((state == step_homing_z_fast) || (state == step_homing_z_back) || (state == step_homing_z_slow)) {
400
+        r = 'Z';
401
+    } else if ((state == step_homing_e_fast) || (state == step_homing_e_back) || (state == step_homing_e_slow)) {
402
+        r = 'E';
403
+    }
404
+
405
+    RESTORE_INTERRUPTS();
406
+    return r;
252 407
 }
253 408
 
254 409
 void steppers_start_homing(void) {
255
-    steppers_initiate_home(2, 0);
410
+    CLEAR_STORE_INTERRUPTS();
411
+
412
+    stepper_x.enableOutputs();
413
+    stepper_y.enableOutputs();
414
+    stepper_z.enableOutputs();
415
+    stepper_e.enableOutputs();
416
+
417
+    //steppers_initiate_home(2, 0);
418
+    // TODO
419
+    steppers_initiate_home(0, 0);
420
+
421
+    RESTORE_INTERRUPTS();
256 422
 }
257 423
 
258 424
 static int steppers_move_axis(AccelStepper &axis, long pos) {
425
+    CLEAR_STORE_INTERRUPTS();
426
+
259 427
     if (state == step_disabled) {
260 428
         Serial.println(F("Enabling stepper drivers"));
261 429
 
@@ -271,6 +439,8 @@ static int steppers_move_axis(AccelStepper &axis, long pos) {
271 439
     }
272 440
 
273 441
     axis.moveTo(pos);
442
+
443
+    RESTORE_INTERRUPTS();
274 444
     return 0;
275 445
 }
276 446
 
@@ -357,26 +527,36 @@ int steppers_move_e(float pos) {
357 527
 }
358 528
 
359 529
 float steppers_pos_x(void) {
530
+    CLEAR_STORE_INTERRUPTS();
360 531
     float v = stepper_x.targetPosition();
532
+    RESTORE_INTERRUPTS();
361 533
     return v / XY_STEPS_PER_MM / X_AXIS_MOVEMENT_DIR;
362 534
 }
363 535
 
364 536
 float steppers_pos_y(void) {
537
+    CLEAR_STORE_INTERRUPTS();
365 538
     float v = stepper_y.targetPosition();
539
+    RESTORE_INTERRUPTS();
366 540
     return v / XY_STEPS_PER_MM / Y_AXIS_MOVEMENT_DIR;
367 541
 }
368 542
 
369 543
 float steppers_pos_z(void) {
544
+    CLEAR_STORE_INTERRUPTS();
370 545
     float v = stepper_z.targetPosition();
546
+    RESTORE_INTERRUPTS();
371 547
     return v / Z_STEPS_PER_MM / Z_AXIS_MOVEMENT_DIR;
372 548
 }
373 549
 
374 550
 float steppers_pos_e(void) {
551
+    CLEAR_STORE_INTERRUPTS();
375 552
     float v = stepper_e.targetPosition();
553
+    RESTORE_INTERRUPTS();
376 554
     return E_PERCENT_TO_MM(v / E_STEPS_PER_PERCENT / E_AXIS_MOVEMENT_DIR);
377 555
 }
378 556
 
379 557
 void steppers_kill(void) {
558
+    CLEAR_STORE_INTERRUPTS();
559
+
380 560
     stepper_x.setCurrentPosition(0);
381 561
     stepper_y.setCurrentPosition(0);
382 562
     stepper_z.setCurrentPosition(0);
@@ -388,6 +568,8 @@ void steppers_kill(void) {
388 568
     stepper_e.disableOutputs();
389 569
 
390 570
     state = step_not_homed;
571
+
572
+    RESTORE_INTERRUPTS();
391 573
 }
392 574
 
393 575
 void steppers_set_speed_x(float speed) {
@@ -397,7 +579,10 @@ void steppers_set_speed_x(float speed) {
397 579
     if (speed > XY_MAX_SPEED) {
398 580
         speed = XY_MAX_SPEED;
399 581
     }
582
+
583
+    CLEAR_STORE_INTERRUPTS();
400 584
     stepper_x.setMaxSpeed(speed * XY_STEPS_PER_MM);
585
+    RESTORE_INTERRUPTS();
401 586
 }
402 587
 
403 588
 void steppers_set_speed_y(float speed) {
@@ -407,7 +592,10 @@ void steppers_set_speed_y(float speed) {
407 592
     if (speed > XY_MAX_SPEED) {
408 593
         speed = XY_MAX_SPEED;
409 594
     }
595
+
596
+    CLEAR_STORE_INTERRUPTS();
410 597
     stepper_y.setMaxSpeed(speed * XY_STEPS_PER_MM);
598
+    RESTORE_INTERRUPTS();
411 599
 }
412 600
 
413 601
 void steppers_set_speed_z(float speed) {
@@ -417,7 +605,10 @@ void steppers_set_speed_z(float speed) {
417 605
     if (speed > Z_MAX_SPEED) {
418 606
         speed = Z_MAX_SPEED;
419 607
     }
608
+
609
+    CLEAR_STORE_INTERRUPTS();
420 610
     stepper_z.setMaxSpeed(speed * Z_STEPS_PER_MM);
611
+    RESTORE_INTERRUPTS();
421 612
 }
422 613
 
423 614
 void steppers_set_speed_e(float speed) {
@@ -427,7 +618,10 @@ void steppers_set_speed_e(float speed) {
427 618
     if (speed > E_MAX_SPEED) {
428 619
         speed = E_MAX_SPEED;
429 620
     }
621
+
622
+    CLEAR_STORE_INTERRUPTS();
430 623
     stepper_e.setMaxSpeed(speed * E_STEPS_PER_PERCENT);
624
+    RESTORE_INTERRUPTS();
431 625
 }
432 626
 
433 627
 void steppers_set_accel_x(float accel) {
@@ -437,7 +631,10 @@ void steppers_set_accel_x(float accel) {
437 631
     if (accel > XY_MAX_ACCEL) {
438 632
         accel = XY_MAX_ACCEL;
439 633
     }
634
+
635
+    CLEAR_STORE_INTERRUPTS();
440 636
     stepper_x.setAcceleration(accel * XY_STEPS_PER_MM);
637
+    RESTORE_INTERRUPTS();
441 638
 }
442 639
 
443 640
 void steppers_set_accel_y(float accel) {
@@ -447,7 +644,10 @@ void steppers_set_accel_y(float accel) {
447 644
     if (accel > XY_MAX_ACCEL) {
448 645
         accel = XY_MAX_ACCEL;
449 646
     }
647
+
648
+    CLEAR_STORE_INTERRUPTS();
450 649
     stepper_y.setAcceleration(accel * XY_STEPS_PER_MM);
650
+    RESTORE_INTERRUPTS();
451 651
 }
452 652
 
453 653
 void steppers_set_accel_z(float accel) {
@@ -457,7 +657,10 @@ void steppers_set_accel_z(float accel) {
457 657
     if (accel > Z_MAX_ACCEL) {
458 658
         accel = Z_MAX_ACCEL;
459 659
     }
660
+
661
+    CLEAR_STORE_INTERRUPTS();
460 662
     stepper_z.setAcceleration(accel * Z_STEPS_PER_MM);
663
+    RESTORE_INTERRUPTS();
461 664
 }
462 665
 
463 666
 void steppers_set_accel_e(float accel) {
@@ -467,5 +670,8 @@ void steppers_set_accel_e(float accel) {
467 670
     if (accel > E_MAX_ACCEL) {
468 671
         accel = E_MAX_ACCEL;
469 672
     }
673
+
674
+    CLEAR_STORE_INTERRUPTS();
470 675
     stepper_e.setAcceleration(accel * E_STEPS_PER_PERCENT);
676
+    RESTORE_INTERRUPTS();
471 677
 }

Loading…
Cancel
Save