egoShield
egoShield.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: egoShield.cpp *
3 * Version: 0.1.0 *
4 * Date: September 4th, 2017 *
5 * Author: Mogens Groth Nicolaisen *
6 * *
7 *********************************************************************************************
8 * egoShield class *
9 * *
10 * This file contains the implementation of the class methods, incorporated in the *
11 * egoShield Arduino library. The library is used by instantiating an egoShield object *
12 * by calling of the overloaded constructor: *
13 * *
14 * example: *
15 * *
16 * egoShield ego; *
17 * *
18 * The instantiation above creates an egoShield object *
19 * after instantiation of the object, the object setup function should be called within *
20 * Arduino's setup function, and the object loop function should be run within the Arduino's *
21 * loop function: *
22 * *
23 * example: *
24 * *
25 * egoShield ego; *
26 * *
27 * void setup() *
28 * { *
29 * ego.setup(); *
30 * } *
31 * *
32 * void loop() *
33 * { *
34 * ego.loop(); *
35 * } *
36 * *
37 * *
38 *********************************************************************************************
39 * (C) 2017 *
40 * *
41 * ON Development IVS *
42 * www.on-development.com *
43 * administration@on-development.com *
44 * *
45 * The code contained in this file is released under the following open source license: *
46 * *
47 * Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International *
48 * *
49 * The code in this file is provided without warranty of any kind - use at own risk! *
50 * neither ON Development IVS nor the author, can be held responsible for any damage *
51 * caused by the use of the code contained in this file ! *
52 * *
53 ********************************************************************************************/
64 #include "egoShield.h"
65 
67 {
68  u8g2 = new U8G2_SSD1306_128X64_NONAME_1_4W_SW_SPI(U8G2_R0, /* clock=*/ 11, /* data=*/ 9, /* cs=*/ U8X8_PIN_NONE, /* dc=*/ 2, /* reset=*/ 10);
69 }
70 
71 void egoShield::setup(uint16_t acc, uint16_t vel, uint8_t uStep, uint16_t fTol, uint16_t fHys, float P, float I, float D)
72 {
73  this->acceleration = acc;
74  this->velocity = vel;
75  this->microStepping = uStep;
76  this->faultTolerance = fTol;
77  this->faultHysteresis = fHys;
78  this->pTerm = P;
79  this->iTerm = I;
80  this->dTerm = D;
81  stepper.setup(PID,this->microStepping,this->faultTolerance,this->faultHysteresis,this->pTerm,this->iTerm,this->dTerm,1);
82  stepper.encoder.setHome();
83  pidFlag = 1;//enable PID
84  //Serial.begin(9600);
85  pinMode(FWBT ,INPUT);
86  pinMode(PLBT ,INPUT);
87  pinMode(RECBT ,INPUT);
88  pinMode(BWBT ,INPUT);
89  digitalWrite(FWBT ,HIGH);//pull-up
90  digitalWrite(PLBT ,HIGH);//pull-up
91  digitalWrite(RECBT ,HIGH);//pull-up
92  digitalWrite(BWBT ,HIGH);//pull-up
93  setPoint = stepper.encoder.getAngleMoved();//set manual move setpoint to current position
94  u8g2->begin();//start display
95  this->startPage();//show startpage
96  delay(2000);//for 2 seconds
97  state = 'a';//start in idle
98 }
99 
100 void egoShield::loop(void)
101 {
102  this->inputs();//check buttons
103  switch (state)
104  {
105  case 'a'://if we are in idle
106  idleMode();
107  break;
108 
109  case 'b'://if we are in play
110  playMode();
111  break;
112 
113  case 'c'://if we are in record
114  recordMode();
115  break;
116 
117  case 'd'://in pause
118  pauseMode();
119  break;
120  }
121 }
122 
124 {
125  this->idlePage(pidFlag,stepper.encoder.getAngleMoved());
126  if(play == 2)//if play/stop/pause is pressed for long time, invert the pid mode
127  {
128  if(pidFlag == 0)
129  {
130  pidFlag = 1;
131  stepper.setup(PID,this->microStepping,this->faultTolerance,this->faultHysteresis,this->pTerm,this->iTerm,this->dTerm,0);//pause PID to allow manual movement
132  }
133  else
134  {
135  pidFlag = 0;
136  stepper.setup(NORMAL,this->microStepping,this->faultTolerance,this->faultHysteresis,this->pTerm,this->iTerm,this->dTerm,0);//pause PID to allow manual movement
137  stepper.hardStop(SOFT);
138  }
139  }
140  else if(fw == 1)//if manual forward signal
141  {
142  setPoint = stepper.encoder.getAngleMoved();
143  setPoint +=5;
144  stepper.moveToAngle(setPoint,0);//move 5deg
145  while(stepper.getMotorState())
146  {
147  delay(1);
148  }
149  }
150  else if(fw == 2)//if manual forward signal long
151  {
152  this->fastForward();
153  }
154  else if(bw == 1)//if manual backward signal
155  {
156  setPoint = stepper.encoder.getAngleMoved();
157  setPoint -=5;
158  stepper.moveToAngle(setPoint,0);
159  while(stepper.getMotorState())
160  {
161  delay(1);
162  }
163  }
164  else if(bw == 2)//if manual backward signal long
165  {
166  this->fastBackward();
167  }
168  else if(rec == 2)
169  {
170  state = 'c';
171  }
172  else if(play == 1)//we want to play sequence when doing a short press
173  {
174  state = 'b';
175  }
176 }
177 
179 {
180  stepper.setMaxVelocity(this->velocity);
181  stepper.setMaxAcceleration(this->acceleration);
183  if(loopMode && place > endmove)
184  {
185  place=0;
186  }
187  else if(place > endmove)//If we are at the end move
188  {
189  place = 0;//reset array counter
190  state = 'a';
191  setPoint = pos[endmove];
192  }
193  else
194  {
195  stepper.moveToAngle(pos[place],0);
196  place++;//increment array counter
197  while(stepper.getMotorState())
198  {
199  this->inputs();//check inputs
200  if(play == 2)//if play/stop/pause is pressed again for long time, stop
201  {
202  place = 0;//reset array counter
203  loopMode = 0;
204  state = 'a';
205  setPoint = stepper.encoder.getAngleMoved();
206  }
207  else if(rec == 1)
208  {
209  state = 'd';//pause
210  }
211  else if(fw == 2)//loop mode start
212  {
213  loopMode = 1;
214  }
215  else if(bw == 2)//loop mode stop
216  {
217  loopMode = 0;
218  }
219  }
220  }
221  stepper.setMaxVelocity(1000);
222  stepper.setMaxAcceleration(1500);
223 }
224 
226 {
227  this->recordPage(pidFlag,0,place,stepper.encoder.getAngleMoved());
228  //if(pidFlag)
229  //{
230  if(fw == 1)//if manual forward signal
231  {
232  setPoint = stepper.encoder.getAngleMoved();
233  setPoint +=5;
234  stepper.moveToAngle(setPoint,0);//move 5deg
235  while(stepper.getMotorState())
236  {
237  delay(1);
238  }
239  }
240  else if(fw == 2)//if manual forward signal long
241  {
242  this->fastForward();
243  }
244  else if(bw == 1)//if manual backward signal
245  {
246  setPoint = stepper.encoder.getAngleMoved();
247  setPoint -=5;
248  stepper.moveToAngle(setPoint,0);
249  while(stepper.getMotorState())
250  {
251  delay(1);
252  }
253  }
254  else if(bw == 2)//if manual forward signal long
255  {
256  this->fastBackward();
257  }
258  //}
259  if(rec == 1)//record position
260  {
261  if(record == 0)//If we were not recording before
262  {
263  stepper.encoder.setHome();//Set current position as home
264  setPoint = 0;
265  place = 0;//Reset the array counter
266  record = 1;//Record flag
267  }
268  this->recordPage(pidFlag,1,place,stepper.encoder.getAngleMoved());
269  delay(500);
270  if(record == 1)//If we have initialized recording
271  {
272  pos[place] = stepper.encoder.getAngleMoved();//Save current position
273  place++;//Increment array counter
274  if(place>CNT)
275  {
276  place=0;
277  }
278  }
279  }
280  else if(play == 2)//stop pressed
281  {
282  endmove = place-1;//set the endmove to the current position
283  place = 0;//reset array counter
284  record = 0;//reset record flag
285  state = 'a';//stop state
286  setPoint = stepper.encoder.getAngleMoved();
287  }
288 }
289 
291 {
293  if(play == 1)//unpause
294  {
295  state = 'b';
296  }
297  else if(play == 2)//stop
298  {
299  state = 'a';
300  setPoint = stepper.encoder.getAngleMoved();
301  }
302 }
304 {
305  fw = this->buttonState(FWBT,0);
306  play = this->buttonState(PLBT,1);
307  rec = this->buttonState(RECBT,2);
308  bw = this->buttonState(BWBT,3);
309 }
310 
311 uint8_t egoShield::buttonState(uint8_t button, uint8_t nmbr)
312 {
313  uint32_t count = 0;
314  uint8_t push = 0;
315  if(digitalRead(button)==0)
316  {
317  while(digitalRead(button)==0 && longPushFlag[nmbr] == 0)
318  {
319  count++;
320  if(count>=5000)//short press
321  {
322  push = 1;
323  }
324  if(count>=100000)//long press
325  {
326  push = 2;
327  longPushFlag[nmbr] = 1;
328  }
329  }
330  }
331  else//no press
332  {
333  push = 0;
334  longPushFlag[nmbr] = 0;
335  }
336  return push;
337 }
338 
340 {
341  setPoint = stepper.encoder.getAngleMoved();
342  setPoint +=10;
343  stepper.moveToAngle(setPoint,0);
344  while(digitalRead(FWBT)==0)
345  {
346  setPoint +=10;
347  stepper.moveToAngle(setPoint,0);
348  this->idlePage(pidFlag,stepper.encoder.getAngleMoved());
349  }
350  while(stepper.getMotorState())
351  {
352  delay(1);
353  }
354 }
355 
357 {
358  setPoint = stepper.encoder.getAngleMoved();
359  setPoint -=10;
360  stepper.moveToAngle(setPoint,0);
361  while(digitalRead(BWBT)==0)
362  {
363  setPoint -=10;
364  stepper.moveToAngle(setPoint,0);
365  this->idlePage(pidFlag,stepper.encoder.getAngleMoved());
366  }
367  while(stepper.getMotorState())
368  {
369  delay(1);
370  }
371 }
372 
374 {
375  u8g2->firstPage();
376  do {
377  u8g2->drawXBM(19, 20, logo_width, logo_height, logo_bits);
378  } while ( u8g2->nextPage() );
379 }
380 
381 void egoShield::idlePage(bool pidMode, float pos)
382 {
383  char buf[16];
384  String sBuf;
385 
386  u8g2->firstPage();
387  do {
388  u8g2->drawBox(1, 1, 128, 12);
389  u8g2->drawBox(1, 48, 128, 68);
390  u8g2->setFontMode(0);
391  u8g2->setDrawColor(0);
392  u8g2->setFontDirection(0);
393  u8g2->setFont(u8g2_font_6x10_tf);
394 
395  //Bottom bar
396  u8g2->drawXBM(5, 51, en_width, en_height, bw_bits);
397  u8g2->drawXBM(112, 51, en_width, en_height, fw_bits);
398  u8g2->drawXBM(32, 50, play_width, play_height, play_bits);
399  u8g2->drawXBM(43, 51, tt_width, tt_height, stop_bits);
400  u8g2->drawXBM(71, 51, tt_width, tt_height, rec_bits);
401  u8g2->drawXBM(85, 51, tt_width, tt_height, pse_bits);
402 
403  //Mode
404  u8g2->drawStr(2,10,"Idle");
405  if(pidMode)
406  {
407  u8g2->drawStr(45,10,"PID ON");
408  }
409  else
410  {
411  u8g2->drawStr(45,10,"PID OFF");
412  }
413  u8g2->setFontMode(1);
414  u8g2->setDrawColor(1);
415  sBuf = "Encoder: ";
416  sBuf += (int32_t)pos;
417  sBuf += (char)176;
418  sBuf.toCharArray(buf, 16);
419  u8g2->drawStr(2,35,buf);
420  } while ( u8g2->nextPage() );
421 }
422 
423 void egoShield::recordPage(bool pidMode, bool recorded, uint8_t index, float pos)
424 {
425  char buf[22];//char array buffer
426  String sBuf;
427 
428  u8g2->firstPage();
429  do {
430  u8g2->drawBox(1, 1, 128, 12);
431  u8g2->drawBox(1, 48, 128, 68);
432  u8g2->setFontMode(0);
433  u8g2->setDrawColor(0);
434  u8g2->setFontDirection(0);
435  u8g2->setFont(u8g2_font_6x10_tf);
436 
437  u8g2->drawXBM(5, 51, en_width, en_height, bw_bits);
438  u8g2->drawXBM(112, 51, en_width, en_height, fw_bits);
439  u8g2->drawXBM(38, 51, tt_width, tt_height, stop_bits);
440  u8g2->drawXBM(76, 51, tt_width, tt_height, rec_bits);
441 
442  //Mode
443  u8g2->drawStr(2,10,"Record");
444  if(pidMode)
445  {
446  u8g2->drawStr(45,10,"PID ON");
447  }
448  else
449  {
450  u8g2->drawStr(45,10,"PID OFF");
451  }
452  u8g2->setFontMode(1);
453  u8g2->setDrawColor(1);
454  if(recorded)
455  {
456  sBuf = "Position ";
457  sBuf += index;
458  sBuf += " recorded";
459  sBuf.toCharArray(buf, 22);
460  u8g2->drawStr(2,35,buf);
461  }
462  else
463  {
464  sBuf = "Encoder: ";
465  sBuf += (int32_t)pos;
466  sBuf += (char)176;
467  sBuf.toCharArray(buf, 16);
468  u8g2->drawStr(2,35,buf);
469  }
470  } while ( u8g2->nextPage() );
471 }
472 
473 void egoShield::playPage(bool loopMode, bool pidMode, uint8_t index)
474 {
475  char buf[3];//char array buffer
476 
477  u8g2->firstPage();
478  do {
479  u8g2->drawBox(1, 1, 128, 12);
480  u8g2->drawBox(1, 48, 128, 68);
481  u8g2->setFontMode(0);
482  u8g2->setDrawColor(0);
483  u8g2->setFontDirection(0);
484  u8g2->setFont(u8g2_font_6x10_tf);
485 
486  if(loopMode)
487  {
488  u8g2->drawXBM(110, 2, loop_width, loop_height, loop_bits);
489  }
490 
491  //Bottom bar
492  u8g2->drawXBM(5, 51, en_width, en_height, bw_bits);
493  u8g2->drawXBM(112, 51, en_width, en_height, fw_bits);
494  u8g2->drawXBM(32, 50, play_width, play_height, play_bits);
495  u8g2->drawXBM(43, 51, tt_width, tt_height, stop_bits);
496  u8g2->drawXBM(77, 51, tt_width, tt_height, pse_bits);
497 
498  //Mode
499  u8g2->drawStr(2,10,"Play");
500  if(pidMode)
501  {
502  u8g2->drawStr(45,10,"PID ON");
503  }
504  else
505  {
506  u8g2->drawStr(45,10,"PID OFF");
507  }
508  u8g2->setFontMode(1);
509  u8g2->setDrawColor(1);
510  u8g2->drawStr(2,35,"Moving to pos");
511  String(index).toCharArray(buf, 3);
512  u8g2->drawStr(90,35,buf);
513  } while ( u8g2->nextPage() );
514 }
515 
516 void egoShield::pausePage(bool loopMode, bool pidMode, uint8_t index)
517 {
518  char buf[3];//char array buffer
519 
520  u8g2->firstPage();
521  do {
522  u8g2->drawBox(1, 1, 128, 12);
523  u8g2->drawBox(1, 48, 128, 68);
524  u8g2->setFontMode(0);
525  u8g2->setDrawColor(0);
526  u8g2->setFontDirection(0);
527  u8g2->setFont(u8g2_font_6x10_tf);
528 
529  if(loopMode)
530  {
531  u8g2->drawXBM(110, 2, loop_width, loop_height, loop_bits);
532  }
533 
534  //Bottom bar
535  u8g2->drawXBM(32, 50, play_width, play_height, play_bits);
536  u8g2->drawXBM(43, 51, tt_width, tt_height, stop_bits);
537 
538  //Mode
539  u8g2->drawStr(2,10,"Pause");
540  if(pidMode)
541  {
542  u8g2->drawStr(45,10,"PID ON");
543  }
544  else
545  {
546  u8g2->drawStr(45,10,"PID OFF");
547  }
548  u8g2->setFontMode(1);
549  u8g2->setDrawColor(1);
550  u8g2->drawStr(2,35,"Paused at pos");
551  String(index).toCharArray(buf, 3);
552  u8g2->drawStr(90,35,buf);
553  } while ( u8g2->nextPage() );
554 }
uint8_t place
Definition: egoShield.h:224
uint8_t endmove
Definition: egoShield.h:226
void loop(void)
Contains the main logic of the shield functionality, e.g. transition between states (idle...
Definition: egoShield.cpp:100
void recordMode(void)
Holds the record logic, showing the record page and recording positions from user input...
Definition: egoShield.cpp:225
uStepper stepper
Creates an uStepper instance.
Definition: egoShield.h:219
float dTerm
Definition: egoShield.h:264
bool record
Definition: egoShield.h:232
void idleMode(void)
Holds the idle logic; page to show, what buttons to enable etc.
Definition: egoShield.cpp:123
uint8_t fw
Definition: egoShield.h:244
char state
Definition: egoShield.h:238
float iTerm
Definition: egoShield.h:262
void setup(uint16_t acc=1500, uint16_t vel=1000, uint8_t uStep=SIXTEEN, uint16_t fTol=10, uint16_t fHys=5, float P=1.0, float I=0.02, float D=0.006)
Initializes buttons, OLED, uStepper and BT-module.
Definition: egoShield.cpp:71
uint8_t play
Definition: egoShield.h:242
void recordPage(bool pidMode, bool recorded, uint8_t index, float pos)
Holds the code for the record page of the OLED.
Definition: egoShield.cpp:423
uint16_t acceleration
Definition: egoShield.h:250
void idlePage(bool pidMode, float pos)
Holds the code for the idle page of the OLED.
Definition: egoShield.cpp:381
float pTerm
Definition: egoShield.h:260
U8G2_SSD1306_128X64_NONAME_1_4W_SW_SPI * u8g2
Definition: egoShield.h:222
void playMode(void)
Holds the play logic, showing play page and running the recorded sequence.
Definition: egoShield.cpp:178
uint8_t microStepping
Definition: egoShield.h:254
float pos[CNT]
Definition: egoShield.h:228
void pauseMode(void)
Holds the pause logic, showing the pause page and pausing the playing of a sequence.
Definition: egoShield.cpp:290
bool longPushFlag[4]
Definition: egoShield.h:236
Function prototypes and definitions for the egoShield library.
uint8_t rec
Definition: egoShield.h:240
uint16_t velocity
Definition: egoShield.h:252
void fastForward(void)
Holds the fast forward logic for driving the stepper motor manually with the pushbuttons.
Definition: egoShield.cpp:339
void inputs(void)
Reads the four buttons and writes their value; no push, short push or long push, to global variables...
Definition: egoShield.cpp:303
void fastBackward(void)
Holds the fast backward logic for driving the stepper motor manually with the pushbuttons.
Definition: egoShield.cpp:356
bool pidFlag
Definition: egoShield.h:230
bool loopMode
Definition: egoShield.h:234
uint16_t faultTolerance
Definition: egoShield.h:256
void startPage(void)
Holds the code for the start page of the OLED.
Definition: egoShield.cpp:373
void pausePage(bool loopMode, bool pidMode, uint8_t index)
Holds the code for the pause page of the OLED.
Definition: egoShield.cpp:516
float setPoint
Definition: egoShield.h:248
uint16_t faultHysteresis
Definition: egoShield.h:258
uint8_t buttonState(uint8_t button, uint8_t nmbr)
Returns the button state of the appropriate button.
Definition: egoShield.cpp:311
uint8_t bw
Definition: egoShield.h:246
egoShield(void)
Constructor of egoShield class.
Definition: egoShield.cpp:66
void playPage(bool loopMode, bool pidMode, uint8_t index)
Holds the code for the play page of the OLED.
Definition: egoShield.cpp:473