stepper.cpp

Thu, 07 Jul 2016 12:23:34 +0200

author
mbayer
date
Thu, 07 Jul 2016 12:23:34 +0200
changeset 2
b373b0288715
parent 1
b584642d4f58
permissions
-rw-r--r--

added missing sanguino files

0
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
1 /*
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
2 stepper.c - stepper motor driver: executes motion plans using stepper motors
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
3 Part of Grbl
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
4
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
5 Copyright (c) 2009-2011 Simen Svale Skogsrud
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
6
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
7 Grbl is free software: you can redistribute it and/or modify
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
8 it under the terms of the GNU General Public License as published by
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
9 the Free Software Foundation, either version 3 of the License, or
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
10 (at your option) any later version.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
11
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
12 Grbl is distributed in the hope that it will be useful,
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
15 GNU General Public License for more details.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
16
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
17 You should have received a copy of the GNU General Public License
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
18 along with Grbl. If not, see <http://www.gnu.org/licenses/>.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
19 */
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
20
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
21 /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
22 and Philipp Tiefenbacher. */
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
23
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
24 #include "Marlin.h"
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
25 #include "stepper.h"
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
26 #include "planner.h"
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
27 #include "temperature.h"
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
28 #include "ultralcd.h"
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
29 #include "language.h"
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
30 #include "led.h"
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
31 #include "speed_lookuptable.h"
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
32
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
33
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
34
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
35 //===========================================================================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
36 //=============================public variables ============================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
37 //===========================================================================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
38 block_t *current_block; // A pointer to the block currently being traced
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
39 volatile bool endstop_z_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
40 bool old_z_min_endstop=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
41
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
42
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
43 //===========================================================================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
44 //=============================private variables ============================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
45 //===========================================================================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
46 //static makes it inpossible to be called from outside of this file by extern.!
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
47
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
48 // Variables used by The Stepper Driver Interrupt
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
49 static unsigned char out_bits; // The next stepping-bits to be output
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
50 static long counter_x, // Counter variables for the bresenham line tracer
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
51 counter_y,
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
52 counter_z,
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
53 counter_e;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
54 volatile static unsigned long step_events_completed; // The number of step events executed in the current block
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
55 #ifdef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
56 static long advance_rate, advance, final_advance = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
57 static long old_advance = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
58 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
59 static long e_steps[3];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
60 static long acceleration_time, deceleration_time;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
61 //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
62 static unsigned short acc_step_rate; // needed for deccelaration start point
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
63 static char step_loops;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
64 static unsigned short OCR1A_nominal;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
65
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
66 volatile long endstops_trigsteps[3]={0,0,0};
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
67 volatile long endstops_stepsTotal,endstops_stepsDone;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
68 static volatile bool endstop_x_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
69 static volatile bool endstop_y_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
70
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
71 static bool old_x_min_endstop=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
72 static bool old_x_max_endstop=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
73 static bool old_y_min_endstop=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
74 static bool old_y_max_endstop=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
75 static bool old_z_max_endstop=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
76
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
77 static bool check_endstops = true;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
78
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
79 volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
80 volatile char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
81
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
82 //===========================================================================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
83 //=============================functions ============================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
84 //===========================================================================
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
85
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
86 #define CHECK_ENDSTOPS if(check_endstops)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
87
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
88 // intRes = intIn1 * intIn2 >> 16
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
89 // uses:
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
90 // r26 to store 0
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
91 // r27 to store the byte 1 of the 24 bit result
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
92 #define MultiU16X8toH16(intRes, charIn1, intIn2) \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
93 asm volatile ( \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
94 "clr r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
95 "mul %A1, %B2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
96 "movw %A0, r0 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
97 "mul %A1, %A2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
98 "add %A0, r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
99 "adc %B0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
100 "lsr r0 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
101 "adc %A0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
102 "adc %B0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
103 "clr r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
104 : \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
105 "=&r" (intRes) \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
106 : \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
107 "d" (charIn1), \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
108 "d" (intIn2) \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
109 : \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
110 "r26" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
111 )
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
112
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
113 // intRes = longIn1 * longIn2 >> 24
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
114 // uses:
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
115 // r26 to store 0
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
116 // r27 to store the byte 1 of the 48bit result
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
117 #define MultiU24X24toH16(intRes, longIn1, longIn2) \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
118 asm volatile ( \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
119 "clr r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
120 "mul %A1, %B2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
121 "mov r27, r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
122 "mul %B1, %C2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
123 "movw %A0, r0 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
124 "mul %C1, %C2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
125 "add %B0, r0 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
126 "mul %C1, %B2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
127 "add %A0, r0 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
128 "adc %B0, r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
129 "mul %A1, %C2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
130 "add r27, r0 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
131 "adc %A0, r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
132 "adc %B0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
133 "mul %B1, %B2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
134 "add r27, r0 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
135 "adc %A0, r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
136 "adc %B0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
137 "mul %C1, %A2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
138 "add r27, r0 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
139 "adc %A0, r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
140 "adc %B0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
141 "mul %B1, %A2 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
142 "add r27, r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
143 "adc %A0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
144 "adc %B0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
145 "lsr r27 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
146 "adc %A0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
147 "adc %B0, r26 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
148 "clr r1 \n\t" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
149 : \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
150 "=&r" (intRes) \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
151 : \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
152 "d" (longIn1), \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
153 "d" (longIn2) \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
154 : \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
155 "r26" , "r27" \
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
156 )
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
157
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
158 // Some useful constants
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
159
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
160 #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
161 #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
162
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
163
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
164 void checkHitEndstops()
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
165 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
166 if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
167 SERIAL_ECHO_START;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
168 SERIAL_ECHOPGM(MSG_ENDSTOPS_HIT);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
169 if(endstop_x_hit) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
170 SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/axis_steps_per_unit[X_AXIS]);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
171 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
172 if(endstop_y_hit) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
173 SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/axis_steps_per_unit[Y_AXIS]);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
174 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
175 if(endstop_z_hit) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
176 SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
177 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
178 SERIAL_ECHOLN("");
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
179 endstop_x_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
180 endstop_y_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
181 endstop_z_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
182 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
183 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
184
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
185 void endstops_hit_on_purpose()
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
186 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
187 endstop_x_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
188 endstop_y_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
189 endstop_z_hit=false;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
190 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
191
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
192 void enable_endstops(bool check)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
193 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
194 check_endstops = check;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
195 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
196
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
197 // __________________________
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
198 // /| |\ _________________ ^
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
199 // / | | \ /| |\ |
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
200 // / | | \ / | | \ s
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
201 // / | | | | | \ p
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
202 // / | | | | | \ e
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
203 // +-----+------------------------+---+--+---------------+----+ e
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
204 // | BLOCK 1 | BLOCK 2 | d
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
205 //
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
206 // time ----->
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
207 //
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
208 // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
209 // first block->accelerate_until step_events_completed, then keeps going at constant speed until
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
210 // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
211 // The slope of acceleration is calculated with the leib ramp alghorithm.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
212
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
213 void st_wake_up() {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
214 // TCNT1 = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
215 ENABLE_STEPPER_DRIVER_INTERRUPT();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
216 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
217
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
218 FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
219 unsigned short timer;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
220 if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
221
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
222 if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
223 step_rate = (step_rate >> 2)&0x3fff;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
224 step_loops = 4;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
225 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
226 else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
227 step_rate = (step_rate >> 1)&0x7fff;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
228 step_loops = 2;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
229 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
230 else {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
231 step_loops = 1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
232 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
233
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
234 if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
235 step_rate -= (F_CPU/500000); // Correct for minimal speed
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
236 if(step_rate >= (8*256)){ // higher step rate
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
237 unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
238 unsigned char tmp_step_rate = (step_rate & 0x00ff);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
239 unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
240 MultiU16X8toH16(timer, tmp_step_rate, gain);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
241 timer = (unsigned short)pgm_read_word_near(table_address) - timer;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
242 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
243 else { // lower step rates
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
244 unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
245 table_address += ((step_rate)>>1) & 0xfffc;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
246 timer = (unsigned short)pgm_read_word_near(table_address);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
247 timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
248 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
249 if(timer < 100) { timer = 100; MYSERIAL.print(MSG_STEPPER_TO_HIGH); MYSERIAL.println(step_rate); }//(20kHz this should never happen)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
250 return timer;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
251 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
252
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
253 // Initializes the trapezoid generator from the current block. Called whenever a new
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
254 // block begins.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
255 FORCE_INLINE void trapezoid_generator_reset() {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
256 #ifdef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
257 advance = current_block->initial_advance;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
258 final_advance = current_block->final_advance;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
259 // Do E steps + advance steps
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
260 e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
261 old_advance = advance >>8;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
262 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
263 deceleration_time = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
264 // step_rate to timer interval
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
265 acc_step_rate = current_block->initial_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
266 acceleration_time = calc_timer(acc_step_rate);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
267 OCR1A = acceleration_time;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
268 OCR1A_nominal = calc_timer(current_block->nominal_rate);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
269
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
270
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
271
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
272 // SERIAL_ECHO_START;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
273 // SERIAL_ECHOPGM("advance :");
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
274 // SERIAL_ECHO(current_block->advance/256.0);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
275 // SERIAL_ECHOPGM("advance rate :");
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
276 // SERIAL_ECHO(current_block->advance_rate/256.0);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
277 // SERIAL_ECHOPGM("initial advance :");
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
278 // SERIAL_ECHO(current_block->initial_advance/256.0);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
279 // SERIAL_ECHOPGM("final advance :");
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
280 // SERIAL_ECHOLN(current_block->final_advance/256.0);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
281
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
282 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
283
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
284 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
285 // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
286 ISR(TIMER1_COMPA_vect)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
287 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
288 // If there is no current block, attempt to pop one from the buffer
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
289 if (current_block == NULL) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
290 // Anything in the buffer?
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
291 current_block = plan_get_current_block();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
292 if (current_block != NULL) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
293 current_block->busy = true;
1
b584642d4f58 several modifications to support laser enable - still needs cleanup
mbayer
parents: 0
diff changeset
294
b584642d4f58 several modifications to support laser enable - still needs cleanup
mbayer
parents: 0
diff changeset
295 if (current_block->laser_on) {
b584642d4f58 several modifications to support laser enable - still needs cleanup
mbayer
parents: 0
diff changeset
296 WRITE(M571_PIN, HIGH);// Fire the laser!
b584642d4f58 several modifications to support laser enable - still needs cleanup
mbayer
parents: 0
diff changeset
297 } else {
b584642d4f58 several modifications to support laser enable - still needs cleanup
mbayer
parents: 0
diff changeset
298 WRITE(M571_PIN, LOW);// Shutdown the laser!
b584642d4f58 several modifications to support laser enable - still needs cleanup
mbayer
parents: 0
diff changeset
299 }
b584642d4f58 several modifications to support laser enable - still needs cleanup
mbayer
parents: 0
diff changeset
300
0
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
301 trapezoid_generator_reset();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
302 counter_x = -(current_block->step_event_count >> 1);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
303 counter_y = counter_x;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
304 counter_z = counter_x;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
305 counter_e = counter_x;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
306 step_events_completed = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
307
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
308 #ifdef Z_LATE_ENABLE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
309 if(current_block->steps_z > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
310 enable_z();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
311 OCR1A = 2000; //1ms wait
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
312 return;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
313 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
314 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
315
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
316 // #ifdef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
317 // e_steps[current_block->active_extruder] = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
318 // #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
319 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
320 else {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
321 OCR1A=2000; // 1kHz.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
322 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
323 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
324
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
325 if (current_block != NULL) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
326 // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
327 out_bits = current_block->direction_bits;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
328
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
329 // Set direction en check limit switches
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
330 if ((out_bits & (1<<X_AXIS)) != 0) { // -direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
331 WRITE(X_DIR_PIN, INVERT_X_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
332 count_direction[X_AXIS]=-1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
333 CHECK_ENDSTOPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
334 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
335 #if X_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
336 bool x_min_endstop=(READ(X_MIN_PIN) != X_ENDSTOPS_INVERTING);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
337 if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
338 endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
339 endstop_x_hit=true;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
340 step_events_completed = current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
341 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
342 old_x_min_endstop = x_min_endstop;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
343 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
344 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
345 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
346 else { // +direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
347 WRITE(X_DIR_PIN,!INVERT_X_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
348 count_direction[X_AXIS]=1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
349 CHECK_ENDSTOPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
350 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
351 #if X_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
352 bool x_max_endstop=(READ(X_MAX_PIN) != X_ENDSTOPS_INVERTING);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
353 if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
354 endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
355 endstop_x_hit=true;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
356 step_events_completed = current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
357 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
358 old_x_max_endstop = x_max_endstop;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
359 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
360 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
361 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
362
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
363 if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
364 WRITE(Y_DIR_PIN,INVERT_Y_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
365 count_direction[Y_AXIS]=-1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
366 CHECK_ENDSTOPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
367 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
368 #if Y_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
369 bool y_min_endstop=(READ(Y_MIN_PIN) != Y_ENDSTOPS_INVERTING);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
370 if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
371 endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
372 endstop_y_hit=true;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
373 step_events_completed = current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
374 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
375 old_y_min_endstop = y_min_endstop;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
376 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
377 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
378 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
379 else { // +direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
380 WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
381 count_direction[Y_AXIS]=1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
382 CHECK_ENDSTOPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
383 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
384 #if Y_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
385 bool y_max_endstop=(READ(Y_MAX_PIN) != Y_ENDSTOPS_INVERTING);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
386 if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
387 endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
388 endstop_y_hit=true;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
389 step_events_completed = current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
390 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
391 old_y_max_endstop = y_max_endstop;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
392 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
393 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
394 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
395
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
396 if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
397 WRITE(Z_DIR_PIN,INVERT_Z_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
398 count_direction[Z_AXIS]=-1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
399 CHECK_ENDSTOPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
400 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
401 #if Z_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
402 bool z_min_endstop=(READ(Z_MIN_PIN) != Z_ENDSTOPS_INVERTING);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
403 if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
404 endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
405 endstop_z_hit=true;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
406 step_events_completed = current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
407 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
408 old_z_min_endstop = z_min_endstop;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
409 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
410 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
411 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
412 else { // +direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
413 WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
414 count_direction[Z_AXIS]=1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
415 CHECK_ENDSTOPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
416 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
417 #if Z_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
418 bool z_max_endstop=(READ(Z_MAX_PIN) != Z_ENDSTOPS_INVERTING);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
419 if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
420 endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
421 endstop_z_hit=true;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
422 step_events_completed = current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
423 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
424 old_z_max_endstop = z_max_endstop;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
425 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
426 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
427 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
428
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
429 #ifndef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
430 if ((out_bits & (1<<E_AXIS)) != 0) { // -direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
431 REV_E_DIR();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
432
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
433 if (m571_enabled) WRITE(M571_PIN, LOW);// M571 disable, never on retract!
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
434 count_direction[E_AXIS]=-1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
435 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
436 else { // +direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
437 NORM_E_DIR();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
438 count_direction[E_AXIS]=1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
439 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
440 #endif //!ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
441
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
442
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
443
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
444 for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
445 #ifndef REPRAPPRO_MULTIMATERIALS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
446 #if MOTHERBOARD != 8 // !teensylu
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
447 MSerial.checkRx(); // Check for serial chars.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
448 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
449 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
450
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
451 #ifdef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
452 counter_e += current_block->steps_e;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
453 if (counter_e > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
454 counter_e -= current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
455 if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
456 e_steps[current_block->active_extruder]--;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
457 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
458 else {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
459 e_steps[current_block->active_extruder]++;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
460 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
461 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
462 #endif //ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
463
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
464 counter_x += current_block->steps_x;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
465 if (counter_x > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
466 WRITE(X_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
467 counter_x -= current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
468 WRITE(X_STEP_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
469 count_position[X_AXIS]+=count_direction[X_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
470 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
471
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
472 counter_y += current_block->steps_y;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
473 if (counter_y > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
474 WRITE(Y_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
475 counter_y -= current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
476 WRITE(Y_STEP_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
477 count_position[Y_AXIS]+=count_direction[Y_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
478 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
479
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
480 counter_z += current_block->steps_z;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
481 if (counter_z > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
482 WRITE(Z_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
483 counter_z -= current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
484 WRITE(Z_STEP_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
485 count_position[Z_AXIS]+=count_direction[Z_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
486 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
487
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
488 #ifndef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
489 counter_e += current_block->steps_e;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
490 if (counter_e > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
491 // M571 enable, since extruder motor active
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
492 if (m571_enabled) WRITE(M571_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
493 // N571 disables real E drive! (ie. on laser operations)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
494 if (!n571_enabled) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
495 WRITE_E_STEP(HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
496 counter_e -= current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
497 WRITE_E_STEP(LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
498 } else counter_e -= current_block->step_event_count;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
499 count_position[E_AXIS]+=count_direction[E_AXIS];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
500 } else {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
501 // M571 disable, no more E_steps to do
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
502 if (m571_enabled) WRITE(M571_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
503
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
504 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
505 #endif //!ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
506 step_events_completed += 1;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
507 if(step_events_completed >= current_block->step_event_count) break;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
508 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
509 // Calculare new timer value
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
510 unsigned short timer;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
511 unsigned short step_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
512 if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
513
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
514 MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
515 acc_step_rate += current_block->initial_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
516
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
517 // upper limit
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
518 if(acc_step_rate > current_block->nominal_rate)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
519 acc_step_rate = current_block->nominal_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
520
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
521 // step_rate to timer interval
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
522 timer = calc_timer(acc_step_rate);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
523 OCR1A = timer;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
524 acceleration_time += timer;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
525 #ifdef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
526 for(int8_t i=0; i < step_loops; i++) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
527 advance += advance_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
528 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
529 //if(advance > current_block->advance) advance = current_block->advance;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
530 // Do E steps + advance steps
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
531 e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
532 old_advance = advance >>8;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
533
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
534 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
535 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
536 else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
537 MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
538
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
539 if(step_rate > acc_step_rate) { // Check step_rate stays positive
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
540 step_rate = current_block->final_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
541 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
542 else {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
543 step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
544 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
545
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
546 // lower limit
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
547 if(step_rate < current_block->final_rate)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
548 step_rate = current_block->final_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
549
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
550 // step_rate to timer interval
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
551 timer = calc_timer(step_rate);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
552 OCR1A = timer;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
553 deceleration_time += timer;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
554 #ifdef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
555 for(int8_t i=0; i < step_loops; i++) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
556 advance -= advance_rate;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
557 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
558 if(advance < final_advance) advance = final_advance;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
559 // Do E steps + advance steps
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
560 e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
561 old_advance = advance >>8;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
562 #endif //ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
563 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
564 else {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
565 OCR1A = OCR1A_nominal;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
566 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
567
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
568 // If current block is finished, reset pointer
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
569 if (step_events_completed >= current_block->step_event_count) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
570 current_block = NULL;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
571 plan_discard_current_block();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
572 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
573 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
574 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
575
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
576 #ifdef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
577 unsigned char old_OCR0A;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
578 // Timer interrupt for E. e_steps is set in the main routine;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
579 // Timer 0 is shared with millies
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
580 ISR(TIMER0_COMPA_vect)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
581 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
582 old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
583 OCR0A = old_OCR0A;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
584 // Set E direction (Depends on E direction + advance)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
585 for(unsigned char i=0; i<4;i++) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
586 if (e_steps[0] != 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
587 WRITE(E0_STEP_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
588 if (e_steps[0] < 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
589 WRITE(E0_DIR_PIN, INVERT_E0_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
590 e_steps[0]++;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
591 WRITE(E0_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
592 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
593 else if (e_steps[0] > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
594 WRITE(E0_DIR_PIN, !INVERT_E0_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
595 e_steps[0]--;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
596 WRITE(E0_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
597 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
598 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
599 #if EXTRUDERS > 1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
600 if (e_steps[1] != 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
601 WRITE(E1_STEP_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
602 if (e_steps[1] < 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
603 WRITE(E1_DIR_PIN, INVERT_E1_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
604 e_steps[1]++;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
605 WRITE(E1_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
606 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
607 else if (e_steps[1] > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
608 WRITE(E1_DIR_PIN, !INVERT_E1_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
609 e_steps[1]--;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
610 WRITE(E1_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
611 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
612 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
613 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
614 #if EXTRUDERS > 2
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
615 if (e_steps[2] != 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
616 WRITE(E2_STEP_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
617 if (e_steps[2] < 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
618 WRITE(E2_DIR_PIN, INVERT_E2_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
619 e_steps[2]++;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
620 WRITE(E2_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
621 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
622 else if (e_steps[2] > 0) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
623 WRITE(E2_DIR_PIN, !INVERT_E2_DIR);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
624 e_steps[2]--;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
625 WRITE(E2_STEP_PIN, HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
626 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
627 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
628 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
629 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
630 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
631 #endif // ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
632
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
633 void st_init()
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
634 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
635 //Initialize Dir Pins
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
636 #if X_DIR_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
637 SET_OUTPUT(X_DIR_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
638 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
639 #if Y_DIR_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
640 SET_OUTPUT(Y_DIR_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
641 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
642 #if Z_DIR_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
643 SET_OUTPUT(Z_DIR_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
644 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
645 #if E0_DIR_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
646 SET_OUTPUT(E0_DIR_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
647 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
648 #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
649 SET_OUTPUT(E1_DIR_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
650 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
651 #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
652 SET_OUTPUT(E2_DIR_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
653 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
654
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
655 //Initialize Enable Pins - steppers default to disabled.
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
656
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
657 #if (X_ENABLE_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
658 SET_OUTPUT(X_ENABLE_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
659 if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
660 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
661 #if (Y_ENABLE_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
662 SET_OUTPUT(Y_ENABLE_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
663 if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
664 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
665 #if (Z_ENABLE_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
666 SET_OUTPUT(Z_ENABLE_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
667 if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
668 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
669 #if (E0_ENABLE_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
670 SET_OUTPUT(E0_ENABLE_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
671 if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
672 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
673 #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
674 SET_OUTPUT(E1_ENABLE_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
675 if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
676 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
677 #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
678 SET_OUTPUT(E2_ENABLE_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
679 if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
680 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
681
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
682 //endstops and pullups
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
683 #ifdef ENDSTOPPULLUPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
684 #if X_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
685 SET_INPUT(X_MIN_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
686 WRITE(X_MIN_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
687 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
688 #if X_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
689 SET_INPUT(X_MAX_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
690 WRITE(X_MAX_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
691 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
692 #if Y_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
693 SET_INPUT(Y_MIN_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
694 WRITE(Y_MIN_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
695 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
696 #if Y_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
697 SET_INPUT(Y_MAX_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
698 WRITE(Y_MAX_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
699 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
700 #if Z_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
701 SET_INPUT(Z_MIN_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
702 WRITE(Z_MIN_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
703 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
704 #if Z_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
705 SET_INPUT(Z_MAX_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
706 WRITE(Z_MAX_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
707 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
708 #else //ENDSTOPPULLUPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
709 #if X_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
710 SET_INPUT(X_MIN_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
711 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
712 #if X_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
713 SET_INPUT(X_MAX_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
714 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
715 #if Y_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
716 SET_INPUT(Y_MIN_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
717 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
718 #if Y_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
719 SET_INPUT(Y_MAX_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
720 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
721 #if Z_MIN_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
722 SET_INPUT(Z_MIN_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
723 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
724 #if Z_MAX_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
725 SET_INPUT(Z_MAX_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
726 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
727 #endif //ENDSTOPPULLUPS
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
728
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
729
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
730 //Initialize Step Pins
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
731 #if (X_STEP_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
732 SET_OUTPUT(X_STEP_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
733 #if X_ENABLE_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
734 if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
735 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
736 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
737 #if (Y_STEP_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
738 SET_OUTPUT(Y_STEP_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
739 #if Y_ENABLE_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
740 if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
741 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
742 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
743 #if (Z_STEP_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
744 SET_OUTPUT(Z_STEP_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
745 #if Z_ENABLE_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
746 if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
747 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
748 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
749 #if (E0_STEP_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
750 SET_OUTPUT(E0_STEP_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
751 #if E0_ENABLE_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
752 if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
753 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
754 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
755 #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
756 SET_OUTPUT(E1_STEP_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
757 #if E1_ENABLE_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
758 if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
759 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
760 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
761 #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
762 SET_OUTPUT(E2_STEP_PIN);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
763 #if E2_ENABLE_PIN > -1
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
764 if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
765 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
766 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
767
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
768 #ifdef CONTROLLERFAN_PIN
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
769 SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
770 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
771
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
772 // M571 disable, switch off default
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
773 if (m571_enabled) WRITE(M571_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
774
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
775
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
776 // waveform generation = 0100 = CTC
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
777 TCCR1B &= ~(1<<WGM13);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
778 TCCR1B |= (1<<WGM12);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
779 TCCR1A &= ~(1<<WGM11);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
780 TCCR1A &= ~(1<<WGM10);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
781
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
782 // output mode = 00 (disconnected)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
783 TCCR1A &= ~(3<<COM1A0);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
784 TCCR1A &= ~(3<<COM1B0);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
785
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
786 // Set the timer pre-scaler
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
787 // Generally we use a divider of 8, resulting in a 2MHz timer
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
788 // frequency on a 16MHz MCU. If you are going to change this, be
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
789 // sure to regenerate speed_lookuptable.h with
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
790 // create_speed_lookuptable.py
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
791 TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
792
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
793 OCR1A = 0x4000;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
794 TCNT1 = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
795 ENABLE_STEPPER_DRIVER_INTERRUPT();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
796
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
797 #ifdef ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
798 #if defined(TCCR0A) && defined(WGM01)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
799 TCCR0A &= ~(1<<WGM01);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
800 TCCR0A &= ~(1<<WGM00);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
801 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
802 e_steps[0] = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
803 e_steps[1] = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
804 e_steps[2] = 0;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
805 TIMSK0 |= (1<<OCIE0A);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
806 #endif //ADVANCE
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
807
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
808 #ifdef ENDSTOPS_ONLY_FOR_HOMING
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
809 enable_endstops(false);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
810 #else
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
811 enable_endstops(true);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
812 #endif
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
813
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
814 sei();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
815 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
816
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
817
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
818 // Block until all buffered steps are executed
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
819 void st_synchronize()
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
820 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
821 while( blocks_queued()) {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
822 manage_heater();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
823 manage_inactivity(1);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
824 LCD_STATUS;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
825 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
826 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
827
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
828 void st_set_position(const long &x, const long &y, const long &z, const long &e)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
829 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
830 CRITICAL_SECTION_START;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
831 count_position[X_AXIS] = x;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
832 count_position[Y_AXIS] = y;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
833 count_position[Z_AXIS] = z;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
834 count_position[E_AXIS] = e;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
835 CRITICAL_SECTION_END;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
836 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
837
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
838 void st_set_e_position(const long &e)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
839 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
840 CRITICAL_SECTION_START;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
841 count_position[E_AXIS] = e;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
842 CRITICAL_SECTION_END;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
843 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
844
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
845 long st_get_position(uint8_t axis)
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
846 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
847 long count_pos;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
848 CRITICAL_SECTION_START;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
849 count_pos = count_position[axis];
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
850 CRITICAL_SECTION_END;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
851 return count_pos;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
852 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
853
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
854 void finishAndDisableSteppers()
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
855 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
856 st_synchronize();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
857 LCD_MESSAGEPGM(MSG_STEPPER_RELEASED);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
858 disable_x();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
859 disable_y();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
860 disable_z();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
861 disable_e0();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
862 disable_e1();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
863 disable_e2();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
864 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
865
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
866 void quickStop()
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
867 {
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
868 DISABLE_STEPPER_DRIVER_INTERRUPT();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
869
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
870 while(blocks_queued())
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
871 plan_discard_current_block();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
872 current_block = NULL;
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
873
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
874 // M571 disable, switch off default
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
875 if (m571_enabled) WRITE(M571_PIN, LOW);
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
876
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
877 ENABLE_STEPPER_DRIVER_INTERRUPT();
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
878 }
2c8ba1964db7 Initial code from reprappro Marlin repository
mbayer
parents:
diff changeset
879

mercurial