Add support for a maximum opening limit
[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 static const float MAXIMUM_ANGLE = 30;   // Maximum angle to allow barn doors to open (30 deg == 2 hours)
54
55 // Nothing below this line should require changing unless your barndoor
56 // is not an Isoceles mount, or you changed the electrical circuit design
57
58 // Constants to set based on electronic construction specs
59 static const int pinOutStep = 9;      // Arduino digital pin connected to EasyDriver step
60 static const int pinOutDirection = 8; // Arduino digital pin connected to EasyDriver direction
61
62 static const int pinInSidereal = 4;  // Arduino analogue pin connected to sidereal mode switch
63 static const int pinInHighspeed = 5; // Arduino analogue pin connected to highspeed mode switch
64 static const int pinInDirection = 3; // Arduino analogue pin connected to direction switch
65
66
67 // Derived constants
68 static const float USTEPS_PER_ROTATION = 360.0 / STEP_SIZE_DEG * MICRO_STEPS; // usteps per rod rotation
69
70
71 // Standard constants
72 static const float SIDE_REAL_SECS = 86164.0419; // time in seconds for 1 rotation of earth
73
74
75 // Setup motor class with parameters targetting an EasyDriver board
76 static AccelStepper motor(AccelStepper::DRIVER,
77                           pinOutStep,
78                           pinOutDirection);
79
80 // A finite state machine with 3 states - sidereal, highspeed and off
81 static State stateSidereal = State(state_sidereal_enter, state_sidereal_update, state_sidereal_exit);
82 static State stateHighspeed = State(state_highspeed_enter, state_highspeed_update, state_highspeed_update);
83 static State stateOff = State(state_off_enter, state_off_update, state_off_exit);
84 static FSM barndoor = FSM(stateOff);
85
86
87 // Given time offset from the 100% closed position, figure out
88 // the total number of steps required to achieve that
89 long time_to_usteps(long tsecs)
90 {
91     return (long)(USTEPS_PER_ROTATION *
92                   THREADS_PER_CM * 2.0 * BASE_LEN_CM *
93                   sin(tsecs * PI / SIDE_REAL_SECS));
94 }
95
96
97 // Given an angle, figure out the usteps required to get to
98 // that point.
99 long angle_to_usteps(float angle)
100 {
101     return time_to_usteps(SIDE_REAL_SECS / 360.0 * angle);
102 }
103
104
105 // Given total number of steps from 100% closed position, figure out
106 // the corresponding total tracking time in seconds
107 long usteps_to_time(long usteps)
108 {
109     return (long)(asin(usteps /
110                        (USTEPS_PER_ROTATION * THREADS_PER_CM * 2.0 * BASE_LEN_CM)) *
111                   SIDE_REAL_SECS / PI);
112 }
113
114 // These variables are initialized when the motor switches
115 // from stopped to running, so we know our starting conditions
116
117 // If the barn door doesn't go to 100% closed, this records
118 // the inital offset we started from for INITIAL_ANGLE
119 static long offsetPositionUSteps;
120 // The maximum we're willing to open the mount to avoid the
121 // doors falling open and smashing the camera. Safety first :-)
122 static long maximumPositionUSteps;
123 // Total motor steps since 100% closed, at the time the
124 // motor started running
125 static long startPositionUSteps;
126 // Total tracking time associated with total motor steps
127 static long startPositionSecs;
128 // Wall clock time at the point the motor switched from
129 // stopped to running.
130 static long startWallClockSecs;
131
132
133 // These variables are used while running to calculate our
134 // constantly changing targets
135
136 // The wall clock time where we need to next calculate tracking
137 // rate / target
138 static long targetWallClockSecs;
139 // Total tracking time associated with our target point
140 static long targetPositionSecs;
141 // Total motor steps associated with target point
142 static long targetPositionUSteps;
143
144
145 // Global initialization when first turned off
146 void setup(void)
147 {
148     pinMode(pinInSidereal, OUTPUT);
149     pinMode(pinInHighspeed, OUTPUT);
150     pinMode(pinInDirection, OUTPUT);
151
152     motor.setPinsInverted(true, false, false);
153     motor.setMaxSpeed(3000);
154
155     offsetPositionUSteps = angle_to_usteps(INITIAL_ANGLE);
156     maximumPositionUSteps = angle_to_usteps(MAXIMUM_ANGLE);
157
158 #ifdef DEBUG
159     Serial.begin(9600);
160 #endif
161 }
162
163
164 // The logical motor position which takes into account the
165 // fact that we have an initial opening angle
166 long motor_position()
167 {
168     return motor.currentPosition() + offsetPositionUSteps;
169 }
170
171
172 // This is called whenever the motor is switch from stopped to running.
173 //
174 // It reads the current motor position which lets us see how many steps
175 // have run since the device was first turned on in the 100% closed
176 // position. From that we then figure out the total tracking time that
177 // corresponds to our open angle. This is then used by plan_tracking()
178 // to figure out subsequent deltas
179 void start_tracking(void)
180 {
181     startPositionUSteps = motor_position();
182     startPositionSecs = usteps_to_time(startPositionUSteps);
183     startWallClockSecs = millis() / 1000;
184     targetWallClockSecs = startWallClockSecs;
185
186 #ifdef DEBUG
187     Serial.print("Enter sidereal\n");
188     Serial.print("offset pos usteps: ");
189     Serial.print(offsetPositionUSteps);
190     Serial.print(", start pos usteps: ");
191     Serial.print(startPositionUSteps);
192     Serial.print(", start pos secs: ");
193     Serial.print(startPositionSecs);
194     Serial.print(", start wclk secs: ");
195     Serial.print(startWallClockSecs);
196     Serial.print("\n\n");
197 #endif
198 }
199
200
201 // This is called when we need to figure out a new target position
202 //
203 // The tangent errors are small enough that over a short period,
204 // we can assume constant linear motion will give constant angular
205 // motion.
206 //
207 // So we set our target values to what we expect them all to be
208 // 15 seconds  in the future
209 void plan_tracking(void)
210 {
211     targetWallClockSecs = targetWallClockSecs + 15;
212     targetPositionSecs = startPositionSecs + (targetWallClockSecs - startWallClockSecs);
213     targetPositionUSteps = time_to_usteps(targetPositionSecs);
214
215 #ifdef DEBUG
216     Serial.print("target pos usteps: ");
217     Serial.print(targetPositionUSteps);
218     Serial.print(", target pos secs: ");
219     Serial.print(targetPositionSecs);
220     Serial.print(", target wclk secs: ");
221     Serial.print(targetWallClockSecs);
222     Serial.print("\n");
223 #endif
224 }
225
226
227 // This is called on every iteration of the main loop
228 //
229 // It looks at our target steps and target wall clock time and
230 // figures out the rate of steps required to get to the target
231 // in the remaining wall clock time. This applies the constant
232 // linear motion expected by  plan_tracking()
233 //
234 // By re-calculating rate of steps on every iteration, we are
235 // self-correcting if we are not invoked by the arduino at a
236 // constant rate
237 void apply_tracking(long currentWallClockSecs)
238 {
239     long timeLeft = targetWallClockSecs - currentWallClockSecs;
240     long stepsLeft = targetPositionUSteps - motor_position();
241     float stepsPerSec = (float)stepsLeft / (float)timeLeft;
242
243 #ifdef DEBUG32
244     Serial.print("Target ");
245     Serial.print(targetPositionUSteps);
246     Serial.print("  curr ");
247     Serial.print(motor_position());
248     Serial.print("  left");
249     Serial.print(stepsLeft);
250     Serial.print("\n");
251 #endif
252
253     if (motor_position() >= maximumPositionUSteps) {
254         motor.stop();
255     } else {
256         motor.setSpeed(stepsPerSec);
257         motor.runSpeed();
258     }
259 }
260
261
262 // Called when switching from stopped to running
263 // in sidereal tracking mode
264 void state_sidereal_enter(void)
265 {
266     start_tracking();
267     plan_tracking();
268 }
269
270
271 // Called on every tick, when running in sidereal
272 // tracking mode
273 //
274 // XXX we don't currently use the direction switch
275 // in sidereal mode. Could use it for sidereal vs lunar
276 // tracking rate perhaps ?
277 void state_sidereal_update(void)
278 {
279     long currentWallClockSecs = millis() / 1000;
280
281     if (currentWallClockSecs >= targetWallClockSecs) {
282         plan_tracking();
283     }
284
285     apply_tracking(currentWallClockSecs);
286 }
287
288 void state_sidereal_exit(void)
289 {
290     // nada
291 }
292
293 void state_highspeed_enter(void)
294 {
295 #ifdef DEBUG
296     Serial.print("Enter highspeed\n");
297 #endif
298 }
299
300
301 // Called on every iteration when in non-tracking highspeed
302 // forward/back mode. Will automatically step when it
303 // hits the 100% closed position to avoid straining
304 // the motor
305 void state_highspeed_update(void)
306 {
307     // pinInDirection is a 2-position switch for choosing direction
308     // of motion
309     if (analogRead(pinInDirection) < 512) {
310         if (motor_position() >= maximumPositionUSteps) {
311             motor.stop();
312         } else {
313             motor.setSpeed(5000);
314             motor.runSpeed();
315         }
316     } else {
317         if (motor.currentPosition() <= 0) {
318             motor.stop();
319         } else {
320             motor.setSpeed(-5000);
321             motor.runSpeed();
322         }
323     }
324 }
325
326 void state_highspeed_exit(void)
327 {
328     // nada
329 }
330
331 void state_off_enter(void)
332 {
333 #ifdef DEBUG
334     Serial.print("Enter off\n");
335 #endif
336     motor.stop();
337 }
338
339 void state_off_update(void)
340 {
341     // nada
342 }
343
344 void state_off_exit(void)
345 {
346     // nada
347 }
348
349
350 void loop(void)
351 {
352     // pinInSidereal/pinInHighspeed are two poles of a 3-position
353     // switch, that let us choose between sidereal tracking,
354     // stopped and highspeed mode
355     if (analogRead(pinInSidereal) < 512) {
356         barndoor.transitionTo(stateSidereal);
357     } else if (analogRead(pinInHighspeed) < 512) {
358         barndoor.transitionTo(stateHighspeed);
359     } else {
360         barndoor.transitionTo(stateOff);
361     }
362     barndoor.update();
363 }
364
365 //
366 // Local variables:
367 //  c-indent-level: 4
368 //  c-basic-offset: 4
369 //  indent-tabs-mode: nil
370 // End:
371 //