Rename state machine states
[fstop138:barndoor.git] / barndoor.ino
1 // -*- c -*-
2 //
3 // barndoor.ino: arduino code for an astrophotography barndoor mount
4 //
5 // Copyright (C) 2014 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
29 // http://arduino-info.wikispaces.com/HAL-LibrariesUpdates
30 #include <FiniteStateMachine.h>
31
32 // http://www.airspayce.com/mikem/arduino/AccelStepper/
33 #include <AccelStepper.h>
34
35 // We don't want to send debug over the serial port by default since
36 // it seriously slows down the main loop causing tracking errors
37 //#define DEBUG
38
39 // Constants to set based on hardware construction specs
40 static const float STEP_SIZE_DEG = 1.8;  // degrees rotation per step
41 static const float MICRO_STEPS = 8;     // number of microsteps per step
42 static const float THREADS_PER_CM = 8;  // number of threads in rod per cm of length
43 static const float BASE_LEN_CM = 30.5;     // length from hinge to center of rod in cm
44
45 // Constants to set based on electronic construction specs
46 static const int pinOutStep = 9;      // Arduino digital pin connected to EasyDriver step
47 static const int pinOutDirection = 8; // Arduino digital pin connected to EasyDriver direction
48
49 static const int pinInSidereal = 4;  // Arduino analogue pin connected to sidereal mode switch
50 static const int pinInHighspeed = 5; // Arduino analogue pin connected to highspeed mode switch
51 static const int pinInDirection = 3; // Arduino analogue pin connected to direction switch
52
53
54 // Derived constants
55 static const float USTEPS_PER_ROTATION = 360.0 / STEP_SIZE_DEG * MICRO_STEPS; // usteps per rod rotation
56
57
58 // Standard constants
59 static const float SIDE_REAL_SECS = 86164.0419; // time in seconds for 1 rotation of earth
60
61
62 // Setup motor class with parameters targetting an EasyDriver board
63 static AccelStepper motor(AccelStepper::DRIVER,
64                           pinOutStep,
65                           pinOutDirection);
66
67 // A finite state machine with 3 states - sidereal, highspeed and off
68 static State stateSidereal = State(state_sidereal_enter, state_sidereal_update, state_sidereal_exit);
69 static State stateHighspeed = State(state_highspeed_enter, state_highspeed_update, state_highspeed_update);
70 static State stateOff = State(state_off_enter, state_off_update, state_off_exit);
71 static FSM barndoor = FSM(stateOff);
72
73
74 // Given time offset from the 100% closed position, figure out
75 // the total number of steps required to achieve that
76 long time_to_usteps(long tsecs)
77 {
78     return (long)(USTEPS_PER_ROTATION *
79                   THREADS_PER_CM * 2.0 * BASE_LEN_CM *
80                   sin(tsecs * PI / SIDE_REAL_SECS));
81 }
82
83 // Given total number of steps from 100% closed position, figure out
84 // the corresponding total tracking time in seconds
85 long usteps_to_time(long usteps)
86 {
87     return (long)(asin(usteps /
88                        (USTEPS_PER_ROTATION * THREADS_PER_CM * 2.0 * BASE_LEN_CM)) *
89                   SIDE_REAL_SECS / PI);
90 }
91
92 void setup(void)
93 {
94     pinMode(pinInSidereal, OUTPUT);
95     pinMode(pinInHighspeed, OUTPUT);
96     pinMode(pinInDirection, OUTPUT);
97
98     motor.setPinsInverted(true, false, false);
99     motor.setMaxSpeed(3000);
100
101 #ifdef DEBUG
102     Serial.begin(9600);
103 #endif
104 }
105
106 // These variables are initialized when the motor switches
107 // from stopped to running, so we know our starting conditions
108
109 // Total motor steps since 100% closed, at the time the
110 // motor started running
111 static long startPositionUSteps;
112 // Total tracking time associated with total motor steps
113 static long startPositionSecs;
114 // Wall clock time at the point the motor switched from
115 // stopped to running.
116 static long startWallClockSecs;
117
118
119 // These variables are used while running to calculate our
120 // constantly changing targets
121
122 // The wall clock time where we need to next calculate tracking
123 // rate / target
124 static long targetWallClockSecs;
125 // Total tracking time associated with our target point
126 static long targetPositionSecs;
127 // Total motor steps associated with target point
128 static long targetPositionUSteps;
129
130
131 // This is called whenever the motor is switch from stopped to running.
132 //
133 // It reads the current motor position which lets us see how many steps
134 // have run since the device was first turned on in the 100% closed
135 // position. From that we then figure out the total tracking time that
136 // corresponds to our open angle. This is then used by plan_tracking()
137 // to figure out subsequent deltas
138 void start_tracking(void)
139 {
140     startPositionUSteps = motor.currentPosition();
141     startPositionSecs = usteps_to_time(startPositionUSteps);
142     startWallClockSecs = millis() / 1000;
143     targetWallClockSecs = startWallClockSecs;
144
145 #ifdef DEBUG
146     Serial.print("Enter sidereal\n");
147     Serial.print("start pos usteps: ");
148     Serial.print(startPositionUSteps);
149     Serial.print(", start pos secs: ");
150     Serial.print(startPositionSecs);
151     Serial.print(", start wclk secs: ");
152     Serial.print(startWallClockSecs);
153     Serial.print("\n\n");
154 #endif
155 }
156
157
158 // This is called when we need to figure out a new target position
159 //
160 // The tangent errors are small enough that over a short period,
161 // we can assume constant linear motion will give constant angular
162 // motion.
163 //
164 // So we set our target values to what we expect them all to be
165 // 15 seconds  in the future
166 void plan_tracking(void)
167 {
168     targetWallClockSecs = targetWallClockSecs + 15;
169     targetPositionSecs = startPositionSecs + (targetWallClockSecs - startWallClockSecs);
170     targetPositionUSteps = time_to_usteps(targetPositionSecs);
171
172 #ifdef DEBUG
173     Serial.print("target pos usteps: ");
174     Serial.print(targetPositionUSteps);
175     Serial.print(", target pos secs: ");
176     Serial.print(targetPositionSecs);
177     Serial.print(", target wclk secs: ");
178     Serial.print(targetWallClockSecs);
179     Serial.print("\n");
180 #endif
181 }
182
183
184 // This is called on every iteration of the main loop
185 //
186 // It looks at our target steps and target wall clock time and
187 // figures out the rate of steps required to get to the target
188 // in the remaining wall clock time. This applies the constant
189 // linear motion expected by  plan_tracking()
190 //
191 // By re-calculating rate of steps on every iteration, we are
192 // self-correcting if we are not invoked by the arduino at a
193 // constant rate
194 void apply_tracking(long currentWallClockSecs)
195 {
196     long timeLeft = targetWallClockSecs - currentWallClockSecs;
197     long stepsLeft = targetPositionUSteps - motor.currentPosition();
198     float stepsPerSec = (float)stepsLeft / (float)timeLeft;
199
200 #ifdef DEBUG32
201     Serial.print("Target ");
202     Serial.print(targetPositionUSteps);
203     Serial.print("  curr ");
204     Serial.print(motor.currentPosition());
205     Serial.print("  left");
206     Serial.print(stepsLeft);
207     Serial.print("\n");
208 #endif
209
210     motor.setSpeed(stepsPerSec);
211     motor.runSpeed();
212 }
213
214
215 // Called when switching from stopped to running
216 // in sidereal tracking mode
217 void state_sidereal_enter(void)
218 {
219     start_tracking();
220     plan_tracking();
221 }
222
223
224 // Called on every tick, when running in sidereal
225 // tracking mode
226 //
227 // XXX we don't currently use the direction switch
228 // in sidereal mode. Could use it for sidereal vs lunar
229 // tracking rate
230 void state_sidereal_update(void)
231 {
232     long currentWallClockSecs = millis() / 1000;
233
234     if (currentWallClockSecs >= targetWallClockSecs) {
235         plan_tracking();
236     }
237
238     apply_tracking(currentWallClockSecs);
239 }
240
241 void state_sidereal_exit(void)
242 {
243     // nada
244 }
245
246 void state_highspeed_enter(void)
247 {
248 #ifdef DEBUG
249     Serial.print("Enter highspeed\n");
250 #endif
251 }
252
253
254 // Called on every iteration when in manul highspeed
255 // forward/back mode. Will automatically step when it
256 // hits the 100% closed position to avoid straining
257 // the motor
258 void state_highspeed_update(void)
259 {
260     // pinInDirection is a 2-position switch for choosing direction
261     // of motion
262     if (analogRead(pinInDirection) < 512) {
263         motor.setSpeed(5000);
264         motor.runSpeed();
265     } else {
266         if (motor.currentPosition() == 0) {
267             motor.stop();
268         } else {
269             motor.setSpeed(-5000);
270             motor.runSpeed();
271         }
272     }
273 }
274
275 void state_highspeed_exit(void)
276 {
277     // nada
278 }
279
280 void state_off_enter(void)
281 {
282 #ifdef DEBUG
283     Serial.print("Enter off\n");
284 #endif
285     motor.stop();
286 }
287
288 void state_off_update(void)
289 {
290     // nada
291 }
292
293 void state_off_exit(void)
294 {
295     // nada
296 }
297
298
299 void loop(void)
300 {
301     // pinInSidereal/pinInHighspeed are two poles of a 3-position
302     // switch, that let us choose between sidereal tracking,
303     // stopped and highspeed mode
304     if (analogRead(pinInSidereal) < 512) {
305         barndoor.transitionTo(stateSidereal);
306     } else if (analogRead(pinInHighspeed) < 512) {
307         barndoor.transitionTo(stateHighspeed);
308     } else {
309         barndoor.transitionTo(stateOff);
310     }
311     barndoor.update();
312 }
313
314 //
315 // Local variables:
316 //  c-indent-level: 4
317 //  c-basic-offset: 4
318 //  indent-tabs-mode: nil
319 // End:
320 //