Accessories
Arduino for motors and lights library.
AccessoryServo.cpp
1 /*************************************************************
2 project: <Accessories>
3 author: <Thierry PARIS>
4 description: <Class for a servo accessory>
5 *************************************************************/
6 
7 #include "Accessories.h"
8 
9 #ifndef NO_SERVO
10 #include "ActionsStack.hpp"
11 
13 {
14  this->pPort = NULL;
15  this->minimumPosition = 0;
16  this->maximumPosition = 180;
17  this->powerState = PowerNoAction;
18 }
19 
20 void AccessoryServo::begin(Port *inpPort, unsigned long inDurationMilli,
21  int inMinimumPosition, int inMaximumPosition, int inMovingPositionsNumber)
22 {
23  this->pPort = inpPort;
24 
25  this->SetDuration(inDurationMilli);
26  Accessory::begin(STATE_NONE);
27  this->minimumPosition = inMinimumPosition;
28  this->maximumPosition = inMaximumPosition;
29  this->prevState = STATE_NONE;
30  inpPort->beginByAccessory(this->minimumPosition);
31  this->prevState = MINIMUM;
32  this->currentPosition = this->minimumPosition;
33  this->targetPosition = -1;
34  this->targetSpeed = 0;
35  this->powerCommandPin = (GPIO_pin_t)DP_INVALID;
36  this->powerDelay = 0;
37  this->powerState = PowerNoAction;
38  this->AdjustMovingPositionsSize(inMovingPositionsNumber);
39 }
40 
41 void AccessoryServo::SetPowerCommand(int inPin, unsigned long inDelay)
42 {
43  this->powerCommandPin = Arduino_to_GPIO_pin(inPin);
44 
45  pinMode2f(this->powerCommandPin, OUTPUT);
46  digitalWrite2f(this->powerCommandPin, HIGH);
47 
48  this->powerDelay = inDelay;
49 }
50 
52 {
53 #ifdef ACCESSORIES_DEBUG_MODE
54  Serial.println(F("AccessoryServo MoveMinimum()"));
55 #endif
56 
57  this->MovePosition(this->minimumPosition);
58 }
59 
61 {
62 #ifdef ACCESSORIES_DEBUG_MODE
63  Serial.println(F("AccessoryServo MoveMaximum()"));
64 #endif
65 
66  this->MovePosition(this->maximumPosition);
67 }
68 
70 {
71  if (this->IsActionDelayPending())
72  return this->GetState();
73 
74 #ifdef ACCESSORIES_DEBUG_MODE
75  Serial.print(F("AccessoryServo MoveToggle() : "));
76 #endif
77 
78  if (this->prevState == MAXIMUM)
79  this->MovePosition(this->minimumPosition);
80  else
81  this->MovePosition(this->maximumPosition);
82 
83  return this->GetState();
84 }
85 
86 void AccessoryServo::SetState(ACC_STATE inState)
87 {
88 #ifdef ACCESSORIES_DEBUG_MODE
89  Serial.print(F("AccessoryServo SetState("));
90 #endif
91 
92  switch (inState)
93  {
94  case MINIMUM:
95 #ifdef ACCESSORIES_DEBUG_MODE
96  Serial.println(F("MINIMUM)"));
97 #endif
98  this->MovePosition(this->minimumPosition);
99  break;
100 
101  case MAXIMUM:
102 #ifdef ACCESSORIES_DEBUG_MODE
103  Serial.println(F("MAXIMUM)"));
104 #endif
105  this->MovePosition(this->maximumPosition);
106  break;
107 
108  default:
109  this->ResetAction();
110  }
111 }
112 
113 void AccessoryServo::Move(unsigned long inId)
114 {
115 #ifdef ACCESSORIES_DEBUG_MODE
116  Serial.println(F("AccessoryServo Move()"));
117 #endif
118 
119  if (this->GetMovingPositionSize() == 1)
120  {
121  this->MoveToggle();
122  return;
123  }
124 
125  this->SetLastMovingPosition(this->IndexOfMovingPosition(inId));
126  int position = this->GetMovingPosition(inId);
127 
128  if (position == MINIMUM || position == MAXIMUM)
129  this->SetState((ACC_STATE)position);
130  else
131  this->MovePosition(position);
132 }
133 
134 void AccessoryServo::MovePosition(int inPosition)
135 {
137  return;
138 
139  this->InternalMovePosition(inPosition);
140 }
141 
142 void AccessoryServo::Event(unsigned long inId, ACCESSORIES_EVENT_TYPE inEvent, int inData)
143 {
144  switch (inEvent)
145  {
146  case ACCESSORIES_EVENT_TOGGLE:
147  this->Move(inId);
148  break;
149 
150  case ACCESSORIES_EVENT_MOVE:
151  switch (inData)
152  {
153  case ACCESSORIES_MOVE_STRAIGHT:
154  case ACCESSORIES_MOVE_TOP:
155  case ACCESSORIES_MOVE_LEFT:
156  this->MoveMinimum();
157  break;
158  case ACCESSORIES_MOVE_DIVERGE:
159  case ACCESSORIES_MOVE_BOTTOM:
160  case ACCESSORIES_MOVE_RIGHT:
161  this->MoveMaximum();
162  break;
163  case ACCESSORIES_MOVE_OFF:
164  case ACCESSORIES_MOVE_STOP:
165  this->pPort->MoveStop();
166  this->ResetAction();
167  this->SetStateRaw(STOP);
168  break;
169  case ACCESSORIES_MOVE_MORE:
170  case ACCESSORIES_MOVE_LESS:
171  {
172  int pos = this->currentPosition + inData;
173  if (pos > this->maximumPosition)
174  pos = this->maximumPosition;
175  if (pos < this->minimumPosition)
176  pos = this->minimumPosition;
177  this->MovePosition(pos);
178  }
179  break;
180  }
181  break;
182 
183  case ACCESSORIES_EVENT_MOVEPOSITION:
184  this->MovePosition(inData);
185  break;
186 
187  case ACCESSORIES_EVENT_MOVEPOSITIONID:
188  this->Move(inId);
189  break;
190 
191  case ACCESSORIES_EVENT_MOVEPOSITIONINDEX:
192  this->Move(this->GetMovingPositionIdByIndex(inData));
193  break;
194 
195  case ACCESSORIES_EVENT_SETSPEED:
196  this->pPort->SetSpeed(inData);
197  break;
198 
199  default:
200  break;
201  }
202 }
203 
204 void AccessoryServo::InternalMovePosition(int inPosition)
205 {
206  if (this->currentPosition == inPosition)
207  {
208 #ifdef ACCESSORIES_DEBUG_MODE
209  Serial.println(F("AccessoryServo::MovePosition : same position - no move!"));
210 #endif
211  return;
212  }
213 
214 #ifdef ACCESSORIES_DEBUG_MODE
215  Serial.println(F("AccessoryServo InternalMovePosition()"));
216 #endif
217 
218  MovementSpeed speed = this->GetMovementSpeed();
219 
220  switch (speed)
221  {
222  case ServoFast:
223  this->pPort->MovePosition(this->GetDuration(), inPosition);
224  this->currentPosition = inPosition;
225  break;
226 
227  case ServoAlone:
228  for (int pos = this->currentPosition; pos < inPosition; pos++)
229  this->pPort->MovePosition(this->GetDuration(), pos);
230  this->currentPosition = inPosition;
231  break;
232 
233  case ServoActionStack:
234  StartAction();
235  this->targetPosition = inPosition;
236  this->targetSpeed = (this->currentPosition < this->targetPosition) ? 1 : -1;
238  break;
239 
240  case ServoSlow:
241  StartAction();
242  this->targetPosition = inPosition;
243  this->targetSpeed = (this->currentPosition < this->targetPosition) ? 1 : -1;
244  break;
245  }
246 
247  if (inPosition == this->minimumPosition)
248  {
249  this->SetStateRaw(MINIMUM);
250  this->prevState = MINIMUM;
251  }
252  else
253  if (inPosition == this->maximumPosition)
254  {
255  this->SetStateRaw(MAXIMUM);
256  this->prevState = MAXIMUM;
257  }
258  else
259  {
260  this->SetStateRaw(STATE_NONE);
261  this->prevState = STATE_NONE;
262  }
263 }
264 
265 bool AccessoryServo::ActionEnded()
266 {
267  if (this->GetActionStartingMillis() <= 0)
268  return true;
269 
270  if (this->powerCommandPin == DP_INVALID || this->powerState == PowerRunning)
271  {
272  //if (millis() - this->GetActionStartingMillis() > this->GetDuration())
273  {
274  if (this->targetSpeed == 0)
275  {
276  // Abort
277  this->targetPosition = -1;
279  if (this->powerState == PowerRunning && this->powerCommandPin != DP_INVALID)
280  {
281 #ifdef ACCESSORIES_DEBUG_MODE
282  Serial.println(F("AccessoryServo after running"));
283 #endif
284  this->powerState = PowerAfterRunning;
285  StartAction();
286  return false;
287  }
288 
289  this->ResetAction();
290  return true;
291  }
292 
293  this->currentPosition += this->targetSpeed;
294  this->pPort->MovePosition(this->GetDuration(), this->currentPosition);
295 
296  if (this->targetPosition == this->currentPosition)
297  {
298  this->targetPosition = -1;
299  this->targetSpeed = 0;
301  if (this->powerState == PowerRunning && this->powerCommandPin != DP_INVALID)
302  {
303 #ifdef ACCESSORIES_DEBUG_MODE
304  Serial.println(F("AccessoryServo after running"));
305 #endif
306  this->powerState = PowerAfterRunning;
307  StartAction();
308  return false;
309  }
310  this->ResetAction();
311  return true;
312  }
313  StartAction();
314  }
315  }
316  else
317  {
318  if (this->powerState == PowerNoAction)
319  {
320 #ifdef ACCESSORIES_DEBUG_MODE
321  Serial.println(F("AccessoryServo start power command"));
322 #endif
323  digitalWrite2f(this->powerCommandPin, LOW);
324  this->powerState = PowerBeforeRunning;
325  StartAction();
326  return false;
327  }
328 
329  // Action time elapsed, do the next work...
330  if (millis() - this->GetActionStartingMillis() > this->powerDelay)
331  {
332  if (this->powerState == PowerBeforeRunning)
333  {
334 #ifdef ACCESSORIES_DEBUG_MODE
335  Serial.println(F("AccessoryServo running"));
336 #endif
337  this->powerState = PowerRunning;
338  StartAction();
339  return false;
340  }
341 
342  if (this->powerState == PowerAfterRunning)
343  {
344 #ifdef ACCESSORIES_DEBUG_MODE
345  Serial.println(F("AccessoryServo end running"));
346 #endif
347  this->powerState = PowerNoAction;
348  digitalWrite2f(this->powerCommandPin, HIGH);
349  this->ResetAction();
350  return true;
351  }
352  }
353  }
354 
355  return false;
356 }
357 
358 MovementSpeed AccessoryServo::GetMovementSpeed() const
359 {
360  if (this->GetDuration() == 0)
361  return ServoFast;
362  if (this->GetDuration() < 6)
363  return ServoAlone;
364  if (this->GetDuration() < 20)
365  return ServoActionStack;
366  return ServoSlow;
367 }
368 
369 #ifndef NO_EEPROM
370 int AccessoryServo::EEPROMLoad(int inPos)
371 {
372  inPos = this->Accessory::EEPROMLoad(inPos);
373 
374  if (this->powerCommandPin != DP_INVALID)
375  digitalWrite2f(this->powerCommandPin, HIGH);
376  this->pPort->MovePosition(0, this->currentPosition);
377  delay(200); //can be done here because we are in setup timing...
378  if (this->powerCommandPin != DP_INVALID)
379  digitalWrite2f(this->powerCommandPin, LOW);
380 
381  return inPos;
382 }
383 #endif
384 
385 #ifdef ACCESSORIES_PRINT_ACCESSORIES
386 void AccessoryServo::printAccessory()
387 {
388  Serial.print(F(" Servo : "));
389  for (int i = 0; i < this->GetMovingPositionSize(); i++)
390  {
391  Serial.print(F(" / ID "));
392  Serial.print(this->GetMovingPositionIdByIndex(i));
393  Serial.print(F(" - "));
394  Serial.print(this->GetMovingPositionByIndex(i));
395  }
396 
397  if (this->GetPort() != NULL)
398  {
399  Serial.print(F(" / Speed "));
400  Serial.print(this->GetPort()->GetSpeed());
401  }
402  if (this->powerCommandPin != (GPIO_pin_t)DP_INVALID)
403  {
404  Serial.print(F(" / PowerPin "));
405  Port::printPortPin(this->powerCommandPin, DIGITAL);
406  Serial.print(F(" / PowerDelay "));
407  Serial.print(this->powerDelay);
408  }
409  Serial.print(F(" / Delay "));
410  Serial.print(this->GetDebounceDelay());
411  Serial.print(F("ms / "));
412  if (this->GetPort() != NULL)
413  this->GetPort()->printPort();
414  Serial.println(F(" "));
415 }
416 #endif
417 
418 #endif
unsigned int GetDebounceDelay() const
Definition: Accessory.hpp:243
Port * GetPort() const
Definition: Accessory.hpp:185
int GetMovingPositionByIndex(int inIndex) const
Definition: Accessory.hpp:307
int EEPROMLoad(int inPos)
unsigned long GetActionStartingMillis() const
Definition: Accessory.hpp:366
virtual void StartAction()
Definition: Accessory.cpp:77
unsigned long GetDuration() const
Definition: Accessory.hpp:254
MovementSpeed GetMovementSpeed() const
virtual int SetSpeed(int inSpeed)
Definition: Port.cpp:108
unsigned long GetMovingPositionIdByIndex(int inIndex) const
Definition: Accessory.hpp:313
void SetStateRaw(ACC_STATE inNewState)
Definition: Accessory.cpp:165
virtual void MoveStop()
Definition: Port.hpp:167
static bool FillingStack
uint8_t IndexOfMovingPosition(unsigned long inId) const
Definition: Accessory.cpp:59
void SetLastMovingPosition(uint8_t inLastPositionIndex)
Definition: Accessory.cpp:157
const uint8_t GetMovingPositionSize() const
Definition: Accessory.hpp:323
void Move(unsigned long inId)
virtual void ResetAction()
Definition: Accessory.hpp:385
ACC_STATE GetState() const
Definition: Accessory.hpp:209
bool IsActionDelayPending() const
Definition: Accessory.hpp:398
virtual void MovePosition(unsigned long inDuration, int inEndPosition)
Definition: Port.hpp:173
void Event(unsigned long inId, ACCESSORIES_EVENT_TYPE inEvent = ACCESSORIES_EVENT_MOVEPOSITIONID, int inData = 0)
void SetPowerCommand(int inPin, unsigned long inDelay = 100)
int GetMovingPosition(unsigned long inId) const
Definition: Accessory.cpp:68
void AdjustMovingPositionsSize(uint8_t inNewSize)
Definition: Accessory.cpp:29
virtual void beginByAccessory(int inStartingPosition)
Definition: Port.hpp:105
void begin(ACC_STATE inStartingState = STATE_NONE)
Definition: Accessory.hpp:180
void SetDuration(unsigned long inDuration)
Definition: Accessory.hpp:344
virtual int EEPROMLoad(int inPos)
Definition: Port.hpp:61
void begin(Port *inpPort, unsigned long inDurationMilli = 0, int inMinimumPosition = 0, int inMaximumPosition = 180, int inMovingPositionsNumber = 1)
ACC_STATE MoveToggle()
ACC_STATE prevState
Definition: Accessory.hpp:170
Port * pPort
Definition: Accessory.hpp:167
void MovePosition(int inPosition)