1 /*
2
3 DIYServo 1.0
4
5 Controller Motor Speed and Movement with Absolute Positioning
6
7 A sketch for turning a standard DC gear motor into a servo
8 using a potentiometer. Can also be used to add finer-grained
9 control over existing servos.
10
11 Control over motor :
12 Speed (PWM)
13 Direction
14 Number of degrees to move (up to 1024) per move
15 How to long to wait between moves
16
17 To get finer control (1024 degrees, instead of 360) of an existing
18 servo, remove the controller and any stop-pins, disconnect the
19 potentiometer wires. Connect power lines from servo motor to a
20 DC motor controller, potentiometer wiper to an input pin on the arduino,
21 and the ouside potentiometer pins to 5v/GND.
22
23 Serial monitor:
24
25 use 'l' to tell the servo to move left
26 use 'r' to tell the servo to move right
27 use 's' to stop the servo
28 use 'g' to go (run the servo)
29
30
31 (c) 2008 C. A. Church - www.dronecolony.com
32
33 7/24/08 - initial version
34
35 */
36
37
38
39 // USING_CONTROLLER says whether we have to bring an
40 // enable pin high (such as for the Compact L298 controller)
41 // before sending PWM down the LT/RT pins
42 // set this to 0 if you don't need an enable pin
43
44
45 #define USING_CONTROLLER 1
46
47 // enable pin
48 #define MOTOR_EN_PIN 8
49 // right direction pin
50 #define MOTOR_RT_PIN 6
51 // left direction pin
52 #define MOTOR_LT_PIN 9
53
54
55 // READ_AVG is how many readings to average
56 // set it to one less than the actual #
57 // e.g.: 10 readings = set to 9
58 //
59 // the more you average, the more accurate your reading is likely to
60 // be -- too many though, and you'll start missing changes if the motor
61 // is moving quickly
62
63 #define READ_AVG 9
64
65
66
67 // motor speed is from 0-255, test with low values
68 // as not all will move consistently for you
69
70 int motor_speed = 75;
71
72 // how many ms to pause between allowed movements
73
74 int motor_pause_tm = 1000;
75
76 // how many 'degrees' (absolute differences between
77 // potentiometer readings) to move before pausing
78
79 int motor_move_deg = 5;
80
81 // a counter for how many degrees we have moved
82
83 int move_deg_cnt = 0;
84
85 // setting to a default value
86
87 int motor_cur_pin = MOTOR_RT_PIN;
88
89 // control indicators
90 bool motor_started = false;
91 bool motor_paused = false;
92 bool first_run = true;
93 bool motor_run = false;
94
95 long paused_tm;
96
97
98 // our current and previous potentiometer readings
99
100 int cur_reading = 0;
101 int pre_reading = 0;
102
103 int steps = 0;
104
105 // our current readings array, and our previous average readings array
106
107 int vals[READ_AVG + 1];
108 int prev_posts[2] = { 0, 0 };
109
110
111
112 void setup() {
113
114 Serial.begin(19200);
115 Serial.println("Ready");
116
117 memset( (char *)vals, 0, sizeof(int) * (READ_AVG + 1) );
118
119 // set motor control pins
120
121 if( USING_CONTROLLER )
122 digitalWrite(MOTOR_EN_PIN, HIGH);
123
124 digitalWrite(MOTOR_LT_PIN, LOW);
125 digitalWrite(MOTOR_RT_PIN, LOW);
126
127 }
128
129 void loop() {
130
131 // see if any input has come in the serial port
132
133 check_input();
134
135 // figure out how many degrees we've moved (if at all)
136
137 move_deg_cnt = move_deg_cnt + read_pot();
138
139 if( motor_run == true ) {
140 // if the motor is supposed to be running
141
142 // the following check is to prevent attempting to rotate all the
143 // way around on a potentiometer that has a stop. If yours doesn't
144 // have a stop in it, ou can remove this check
145
146 if( (motor_cur_pin == MOTOR_RT_PIN && prev_posts[1] >= 1020 ) ||
147 ( motor_cur_pin == MOTOR_LT_PIN && prev_posts[1] <= 3 ) ) {
148
149 // we've reached our maximum point (don't want to harm our
150 // potentiometer
151
152 motor_run = false;
153 motor_started = false;
154 motor_paused = false;
155
156 // bring pin low
157
158 digitalWrite(motor_cur_pin, LOW);
159
160 // print status
161
162 Serial.println("Have Reached Edge of Movement");
163 }
164 else if( motor_started == false ) {
165 // the motor is supposed to be running, but we haven't started
166 // it yet
167
168 // PWM output
169
170 analogWrite(motor_cur_pin, motor_speed);
171
172 // set status values
173
174 motor_started = true;
175 motor_paused = false;
176 first_run = true;
177 }
178 else if( move_deg_cnt >= motor_move_deg && motor_paused == false && first_run == false) {
179
180 // we've gone our specific # of degrees, pause by stopping the
181 // motor
182
183 Serial.println("Pausing");
184
185 digitalWrite(motor_cur_pin, LOW);
186 motor_paused = true;
187
188 // record when we started our pause (so we know when to stop)
189
190 paused_tm = millis();
191 }
192 else if( motor_paused == true && (millis() - paused_tm) > motor_pause_tm ) {
193
194 // if enough time has passed to stop pausing
195
196 Serial.println("Unpausing");
197 motor_paused = false;
198 paused_tm = millis();
199
200 // set move_deg_cnt to zero when re-starting to avoid any
201 // jitter while paused
202
203 move_deg_cnt = 0;
204
205 // generate PWM
206 analogWrite(motor_cur_pin, motor_speed);
207
208 }
209 }
210
211 }
212
213 int read_pot() {
214
215 //read the voltage on the potentiometer:
216 cur_reading = analogRead(0);
217 int diff = 0;
218
219
220 // we're going to average the last READ_AVG reads
221 // put in a value for our current step
222
223 vals[steps] = cur_reading;
224
225 // if we've saved enough values to go ahead and perform an average...
226
227 if( steps == READ_AVG ) {
228
229 // reset our read counter
230
231 steps = -1;
232
233 // determine the average value read
234 // -- this is mostly to deal with big jitter
235
236 int tot = 0;
237 int avg = 0;
238
239 // sum up totals
240
241 for (int i = 0; i <= READ_AVG; i++)
242 tot += vals[i];
243
244
245 avg = tot / READ_AVG + 1;
246
247 // ignore current reading if it was either of our last two readings
248 // avoid bouncing back and forth between two readings (slight voltage
249 // variation in the same range)
250
251 if( avg == prev_posts[0] || avg == prev_posts[1] ) {
252 return(0);
253 }
254
255 // determine the absolute difference between the current average
256 // and the previous average
257
258 diff = avg > prev_posts[1] ? avg - prev_posts[1] : prev_posts[1] - avg;
259
260
261 // if there's a difference between the averages
262
263 if( diff > 0 ) {
264
265 // print our new reading
266
267 Serial.println(avg, DEC);
268
269 // move our last reading back, and put our current reading in
270 // our array to track the last two positions
271
272 prev_posts[0] = prev_posts[1];
273 prev_posts[1] = avg;
274
275 // update this so the pause check knows that we have changed a position
276 // (otherwise, starting in a position oher than 0 will mess up our
277 // pause check)
278
279 first_run = false;
280 }
281
282
283 }
284
285 // increment our saved value # for the next loop
286
287 steps++;
288
289 // return the difference recorded
290 return(diff);
291
292
293 }
294
295
296 void check_input() {
297 if ( Serial.available()) {
298 char ch = Serial.read();
299
300 switch(ch) {
301 case 'g':
302 Serial.println("Go - Running Motor");
303 motor_run = true;
304 digitalWrite(13, HIGH);
305 break;
306 case 's':
307 Serial.println("Stopping Motor");
308 motor_run = false;
309 motor_started = false;
310 analogWrite(motor_cur_pin, 0);
311 digitalWrite(13, LOW);
312 break;
313 case 'l':
314 motor_cur_pin = MOTOR_LT_PIN;
315 Serial.println("Direction = LEFT");
316 break;
317 case 'r':
318 motor_cur_pin = MOTOR_RT_PIN;
319 Serial.println("Direction = RIGHT");
320 break;
321 }
322 }
323 }
| 
