uStepper S
uStepperServo.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepperServo.cpp *
3 * Version: 2.2.0 *
4 * Date: September 22nd, 2020 *
5 * Authors: Thomas Hørring Olsen *
6 * Emil Jacobsen *
7 * *
8 *********************************************************************************************
9 * uStepperServo class *
10 * *
11 * This file contains the implementation of the class methods, incorporated in the *
12 * uStepperServo Arduino library. The library is used by instantiating an uStepperServo *
13 * object by calling either of the two overloaded constructors: *
14 * *
15 * example: *
16 * *
17 * uStepperServo servo; *
18 * *
19 * *
20 * after instantiation of the object, the object attach function, should be called within *
21 * arduino's setup function: *
22 * *
23 * example: *
24 * *
25 * uStepperServo servo; *
26 * *
27 * void setup() *
28 * { *
29 * servo.attach(10); *
30 * } *
31 * *
32 * This will attach a servo to pin 10, which is the argument of the attach function. *
33 * *
34 * The servo pulse widths are normally around 500 us for 0 deg and 2500 us for 180 deg. *
35 * The default values in this library are 1472 and 2400 us - giving a work area of *
36 * ~90-180deg. These values can be redefined to fit your servos specifications by calling *
37 * the setMaximumPulse and SetMinimumPulse functions. However, because of running the *
38 * stepper algorithm simultaniously with the servo, there is a risk of twitching if *
39 * using lower values than the 1500 us. *
40 * *
41 * example: *
42 * *
43 * uStepperServo servo; *
44 * *
45 * void setup() *
46 * { *
47 * servo.attach(10); *
48 * servo.SetMaximumPulse(2400); *
49 * servo.SetMinimumPulse(1500);//Should be kept above 1500!! *
50 * } *
51 * *
52 * To apply the pulses to the attached servos, the refresh function should be called *
53 * periodically at a rate of 20-50 Hz, i.e. every 50-20 ms. Calling the function more *
54 * often than every 20 ms is not a problem for the servo library. *
55 * *
56 * example *
57 * *
58 * uStepperServo servo; *
59 * *
60 * void setup() *
61 * { *
62 * servo.attach(10); *
63 * servo.SetMaximumPulse(2400); *
64 * servo.SetMinimumPulse(1500); //Should be kept above 1500!! *
65 * } *
66 * *
67 * void loop() *
68 * { *
69 * uStepperServo::refresh(); *
70 * } *
71 * After this, the library is ready to control the Servo! *
72 * *
73 *********************************************************************************************
74 * (C) 2020 *
75 * *
76 * uStepper ApS *
77 * www.ustepper.com *
78 * administration@ustepper.com *
79 * *
80 * The code contained in this file is released under the following open source license: *
81 * *
82 * Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International *
83 * *
84 * The code in this file is provided without warranty of any kind - use at own risk! *
85 * neither uStepper ApS nor the author, can be held responsible for any damage *
86 * caused by the use of the code contained in this file ! *
87 * *
88 ********************************************************************************************/
99 #include <uStepperServo.h>
100 
102 
103 
104 
105 uStepperServo::uStepperServo() : pin(0),angle(0xff),pulse(0),min16(92),max16(150),next(0)
106 {
107 
108 }
109 
111 {
112  min16 = t/16;
113 }
114 
116 {
117  max16 = t/16;
118 }
119 
120 uint8_t uStepperServo::attach(int pinArg)
121 {
122  pin = pinArg;
123  angle = 0xff;
124  pulse = 0;
125  next = first;
126  first = this;
127  digitalWrite(pin,0);
128  pinMode(pin,OUTPUT);
129  return 1;
130 }
131 
133 {
134  for ( uStepperServo **p = &first; *p != 0; p = &((*p)->next) ) {
135  if ( *p == this) {
136  *p = this->next;
137  this->next = 0;
138  return;
139  }
140  }
141 }
142 
143 void uStepperServo::write(int angleArg)
144 {
145  if ( angleArg < 0) angleArg = 0;
146  if ( angleArg > 180) angleArg = 180;
147  angle = angleArg;
148  // bleh, have to use longs to prevent overflow, could be tricky if always a 16MHz clock, but not true
149  // That 64L on the end is the TCNT0 prescaler, it will need to change if the clock's prescaler changes,
150  // but then there will likely be an overflow problem, so it will have to be handled by a human.
151  pulse = (min16*16L*clockCyclesPerMicrosecond() + (max16-min16)*(16L*clockCyclesPerMicrosecond())*angle/180L)/64L;
152 }
153 
155 {
156  uint8_t count = 0, i = 0;
157  uint16_t base = 0;
158  uStepperServo *p;
159  static unsigned long lastRefresh = 0;
160  unsigned long m = millis();
161 
162  // if we haven't wrapped millis, and 20ms have not passed, then don't do anything
163  if ( m >= lastRefresh && m < lastRefresh + 20) return;
164  lastRefresh = m;
165 
166  for ( p = first; p != 0; p = p->next ) if ( p->pulse) count++;
167  if ( count == 0) return;
168 
169  // gather all the uStepperServos in an array
170  uStepperServo *s[count];
171  for ( p = first; p != 0; p = p->next ) if ( p->pulse) s[i++] = p;
172 
173  // bubblesort the uStepperServos by pulse time, ascending order
174  for(;;) {
175  uint8_t moved = 0;
176  for ( i = 1; i < count; i++) {
177  if ( s[i]->pulse < s[i-1]->pulse) {
178  uStepperServo *t = s[i];
179  s[i] = s[i-1];
180  s[i-1] = t;
181  moved = 1;
182  }
183  }
184  if ( !moved) break;
185  }
186 
187  // turn on all the pins
188  // Note the timing error here... when you have many uStepperServos going, the
189  // ones at the front will get a pulse that is a few microseconds too long.
190  // Figure about 4uS/uStepperServo after them. This could be compensated, but I feel
191  // it is within the margin of error of software uStepperServos that could catch
192  // an extra interrupt handler at any time.
193 
194  for ( i = 0; i < count; i++) digitalWrite( s[i]->pin, 1);
195 
196  uint8_t start = TCNT0;
197  uint8_t now = start;
198  uint8_t last = now;
199 
200  // Now wait for each pin's time in turn..
201  for ( i = 0; i < count; i++) {
202  uint16_t go = start + s[i]->pulse;// current time + pulse length for specific servo
203 
204  // loop until we reach or pass 'go' time
205  cli();
206  for (;;) {
207  now = TCNT0;
208  if ( now < last) base += 256;//Timer 0 tops at 255, so add 256 on overflow
209  last = now;//update overflow check variable
210 
211  if(base + now >= go - 16)
212  {
213 
214  }
215 
216  if ( base+now > go) {
217  digitalWrite( s[i]->pin,0);
218 
219  break;
220  }
221  }
222  sei();
223  }
224 }
uStepperServo::refresh
static void refresh()
Updates servo output pins.
Definition: uStepperServo.cpp:154
uStepperEncoder::angle
volatile uint16_t angle
Definition: uStepperEncoder.h:209
uStepperServo::attach
uint8_t attach(int pinArg)
Attaches the servo motor to a specific pin.
Definition: uStepperServo.cpp:120
uStepperServo::detach
void detach()
Detaches the servo motor from the uStepper.
Definition: uStepperServo.cpp:132
uStepperServo::write
void write(int angleArg)
Specify angle of servo motor.
Definition: uStepperServo.cpp:143
uStepperServo::setMaximumPulse
void setMaximumPulse(uint16_t t)
Sets the maximum pulse.
Definition: uStepperServo.cpp:115
uStepperServo::setMinimumPulse
void setMinimumPulse(uint16_t t)
Sets the minimum pulse.
Definition: uStepperServo.cpp:110
uStepperServo.h
Function prototypes and definitions for the uStepper Servo library.
uStepperServo::next
class uStepperServo * next
Definition: uStepperServo.h:59
uStepperServo::first
static uStepperServo * first
Definition: uStepperServo.h:62
uStepperServo
Prototype of class for ustepper servo.
Definition: uStepperServo.h:44
uStepperServo::uStepperServo
uStepperServo()
Constructor for servo class.
Definition: uStepperServo.cpp:105
uStepperServo::min16
uint8_t min16
Definition: uStepperServo.h:53
uStepperServo::pin
uint8_t pin
Definition: uStepperServo.h:47
uStepperServo::max16
uint8_t max16
Definition: uStepperServo.h:55
uStepperServo::angle
uint8_t angle
Definition: uStepperServo.h:49
uStepperServo::pulse
uint16_t pulse
Definition: uStepperServo.h:51