Add support for an initial offset angle
[fstop138:barndoor.git] / barndoor.ino
1 // -*- c -*-
2 //
3 // barndoor.ino: arduino code for an astrophotography barndoor mount
4 //
5 // Copyright (C) 2014-2015 Daniel P. Berrange
6 //
7 // This program is free software: you can redistribute it and/or modify
8 // it under the terms of the GNU General Public License as published by
9 // the Free Software Foundation, either version 3 of the License, or
10 // (at your option) any later version.
11 //
12 // This program is distributed in the hope that it will be useful,
13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 // GNU General Public License for more details.
16 //
17 // You should have received a copy of the GNU General Public License
18 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
19 //
20 // This code is used to drive the circuit described at
21 //
22 //  http://fstop138.berrange.com/2014/01/building-an-barn-door-mount-part-1-arduino-stepper-motor-control/
23 //
24 // Based on the maths calculations documented at
25 //
26 //  http://fstop138.berrange.com/2014/01/building-an-barn-door-mount-part-2-calculating-mount-movements/
27 //
28 // The code assumes an **isosceles** drive barn door mount design.
29 //
30 // Other barndoor drive designs will require different mathematical
31 // formulas to correct errors
32
33 // http://arduino-info.wikispaces.com/HAL-LibrariesUpdates
34 #include <FiniteStateMachine.h>
35
36 // http://www.airspayce.com/mikem/arduino/AccelStepper/
37 #include <AccelStepper.h>
38
39 // We don't want to send debug over the serial port by default since
40 // it seriously slows down the main loop causing tracking errors
41 //#define DEBUG
42
43 // Constants to set based on hardware construction specs
44 //
45 // Assuming you followed the blog linked above, these few variables
46 // should be the only things that you need to change in general
47 //
48 static const float STEP_SIZE_DEG = 1.8;  // Degrees rotation per step
49 static const float MICRO_STEPS = 8;      // Number of microsteps per step
50 static const float THREADS_PER_CM = 8;   // Number of threads in rod per cm of length
51 static const float BASE_LEN_CM = 30.5;   // Length from hinge to center of rod in cm
52 static const float INITIAL_ANGLE = 0;    // Initial angle of barn doors when switched on
53
54 // Nothing below this line should require changing unless your barndoor
55 // is not an Isoceles mount, or you changed the electrical circuit design
56
57 // Constants to set based on electronic construction specs
58 static const int pinOutStep = 9;      // Arduino digital pin connected to EasyDriver step
59 static const int pinOutDirection = 8; // Arduino digital pin connected to EasyDriver direction
60
61 static const int pinInSidereal = 4;  // Arduino analogue pin connected to sidereal mode switch
62 static const int pinInHighspeed = 5; // Arduino analogue pin connected to highspeed mode switch
63 static const int pinInDirection = 3; // Arduino analogue pin connected to direction switch
64
65
66 // Derived constants
67 static const float USTEPS_PER_ROTATION = 360.0 / STEP_SIZE_DEG * MICRO_STEPS; // usteps per rod rotation
68
69
70 // Standard constants
71 static const float SIDE_REAL_SECS = 86164.0419; // time in seconds for 1 rotation of earth
72
73
74 // Setup motor class with parameters targetting an EasyDriver board
75 static AccelStepper motor(AccelStepper::DRIVER,
76                           pinOutStep,
77                           pinOutDirection);
78
79 // A finite state machine with 3 states - sidereal, highspeed and off
80 static State stateSidereal = State(state_sidereal_enter, state_sidereal_update, state_sidereal_exit);
81 static State stateHighspeed = State(state_highspeed_enter, state_highspeed_update, state_highspeed_update);
82 static State stateOff = State(state_off_enter, state_off_update, state_off_exit);
83 static FSM barndoor = FSM(stateOff);
84
85
86 // Given time offset from the 100% closed position, figure out
87 // the total number of steps required to achieve that
88 long time_to_usteps(long tsecs)
89 {
90     return (long)(USTEPS_PER_ROTATION *
91                   THREADS_PER_CM * 2.0 * BASE_LEN_CM *
92                   sin(tsecs * PI / SIDE_REAL_SECS));
93 }
94
95
96 // Given an angle, figure out the usteps required to get to
97 // that point.
98 long angle_to_usteps(float angle)
99 {
100     return time_to_usteps(SIDE_REAL_SECS / 360.0 * angle);
101 }
102
103
104 // Given total number of steps from 100% closed position, figure out
105 // the corresponding total tracking time in seconds
106 long usteps_to_time(long usteps)
107 {
108     return (long)(asin(usteps /
109                        (USTEPS_PER_ROTATION * THREADS_PER_CM * 2.0 * BASE_LEN_CM)) *
110                   SIDE_REAL_SECS / PI);
111 }
112
113 // These variables are initialized when the motor switches
114 // from stopped to running, so we know our starting conditions
115
116 // If the barn door doesn't go to 100% closed, this records
117 // the inital offset we started from for INITIAL_ANGLE
118 static long offsetPositionUSteps;
119 // Total motor steps since 100% closed, at the time the
120 // motor started running
121 static long startPositionUSteps;
122 // Total tracking time associated with total motor steps
123 static long startPositionSecs;
124 // Wall clock time at the point the motor switched from
125 // stopped to running.
126 static long startWallClockSecs;
127
128
129 // These variables are used while running to calculate our
130 // constantly changing targets
131
132 // The wall clock time where we need to next calculate tracking
133 // rate / target
134 static long targetWallClockSecs;
135 // Total tracking time associated with our target point
136 static long targetPositionSecs;
137 // Total motor steps associated with target point
138 static long targetPositionUSteps;
139
140
141 // Global initialization when first turned off
142 void setup(void)
143 {
144     pinMode(pinInSidereal, OUTPUT);
145     pinMode(pinInHighspeed, OUTPUT);
146     pinMode(pinInDirection, OUTPUT);
147
148     motor.setPinsInverted(true, false, false);
149     motor.setMaxSpeed(3000);
150
151     offsetPositionUSteps = angle_to_usteps(INITIAL_ANGLE);
152
153 #ifdef DEBUG
154     Serial.begin(9600);
155 #endif
156 }
157
158
159 // The logical motor position which takes into account the
160 // fact that we have an initial opening angle
161 long motor_position()
162 {
163     return motor.currentPosition() + offsetPositionUSteps;
164 }
165
166
167 // This is called whenever the motor is switch from stopped to running.
168 //
169 // It reads the current motor position which lets us see how many steps
170 // have run since the device was first turned on in the 100% closed
171 // position. From that we then figure out the total tracking time that
172 // corresponds to our open angle. This is then used by plan_tracking()
173 // to figure out subsequent deltas
174 void start_tracking(void)
175 {
176     startPositionUSteps = motor_position();
177     startPositionSecs = usteps_to_time(startPositionUSteps);
178     startWallClockSecs = millis() / 1000;
179     targetWallClockSecs = startWallClockSecs;
180
181 #ifdef DEBUG
182     Serial.print("Enter sidereal\n");
183     Serial.print("offset pos usteps: ");
184     Serial.print(offsetPositionUSteps);
185     Serial.print(", start pos usteps: ");
186     Serial.print(startPositionUSteps);
187     Serial.print(", start pos secs: ");
188     Serial.print(startPositionSecs);
189     Serial.print(", start wclk secs: ");
190     Serial.print(startWallClockSecs);
191     Serial.print("\n\n");
192 #endif
193 }
194
195
196 // This is called when we need to figure out a new target position
197 //
198 // The tangent errors are small enough that over a short period,
199 // we can assume constant linear motion will give constant angular
200 // motion.
201 //
202 // So we set our target values to what we expect them all to be
203 // 15 seconds  in the future
204 void plan_tracking(void)
205 {
206     targetWallClockSecs = targetWallClockSecs + 15;
207     targetPositionSecs = startPositionSecs + (targetWallClockSecs - startWallClockSecs);
208     targetPositionUSteps = time_to_usteps(targetPositionSecs);
209
210 #ifdef DEBUG
211     Serial.print("target pos usteps: ");
212     Serial.print(targetPositionUSteps);
213     Serial.print(", target pos secs: ");
214     Serial.print(targetPositionSecs);
215     Serial.print(", target wclk secs: ");
216     Serial.print(targetWallClockSecs);
217     Serial.print("\n");
218 #endif
219 }
220
221
222 // This is called on every iteration of the main loop
223 //
224 // It looks at our target steps and target wall clock time and
225 // figures out the rate of steps required to get to the target
226 // in the remaining wall clock time. This applies the constant
227 // linear motion expected by  plan_tracking()
228 //
229 // By re-calculating rate of steps on every iteration, we are
230 // self-correcting if we are not invoked by the arduino at a
231 // constant rate
232 void apply_tracking(long currentWallClockSecs)
233 {
234     long timeLeft = targetWallClockSecs - currentWallClockSecs;
235     long stepsLeft = targetPositionUSteps - motor_position();
236     float stepsPerSec = (float)stepsLeft / (float)timeLeft;
237
238 #ifdef DEBUG32
239     Serial.print("Target ");
240     Serial.print(targetPositionUSteps);
241     Serial.print("  curr ");
242     Serial.print(motor_position());
243     Serial.print("  left");
244     Serial.print(stepsLeft);
245     Serial.print("\n");
246 #endif
247
248     motor.setSpeed(stepsPerSec);
249     motor.runSpeed();
250 }
251
252
253 // Called when switching from stopped to running
254 // in sidereal tracking mode
255 void state_sidereal_enter(void)
256 {
257     start_tracking();
258     plan_tracking();
259 }
260
261
262 // Called on every tick, when running in sidereal
263 // tracking mode
264 //
265 // XXX we don't currently use the direction switch
266 // in sidereal mode. Could use it for sidereal vs lunar
267 // tracking rate perhaps ?
268 void state_sidereal_update(void)
269 {
270     long currentWallClockSecs = millis() / 1000;
271
272     if (currentWallClockSecs >= targetWallClockSecs) {
273         plan_tracking();
274     }
275
276     apply_tracking(currentWallClockSecs);
277 }
278
279 void state_sidereal_exit(void)
280 {
281     // nada
282 }
283
284 void state_highspeed_enter(void)
285 {
286 #ifdef DEBUG
287     Serial.print("Enter highspeed\n");
288 #endif
289 }
290
291
292 // Called on every iteration when in non-tracking highspeed
293 // forward/back mode. Will automatically step when it
294 // hits the 100% closed position to avoid straining
295 // the motor
296 void state_highspeed_update(void)
297 {
298     // pinInDirection is a 2-position switch for choosing direction
299     // of motion
300     if (analogRead(pinInDirection) < 512) {
301         motor.setSpeed(5000);
302         motor.runSpeed();
303     } else {
304         if (motor.currentPosition() == 0) {
305             motor.stop();
306         } else {
307             motor.setSpeed(-5000);
308             motor.runSpeed();
309         }
310     }
311 }
312
313 void state_highspeed_exit(void)
314 {
315     // nada
316 }
317
318 void state_off_enter(void)
319 {
320 #ifdef DEBUG
321     Serial.print("Enter off\n");
322 #endif
323     motor.stop();
324 }
325
326 void state_off_update(void)
327 {
328     // nada
329 }
330
331 void state_off_exit(void)
332 {
333     // nada
334 }
335
336
337 void loop(void)
338 {
339     // pinInSidereal/pinInHighspeed are two poles of a 3-position
340     // switch, that let us choose between sidereal tracking,
341     // stopped and highspeed mode
342     if (analogRead(pinInSidereal) < 512) {
343         barndoor.transitionTo(stateSidereal);
344     } else if (analogRead(pinInHighspeed) < 512) {
345         barndoor.transitionTo(stateHighspeed);
346     } else {
347         barndoor.transitionTo(stateOff);
348     }
349     barndoor.update();
350 }
351
352 //
353 // Local variables:
354 //  c-indent-level: 4
355 //  c-basic-offset: 4
356 //  indent-tabs-mode: nil
357 // End:
358 //