uStepper
uStepper.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepper.cpp *
3 * Version: 1.0.0 *
4 * date: October 13th, 2016 *
5 * Author: Thomas Hørring Olsen *
6 * *
7 *********************************************************************************************
8 * uStepper class *
9 * *
10 * This file contains the implementation of the class methods, incorporated in the *
11 * uStepper arduino library. The library is used by instantiating an uStepper object *
12 * by calling either of the two overloaded constructors: *
13 * *
14 * example: *
15 * *
16 * uStepper stepper; *
17 * *
18 * OR *
19 * *
20 * uStepper stepper(500, 2000); *
21 * *
22 * The first instantiation above creates a uStepper object with default acceleration *
23 * and maximum speed (1000 steps/s^2 and 1000steps/s respectively). *
24 * The second instantiation overwrites the default settings of acceleration and *
25 * maximum speed (in this case 500 steps/s^2 and 2000 steps/s, respectively); *
26 * *
27 * after instantiation of the object, the object setup function should be called within *
28 * arduino's setup function: *
29 * *
30 * example: *
31 * *
32 * uStepper stepper; *
33 * *
34 * void setup() *
35 * { *
36 * stepper.setup(); *
37 * } *
38 * *
39 * void loop() *
40 * { *
41 * *
42 * } *
43 * *
44 * After this, the library is ready to control the motor! *
45 * *
46 *********************************************************************************************
47 * (C) 2016 *
48 * *
49 * ON Development IVS *
50 * www.on-development.com *
51 * administration@on-development.com *
52 * *
53 * The code contained in this file is released under the following open source license: *
54 * *
55 * Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International *
56 * *
57 * The code in this file is provided without warranty of any kind - use at own risk! *
58 * neither ON Development IVS nor the author, can be held responsible for any damage *
59 * caused by the use of the code contained in this file ! *
60 * *
61 ********************************************************************************************/
71 #include <uStepper.h>
72 #include <math.h>
73 
74 uStepper *pointer;
75 volatile int32_t *p __attribute__((used));
76 
77 i2cMaster I2C;
78 
79 extern "C" {
80 
81  void interrupt1(void)
82  {
83  pointer->speedValue[1] = pointer->speedValue[0];
84  pointer->speedValue[0] = micros();
85 
86  if((PIND & (0x20))) //CCW
87  {
88  if(pointer->control == 0)
89  {
90  PORTD |= (1 << 7); //Set dir to CCW
91 
92  PORTD |= (1 << 4); //generate step pulse
93 
94  pointer->stepsSinceReset--;
95  PORTD &= ~(1 << 4); //pull step pin low again
96  }
97  pointer->stepCnt--; //DIR is set to CCW, therefore we subtract 1 step from step count (negative values = number of steps in CCW direction from initial postion)
98  }
99  else //CW
100  {
101  if(pointer->control == 0)
102  {
103 
104  PORTD &= ~(1 << 7); //Set dir to CW
105 
106  PORTD |= (1 << 4); //generate step pulse
107  pointer->stepsSinceReset++;
108  PORTD &= ~(1 << 4); //pull step pin low again
109  }
110  pointer->stepCnt++; //DIR is set to CW, therefore we add 1 step to step count (positive values = number of steps in CW direction from initial postion)
111  }
112  }
113 
114  void interrupt0(void)
115  {
116  if(PIND & 0x04)
117  {
118 
119  PORTB |= (1 << 0);
120  }
121  else
122  {
123  PORTB &= ~(1 << 0);
124  }
125  }
126 
127  void TIMER2_COMPA_vect(void)
128  {
129  asm volatile("push r16 \n\t");
130  asm volatile("in r16,0x3F \n\t");
131  asm volatile("push r16 \n\t");
132  asm volatile("push r30 \n\t");
133  asm volatile("push r31 \n\t");
134  asm volatile("lds r30,p \n\t");
135  asm volatile("lds r31,p+1 \n\t");
136 
137  asm volatile("ldd r16,z+24 \n\t");
138  asm volatile("cpi r16,0x01 \n\t");
139  asm volatile("breq _pid \n\t");
140 
141  asm volatile("ldd r16,z+0 \n\t");
142  asm volatile("cpi r16,0x00 \n\t");
143  asm volatile("brne _pid \n\t");
144 
145  asm volatile("ldd r16,z+1 \n\t");
146  asm volatile("cpi r16,0x00 \n\t");
147  asm volatile("brne _pid \n\t");
148 
149  asm volatile("ldd r16,z+2 \n\t");
150  asm volatile("cpi r16,0x00 \n\t");
151  asm volatile("brne _pid \n\t");
152 
153  asm volatile("ldd r16,z+3 \n\t");
154  asm volatile("cpi r16,0x00 \n\t");
155  asm volatile("brne _pid \n\t");
156 
157  asm volatile("subi r30,83 \n\t");
158  asm volatile("sbci r31,0 \n\t");
159 
160  asm volatile("jmp _AccelerationAlgorithm \n\t"); //Execute the acceleration profile algorithm
161 
162  asm volatile("_pid: \n\t");
163  asm volatile("push r0 \n\t");
164  asm volatile("push r1 \n\t");
165  asm volatile("push r2 \n\t");
166  asm volatile("push r3 \n\t");
167  asm volatile("push r4 \n\t");
168  asm volatile("push r5 \n\t");
169  asm volatile("push r6 \n\t");
170  asm volatile("push r7 \n\t");
171  asm volatile("push r8 \n\t");
172  asm volatile("push r9 \n\t");
173  asm volatile("push r10 \n\t");
174  asm volatile("push r11 \n\t");
175  asm volatile("push r12 \n\t");
176  asm volatile("push r13 \n\t");
177  asm volatile("push r14 \n\t");
178  asm volatile("push r15 \n\t");
179  asm volatile("push r17 \n\t");
180  asm volatile("push r18 \n\t");
181  asm volatile("push r19 \n\t");
182  asm volatile("push r20 \n\t");
183  asm volatile("push r21 \n\t");
184  asm volatile("push r22 \n\t");
185  asm volatile("push r23 \n\t");
186  asm volatile("push r24 \n\t");
187  asm volatile("push r25 \n\t");
188  asm volatile("push r26 \n\t");
189  asm volatile("push r27 \n\t");
190  asm volatile("push r28 \n\t");
191  asm volatile("push r29 \n\t");
192 
193  if(pointer->counter < pointer->delay) //This value defines the speed at which the motor rotates when compensating for lost steps. This value should be tweaked to the application
194  {
195  pointer->counter++;
196  }
197  else
198  {
199  PORTD |= (1 << 4);
200  asm volatile("nop \n\t");
201  asm volatile("nop \n\t");
202  asm volatile("nop \n\t");
203  asm volatile("nop \n\t");
204  asm volatile("nop \n\t");
205  asm volatile("nop \n\t");
206  asm volatile("nop \n\t");
207  asm volatile("nop \n\t");
208  asm volatile("nop \n\t");
209  asm volatile("nop \n\t");
210  asm volatile("nop \n\t");
211  asm volatile("nop \n\t");
212  asm volatile("nop \n\t");
213  asm volatile("nop \n\t");
214  asm volatile("nop \n\t");
215  asm volatile("nop \n\t");
216  asm volatile("nop \n\t");
217  PORTD &= ~(1 << 4);
218  pointer->counter = 0;
219  if(pointer->control > 0)
220  {
221  pointer->control--;
222  }
223  else
224  {
225  pointer->control++;
226  }
227 
228  if(!pointer->control && !pointer->state)
229  {
230  pointer->stopTimer();
231  }
232  }
233  asm volatile("pop r29 \n\t");
234  asm volatile("pop r28 \n\t");
235  asm volatile("pop r27 \n\t");
236  asm volatile("pop r26 \n\t");
237  asm volatile("pop r25 \n\t");
238  asm volatile("pop r24 \n\t");
239  asm volatile("pop r23 \n\t");
240  asm volatile("pop r22 \n\t");
241  asm volatile("pop r21 \n\t");
242  asm volatile("pop r20 \n\t");
243  asm volatile("pop r19 \n\t");
244  asm volatile("pop r18 \n\t");
245  asm volatile("pop r17 \n\t");
246  asm volatile("pop r15 \n\t");
247  asm volatile("pop r14 \n\t");
248  asm volatile("pop r13 \n\t");
249  asm volatile("pop r12 \n\t");
250  asm volatile("pop r11 \n\t");
251  asm volatile("pop r10 \n\t");
252  asm volatile("pop r9 \n\t");
253  asm volatile("pop r8 \n\t");
254  asm volatile("pop r7 \n\t");
255  asm volatile("pop r6 \n\t");
256  asm volatile("pop r5 \n\t");
257  asm volatile("pop r4 \n\t");
258  asm volatile("pop r3 \n\t");
259  asm volatile("pop r2 \n\t");
260  asm volatile("pop r1 \n\t");
261  asm volatile("pop r0 \n\t");
262  asm volatile("pop r31 \n\t");
263  asm volatile("pop r30 \n\t");
264  asm volatile("pop r16 \n\t");
265  asm volatile("out 0x3F,r16 \n\t");
266  asm volatile("pop r16 \n\t");
267  asm volatile("reti \n\t");
268  }
269 
270  /*
271  void TIMER2_COMPA_vect(void)
272  {
273 
274  asm volatile("push r16 \n\t");
275  asm volatile("in r16,0x3F \n\t");
276  asm volatile("push r16 \n\t");
277  asm volatile("push r30 \n\t");
278  asm volatile("push r31 \n\t");
279  asm volatile("lds r30,pointer \n\t");
280  asm volatile("lds r31,pointer+1 \n\t");
281  asm volatile("ldd r16,z+56 \n\t");
282  asm volatile("sbrs r16,0 \n\t");
283  asm volatile("jmp _AccelerationAlgorithm \n\t"); //Execute the acceleration profile algorithm
284 
285  asm volatile("push r17 \n\t");
286  asm volatile("push r18 \n\t");
287  asm volatile("push r19 \n\t");
288  asm volatile("push r20 \n\t");
289 
290  asm volatile("ldi r16,54 \n\t"); //Offset pointer to point to delay variable as first address
291  asm volatile("add r30,r16 \n\t");
292  asm volatile("clr r16 \n\t");
293  asm volatile("adc r31,r16 \n\t");
294  asm volatile("ldd r16,z+0 \n\t"); //Load delay variable
295  asm volatile("ldd r17,z+1 \n\t"); //load delay variable
296  asm volatile("ldd r18,z+22 \n\t"); //load counter
297  asm volatile("ldd r19,z+23 \n\t");
298 
299  asm volatile("cpse r17,r19 \n\t"); //if high nibble of counter and delay variable is equal, it could be time to step.
300  asm volatile("rjmp _notRdyTimer2 \n\t"); //if not, it certainly is not time
301  asm volatile("cpse r16,r18 \n\t"); //if low nibble of counter and delay variable is equal, it is time to step.
302  asm volatile("rjmp _notRdyTimer2 \n\t"); //if not, it is not time yet
303  asm volatile("sbi 0x05,5 \n\t");
304  asm volatile("sbi 0x0B,4 \n\t"); //PORTD |= (1 << 4);
305  asm volatile("nop \n\t");
306  asm volatile("nop \n\t");
307  asm volatile("nop \n\t");
308  asm volatile("nop \n\t");
309  asm volatile("nop \n\t");
310  asm volatile("nop \n\t");
311  asm volatile("nop \n\t");
312  asm volatile("nop \n\t");
313  asm volatile("nop \n\t");
314  asm volatile("nop \n\t");
315  asm volatile("nop \n\t");
316  asm volatile("nop \n\t");
317  asm volatile("nop \n\t");
318  asm volatile("nop \n\t");
319  asm volatile("nop \n\t");
320  asm volatile("nop \n\t");
321  asm volatile("nop \n\t");
322  asm volatile("cbi 0x0B,4 \n\t"); //PORTD &= ~(1 << 4);
323 
324  asm volatile("clr r17 \n\t");
325  asm volatile("std z+22,r17 \n\t");
326  asm volatile("std z+23,r17 \n\t");
327 
328  asm volatile("ldd r16,z+28 \n\t"); //Load high nibble of control variable
329  asm volatile("ldd r17,z+29 \n\t"); //Load high nibble of control variable
330  asm volatile("ldd r18,z+30 \n\t"); //Load high nibble of control variable
331  asm volatile("ldd r19,z+31 \n\t"); //Load high nibble of control variable
332  asm volatile("cpi r16,0x00 \n\t");
333  asm volatile("brne _adjustControl \n\t");
334  asm volatile("cpi r17,0x00 \n\t");
335  asm volatile("brne _adjustControl \n\t");
336  asm volatile("cpi r18,0x00 \n\t");
337  asm volatile("brne _adjustControl \n\t");
338  asm volatile("cpi r19,0x00 \n\t");
339  asm volatile("brne _adjustControl \n\t");
340  asm volatile("lds r16,0x0070 \n\t"); //load contents of TIMSK2
341  asm volatile("andi r16,0xFD \n\t"); //Clear bit 1 (OCIE2A) of TIMSK2 in order to stop timer 2
342  asm volatile("sts 0x0070,r16 \n\t"); //Store new TIMSK2 value into TIMSK2 register
343 
344  asm volatile("rjmp _endTimer2 \n\t");
345 
346  asm volatile("_adjustControl: \n\t");
347  asm volatile("cpi r19,0x80 \n\t");
348  asm volatile("brlo _subtractControl \n\t");
349 
350  asm volatile("ldi r20,0x01\n\t");
351  asm volatile("add r16,r20 \n\t");
352  asm volatile("clr r20 \n\t");
353  asm volatile("adc r17,r20 \n\t");
354  asm volatile("adc r18,r20 \n\t");
355  asm volatile("adc r19,r20 \n\t");
356  asm volatile("rjmp _endStepTimer2 \n\t");
357 
358  asm volatile("_subtractControl: \n\t");
359  asm volatile("brlo _subtractControl \n\t");
360  asm volatile("subi r16,0x01 \n\t");
361  asm volatile("sbci r17,0x00 \n\t");
362  asm volatile("sbci r18,0x00 \n\t");
363  asm volatile("sbci r19,0x00 \n\t");
364  asm volatile("rjmp _endStepTimer2 \n\t");
365 
366  asm volatile("_notRdyTimer2: \n\t");
367  asm volatile("inc r18 \n\t");
368  asm volatile("clr r16 \n\t");
369  asm volatile("adc r19,r16 \n\t");
370  asm volatile("std z+38,r18 \n\t");
371  asm volatile("std z+39,r19 \n\t");
372  asm volatile("rjmp _endTimer2 \n\t");
373 
374  asm volatile("_endStepTimer2: \n\t");
375  asm volatile("std z+4,r16 \n\t");
376  asm volatile("std z+5,r17 \n\t");
377  asm volatile("std z+6,r18 \n\t");
378  asm volatile("std z+7,r19 \n\t");
379 
380  asm volatile("_endTimer2: \n\t");
381 
382  asm volatile("pop r20 \n\t");
383  asm volatile("pop r19 \n\t");
384  asm volatile("pop r18 \n\t");
385  asm volatile("pop r17 \n\t");
386  asm volatile("pop r31 \n\t");
387  asm volatile("pop r30 \n\t");
388  asm volatile("pop r16 \n\t");
389  asm volatile("out 0x3F,r16 \n\t");
390  asm volatile("pop r16 \n\t");
391  asm volatile("cbi 0x05,5 \n\t");
392  asm volatile("reti \n\t");
393  }*/
394 
395  void TIMER1_COMPA_vect(void)
396  {
397  uint8_t data[2];
398  uint16_t curAngle;
399  int16_t deltaAngle;
400  float newSpeed;
401  static float deltaSpeedAngle = 0.0;
402  static uint8_t loops = 0;
403  static uint16_t oldAngle = 0;
404  static int16_t revolutions = 0;
405 
406  sei();
407  if(I2C.getStatus() != I2CFREE)
408  {
409  return;
410  }
411 
412  if(pointer->mode == DROPIN)
413  {
414  pointer->pidDropIn();
415  return;
416  }
417  else if(pointer->mode == PID)
418  {
419  pointer->pid();
420  return;
421  }
422 
423  I2C.read(ENCODERADDR, ANGLE, 2, data);
424 
425  curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
426  pointer->encoder.angle = curAngle;
427  curAngle -= pointer->encoder.encoderOffset;
428  if(curAngle > 4095)
429  {
430  curAngle -= 61440;
431  }
432 
433  deltaAngle = (int16_t)oldAngle - (int16_t)curAngle;
434 
435  if(deltaAngle < -2047)
436  {
437  revolutions--;
438  deltaAngle += 4096;
439  }
440 
441  else if(deltaAngle > 2047)
442  {
443  revolutions++;
444  deltaAngle -= 4096;
445  }
446 
447  if( loops < 10)
448  {
449  loops++;
450  deltaSpeedAngle += (float)deltaAngle;
451  }
452  else
453  {
454  newSpeed = deltaSpeedAngle*ENCODERSPEEDCONSTANT;
455  if(pointer->dropIn)
456  {
457  newSpeed *= 0.5;
458  }
459  pointer->encoder.curSpeed = newSpeed;
460  loops = 0;
461  deltaSpeedAngle = 0.0;
462  }
463 
464  pointer->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)revolutions);
465  oldAngle = curAngle;
466  }
467 }
468 
469 float2::float2(void)
470 {
471 
472 }
473 
474 float float2::getFloatValue(void)
475 {
476  union
477  {
478  float f;
479  uint32_t i;
480  } a;
481 
482  a.i = (uint32_t)(this->value >> 25);
483 
484  return a.f;
485 }
486 
487 uint64_t float2::getRawValue(void)
488 {
489  return this->value;
490 }
491 
492 void float2::setValue(float val)
493 {
494  union
495  {
496  float f;
497  uint32_t i;
498  } a;
499 
500  a.f = val;
501 
502  this->value = ((uint64_t)a.i) << 25;
503 }
504 
505 bool float2::operator<=(const float &value)
506 {
507  if(this->getFloatValue() > value)
508  {
509  return 0;
510  }
511 
512  if(this->getFloatValue() == value)
513  {
514  if((this->value & 0x0000000000007FFF) > 0)
515  {
516  return 0;
517  }
518  }
519 
520  return 1;
521 }
522 
523 bool float2::operator<=(const float2 &value)
524 {
525  if((this->value >> 56) > (value.value >> 56)) // sign bit of "this" bigger than sign bit of "value"?
526  {
527  return 1; //"This" is negative while "value" is not. ==> "this" < "value"
528  }
529 
530  if((this->value >> 56) == (value.value >> 56)) //Sign bit of "this" == sign bit of "value"?
531  {
532  if( (this->value >> 48) < (value.value >> 48) ) //Exponent of "this" < exponent of "value"?
533  {
534  return 1; //==> "this" is smaller than "value"
535  }
536 
537  if( (this->value >> 48) == (value.value >> 48) ) //Exponent of "this" == exponent of "value"?
538  {
539  if((this->value & 0x0000FFFFFFFFFFFF) <= (value.value & 0x0000FFFFFFFFFFFF)) //mantissa of "this" <= mantissa of "value"?
540  {
541  return 1; //==> "this" <= "value"
542  }
543  }
544  }
545 
546  return 0; //"this" > "value"
547 }
548 
549 float2 & float2::operator=(const float &value)
550 {
551  this->setValue(value);
552 
553  return *this;
554 }
555 
556 float2 & float2::operator+=(const float &value)
557 {
558 
559 }
560 
561 float2 & float2::operator+=(const float2 &value)
562 {
563  float2 temp = value;
564  uint64_t tempMant, tempExp;
565  uint8_t cnt; //how many times should we shift the mantissa of the smallest number to add the two mantissa's
566 
567  if((this->value >> 56) == (temp.value >> 56))
568  {
569  if(*this <= temp)
570  {
571  cnt = (temp.value >> 48) - (this->value >> 48);
572  if(cnt < 48)
573  {
574  tempExp = (temp.value >> 48);
575 
576  this->value &= 0x0000FFFFFFFFFFFF;
577  this->value |= 0x0001000000000000;
578  this->value >>= cnt;
579 
580  tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
581  tempMant += this->value;
582 
583  while(tempMant > 0x2000000000000)
584  {
585  tempMant >>= 1;
586  tempExp++;
587  }
588 
589  tempMant &= 0x0000FFFFFFFFFFFF;
590  this->value = (tempExp << 48) | tempMant;
591  }
592  else
593  {
594  this->value = temp.value;
595  }
596  }
597 
598  else
599  {
600  cnt = (this->value >> 48) - (temp.value >> 48);
601 
602  if(cnt < 48)
603  {
604  tempExp = (this->value >> 48);
605 
606  temp.value &= 0x0000FFFFFFFFFFFF;
607  temp.value |= 0x0001000000000000;
608  temp.value >>= cnt;
609 
610  tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
611  tempMant += temp.value;
612 
613  while(tempMant > 0x2000000000000)
614  {
615  tempMant >>= 1;
616  tempExp++;
617  }
618 
619  tempMant &= 0x0000FFFFFFFFFFFF;
620  this->value = (tempExp << 48) | tempMant;
621  }
622  }
623  }
624 
625  else if((this->value >> 56) == 1)
626  {
627  this->value &= 0x00FFFFFFFFFFFFFF; //clear sign bit, to consider absolute value
628 
629  if(*this <= temp)
630  {
631  cnt = (temp.value >> 48) - (this->value >> 48);
632 
633  if(cnt < 48)
634  {
635  tempExp = (temp.value >> 48);
636 
637  this->value &= 0x0000FFFFFFFFFFFF;
638  this->value |= 0x0001000000000000;
639  this->value >>= cnt;
640 
641  tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
642 
643  tempMant -= this->value;
644 
645  if(tempMant > 0x8000000000000000)
646  {
647 
648  tempMant &= 0x0000FFFFFFFFFFFF;
649  tempExp--;
650  }
651 
652  while(tempMant < 0x1000000000000)
653  {
654  tempMant <<= 1;
655  tempExp--;
656  }
657 
658  tempMant &= 0x0000FFFFFFFFFFFF;
659 
660  this->value = (tempExp << 48) | tempMant;
661  }
662 
663  else
664  {
665  this->value = temp.value;
666  }
667  }
668 
669  else
670  {
671  cnt = (this->value >> 48) - (temp.value >> 48);
672  if(cnt < 48)
673  {
674  tempExp = (this->value >> 48);
675 
676  temp.value &= 0x0000FFFFFFFFFFFF;
677  temp.value |= 0x0001000000000000;
678  temp.value >>= cnt;
679 
680  tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
681 
682  tempMant -= temp.value;
683 
684  if(tempMant > 0x8000000000000000)
685  {
686  tempMant &= 0x0000FFFFFFFFFFFF;
687  tempExp--;
688  }
689 
690  while(tempMant < 0x1000000000000)
691  {
692  tempMant <<= 1;
693  tempExp--;
694  }
695 
696  tempMant &= 0x0000FFFFFFFFFFFF;
697 
698  this->value = (tempExp << 48) | tempMant;
699  this->value |= 0x0100000000000000;
700  }
701  }
702  }
703 
704  else
705  {
706  temp.value &= 0x00FFFFFFFFFFFFFF; //clear sign bit, to consider absolute value
707 
708  if(temp <= *this)
709  {
710  cnt = (this->value >> 48) - (temp.value >> 48);
711  if(cnt < 48)
712  {
713  tempExp = (this->value >> 48);
714 
715  temp.value &= 0x0000FFFFFFFFFFFF;
716  temp.value |= 0x0001000000000000;
717  temp.value >>= cnt;
718 
719  tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
720 
721  tempMant -= temp.value;
722 
723  if(tempMant > 0x8000000000000000)
724  {
725  tempMant &= 0x0000FFFFFFFFFFFF;
726  tempExp--;
727  }
728 
729  while(tempMant < 0x1000000000000)
730  {
731  tempMant <<= 1;
732  tempExp--;
733  }
734 
735  tempMant &= 0x0000FFFFFFFFFFFF;
736 
737  this->value = (tempExp << 48) | tempMant;
738  }
739  }
740 
741  else
742  {
743  cnt = (temp.value >> 48) - (this->value >> 48);
744  if(cnt < 48)
745  {
746  tempExp = (temp.value >> 48);
747 
748  this->value &= 0x0000FFFFFFFFFFFF;
749  this->value |= 0x0001000000000000;
750  this->value >>= cnt;
751 
752  tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
753 
754  tempMant -= this->value;
755 
756  if(tempMant > 0x8000000000000000)
757  {
758  tempMant &= 0x0000FFFFFFFFFFFF;
759  tempExp--;
760  }
761 
762  while(tempMant < 0x1000000000000)
763  {
764  tempMant <<= 1;
765  tempExp--;
766  }
767 
768  tempMant &= 0x0000FFFFFFFFFFFF;
769 
770  this->value = (tempExp << 48) | tempMant;
771  this->value |= 0x0100000000000000;
772  }
773 
774  else
775  {
776  this->value = temp.value;
777  this->value |= 0x0100000000000000;
778  }
779  }
780  }
781 
782  return *this;
783 
784 }
785 
787 {
788 
789 }
790 
792 {
793  float T = 0.0;
794  float Vout = 0.0;
795  float NTC = 0.0;
796 
797  Vout = analogRead(TEMP)*0.0048828125; //0.0048828125 = 5V/1024
798  NTC = ((R*5.0)/Vout)-R; //the current NTC resistance
799  NTC = log(NTC);
800  T = A + B*NTC + C*NTC*NTC*NTC; //Steinhart-Hart equation
801  T = 1.0/T;
802 
803  return T - 273.15;
804 }
805 
807 {
808  I2C.begin();
809 }
810 
812 {
813  return (float)this->angleMoved*0.087890625;
814 }
815 
817 {
818  return this->curSpeed;
819 }
820 
821 void uStepperEncoder::setup(uint8_t mode)
822 {
823  uint8_t data[2];
824  TCNT1 = 0;
825 
826  if(mode)
827  {
828  ICR1 = 32000;
829  }
830  else
831  {
832  ICR1 = 16000;
833  }
834 
835  TIFR1 = 0;
836  TIMSK1 = (1 << OCIE1A);
837  TCCR1A = (1 << WGM11);
838  TCCR1B = (1 << WGM12) | (1 << WGM13) | (1 << CS10);
839  I2C.read(ENCODERADDR, ANGLE, 2, data);
840  this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
841 
842  this->oldAngle = 0;
843  this->angleMoved = 0;
844 
845  sei();
846 }
847 
849 {
850  cli();
851  uint8_t data[2];
852  I2C.read(ENCODERADDR, ANGLE, 2, data);
853  this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
854 
855  this->angle = 0;
856  this->oldAngle = 0;
857  this->angleMoved = 0;
858  sei();
859 }
860 
862 {
863  return (float)this->angle*0.087890625;
864 }
865 
867 {
868  uint8_t data[2];
869 
870  I2C.read(ENCODERADDR, MAGNITUDE, 2, data);
871 
872  return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
873 }
874 
876 {
877  uint8_t data;
878 
879  I2C.read(ENCODERADDR, MAGNITUDE, 1, &data);
880 
881  return data;
882 }
883 
885 {
886  uint8_t data;
887 
888  I2C.read(ENCODERADDR, AGC, 1, &data);
889 
890  data &= 0x38; //For some reason the encoder returns random values on reserved bits. Therefore we make sure reserved bits are cleared before checking the reply !
891 
892  if(data == 0x08)
893  {
894  return 1; //magnet too strong
895  }
896 
897  else if(data == 0x10)
898  {
899  return 2; //magnet too weak
900  }
901 
902  else if(data == 0x20)
903  {
904  return 0; //magnet detected and within limits
905  }
906 
907  return 3; //Something went horribly wrong !
908 }
909 
911 {
912 
913  this->state = STOP;
914 
915  this->setMaxAcceleration(1000.0);
916  this->setMaxVelocity(1000.0);
917 
918  p = &(this->control);
919  pointer = this;
920 
921  DDRD |= (1 << 7); //set direction pin to output
922  DDRD |= (1 << 4); //set step pin to output
923  DDRB |= (1 << 0); //set enable pin to output
924 }
925 
926 uStepper::uStepper(float accel, float vel)
927 {
928  this->state = STOP;
929 
930  this->setMaxVelocity(vel);
931  this->setMaxAcceleration(accel);
932 
933  p = &(this->control);
934  pointer = this;
935 
936  DDRD |= (1 << 7); //set direction pin to output
937  DDRD |= (1 << 4); //set step pin to output
938  DDRB |= (1 << 0); //set enable pin to output
939 }
940 
942 {
943  this->acceleration = accel;
944 
945  this->stopTimer(); //Stop timer so we dont fuck up stuff !
946  this->multiplier.setValue((this->acceleration/(INTFREQ*INTFREQ))); //Recalculate multiplier variable, used by the acceleration algorithm since acceleration has changed!
947 
948  if(this->state != STOP)
949  {
950  if(this->continous == 1) //If motor was running continously
951  {
952  this->runContinous(this->direction); //We should make it run continously again
953  }
954  else //If motor still needs to perform some steps
955  {
956  this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold); //we should make sure the motor gets to execute the remaining steps
957  }
958  }
959 }
960 
962 {
963  return this->acceleration;
964 }
965 
967 {
968  if(vel < 0.5005)
969  {
970  this->velocity = 0.5005; //Limit velocity in order to not overflow delay variable
971  }
972 
973  else if(vel > 28000.0)
974  {
975  this->velocity = 28000.0; //limit velocity in order to not underflow delay variable
976  }
977 
978  else
979  {
980  this->velocity = vel;
981  }
982 
983  this->stopTimer(); //Stop timer so we dont fuck up stuff !
984  this->cruiseDelay = (uint16_t)((INTFREQ/this->velocity) - 0.5); //Calculate cruise delay, so we dont have to recalculate this in the interrupt routine
985 
986  if(this->state != STOP) //If motor was running, we should make sure it runs again
987  {
988  if(this->continous == 1) //If motor was running continously
989  {
990  this->runContinous(this->direction); //We should make it run continously again
991  }
992  else //If motor still needs to perform some steps
993  {
994  this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold); //we should make sure it gets to execute these steps
995  }
996  }
997 }
998 
1000 {
1001  return this->velocity;
1002 }
1003 
1005 {
1006  float curVel;
1007 
1008  if(this->mode == DROPIN)
1009  {
1010  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1011  }
1012 
1013  this->stopTimer(); //Stop interrupt timer, so we don't fuck up stuff !
1014  this->continous = 1; //Set continous variable to 1, in order to let the interrupt routine now, that the motor should run continously
1015 
1016  if(state != STOP) //if the motor is currently running and we want to move the opposite direction, we need to decelerate in order to change direction.
1017  {
1018  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
1019  if(dir != digitalRead(DIR)) //If motor is currently running the opposite direction as desired
1020  {
1021  this->direction = dir;
1022  this->state = INITDECEL; //We should decelerate the motor to full stop before accelerating the speed in the opposite direction
1023  this->initialDecelSteps = (uint32_t)(((curVel*curVel))/(2.0*this->acceleration)); //the amount of steps needed to bring the motor to full stop. (S = (V^2 - V0^2)/(2*-a)))
1024  this->accelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration)); //Number of steps to bring the motor to max speed (S = (V^2 - V0^2)/(2*a)))
1025 
1026  this->exactDelay.setValue(INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1027 
1028  if(this->exactDelay.getFloatValue() >= 65535.5)
1029  {
1030  this->delay = 0xFFFF;
1031  }
1032  else
1033  {
1034  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1035  }
1036  }
1037  else //If the motor is currently rotating the same direction as the desired direction
1038  {
1039  if(curVel > this->velocity) //If current velocity is greater than desired velocity
1040  {
1041  this->state = INITDECEL; //We need to decelerate the motor to desired velocity
1042  this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration)); //Number of steps to bring the motor down from current speed to max speed (S = (V^2 - V0^2)/(2*-a)))
1043  this->accelSteps = 0; //No acceleration phase is needed
1044  }
1045 
1046  else if(curVel < this->velocity) //If the current velocity is less than the desired velocity
1047  {
1048  this->state = ACCEL; //Start accelerating
1049  this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration)); //Number of Steps needed to accelerate from current velocity to full speed
1050  }
1051 
1052  else //If motor is currently running at desired speed
1053  {
1054  this->state = CRUISE; //We should just run at cruise speed
1055  }
1056  }
1057  }
1058 
1059  else //If motor is currently stopped (state = STOP)
1060  {
1061  this->direction = dir;
1062  this->state = ACCEL; //Start accelerating
1063  if(dir) //Set the motor direction pin to the desired setting
1064  {
1065  PORTD |= (1 << 7);
1066  }
1067  else
1068  {
1069  PORTD &= ~(1 << 7);
1070  }
1071  this->accelSteps = (velocity*velocity)/(2.0*acceleration); //Number of steps to bring the motor to max speed (S = (V^2 - V0^2)/(2*a)))
1072 
1073  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1074 
1075  if(this->exactDelay.getFloatValue() > 65535.0)
1076  {
1077  this->delay = 0xFFFF;
1078  }
1079  else
1080  {
1081  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1082  }
1083  }
1084 
1085  this->startTimer(); //start timer so we can perform steps
1086  this->enableMotor(); //Enable motor
1087 }
1088 
1089 void uStepper::moveSteps(uint32_t steps, bool dir, bool holdMode)
1090 {
1091  float curVel;
1092 
1093  if(this->mode == DROPIN)
1094  {
1095  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1096  }
1097 
1098  this->stopTimer(); //Stop interrupt timer so we dont fuck stuff up !
1099  steps--;
1100  this->direction = dir; //Set direction variable to the desired direction of rotation for the interrupt routine
1101  this->hold = holdMode; //Set the hold variable to desired hold mode (block motor or release motor after end movement) for the interrupt routine
1102  this->totalSteps = steps; //Load the desired number of steps into the totalSteps variable for the interrupt routine
1103  this->continous = 0; //Set continous variable to 0, since the motor should not run continous
1104 
1105  if(state != STOP) //if the motor is currently running and we want to move the opposite direction, we need to decelerate in order to change direction.
1106  {
1107  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
1108 
1109  if(dir != digitalRead(DIR)) //If current direction is different from desired direction
1110  {
1111  this->state = INITDECEL; //We should decelerate the motor to full stop
1112  this->initialDecelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration)); //the amount of steps needed to bring the motor to full stop. (S = (V^2 - V0^2)/(2*-a)))
1113  this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration)); //Number of steps to bring the motor to max speed (S = (V^2 - V0^2)/(2*a)))
1114  this->totalSteps += this->initialDecelSteps; //Add the steps used for initial deceleration to the totalSteps variable, since we moved this number of steps, passed the initial position, and therefore need to move this amount of steps extra, in the desired direction
1115 
1116  if(this->accelSteps > (this->totalSteps >> 1)) //If we need to accelerate for longer than half of the total steps, we need to start decelerating before we reach max speed
1117  {
1118  this->accelSteps = this->decelSteps = (this->totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1119  this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps; //If there are still a step left to perform, due to rounding errors, do this step as an acceleration step
1120  }
1121  else
1122  {
1123  this->decelSteps = this->accelSteps; //If top speed is reached before half the total steps are performed, deceleration period should be same length as acceleration period
1124  this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps; //Perform remaining steps, as cruise steps
1125  }
1126 
1127  this->exactDelay.setValue(INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1128 
1129  if(this->exactDelay.getFloatValue() >= 65535.5)
1130  {
1131  this->delay = 0xFFFF;
1132  }
1133  else
1134  {
1135  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1136  }
1137  }
1138  else //If the motor is currently rotating the same direction as desired, we dont necessarily need to decelerate
1139  {
1140  if(curVel > this->velocity) //If current velocity is greater than desired velocity
1141  {
1142  this->state = INITDECEL; //We need to decelerate the motor to desired velocity
1143  this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration)); //Number of steps to bring the motor down from current speed to max speed (S = (V^2 - V0^2)/(2*-a)))
1144  this->accelSteps = 0; //No acceleration phase is needed
1145  this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration)); //Number of steps needed to decelerate the motor from top speed to full stop
1146  this->exactDelay.setValue((INTFREQ/sqrt((curVel*curVel) + 2*this->acceleration)));
1147 
1148  if(this->totalSteps <= (this->initialDecelSteps + this->decelSteps))
1149  {
1150  this->cruiseSteps = 0;
1151  }
1152  else
1153  {
1154  this->cruiseSteps = steps - this->initialDecelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1155  }
1156 
1157 
1158  }
1159 
1160  else if(curVel < this->velocity) //If current velocity is less than desired velocity
1161  {
1162  this->state = ACCEL; //Start accelerating
1163  this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration)); //Number of Steps needed to accelerate from current velocity to full speed
1164 
1165  if(this->accelSteps > (this->totalSteps >> 1)) //If we need to accelerate for longer than half of the total steps, we need to start decelerating before we reach max speed
1166  {
1167  this->accelSteps = this->decelSteps = (this->totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1168  this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps; //If there are still a step left to perform, due to rounding errors, do this step as an acceleration step
1169  this->cruiseSteps = 0;
1170  }
1171  else
1172  {
1173  this->decelSteps = this->accelSteps; //If top speed is reached before half the total steps are performed, deceleration period should be same length as acceleration period
1174  this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps; //Perform remaining steps, as cruise steps
1175  }
1176 
1177  this->cruiseSteps = steps - this->accelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1178  this->initialDecelSteps = 0; //No initial deceleration phase needed
1179  }
1180 
1181  else //If current velocity is equal to desired velocity
1182  {
1183  this->state = CRUISE; //We are already at desired speed, therefore we start at cruise phase
1184  this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration)); //Number of steps needed to decelerate the motor from top speed to full stop
1185  this->accelSteps = 0; //No acceleration phase needed
1186  this->initialDecelSteps = 0; //No initial deceleration phase needed
1187 
1188  if(this->decelSteps >= this->totalSteps)
1189  {
1190  this->cruiseSteps = 0;
1191  }
1192  else
1193  {
1194  this->cruiseSteps = steps - this->decelSteps; //Perform remaining steps as cruise steps
1195  }
1196  }
1197  }
1198  }
1199 
1200  else //If motor is currently at full stop (state = STOP)
1201  {
1202  if(dir) //Set the motor direction pin to the desired setting
1203  {
1204  PORTD |= (1 << 7);
1205  }
1206  else
1207  {
1208  PORTD &= ~(1 << 7);
1209  }
1210  this->state = ACCEL;
1211  this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration)); //Number of steps to bring the motor to max speed (S = (V^2 - V0^2)/(2*a)))
1212  this->initialDecelSteps = 0; //No initial deceleration phase needed
1213 
1214  if(this->accelSteps > (steps >> 1)) //If we need to accelerate for longer than half of the total steps, we need to start decelerating before we reach max speed
1215  {
1216  this->cruiseSteps = 0; //No cruise phase needed
1217  this->accelSteps = this->decelSteps = (steps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1218  this->accelSteps += steps - this->accelSteps - this->decelSteps; //if there are still a step left to perform, due to rounding errors, do this step as an acceleration step
1219  }
1220 
1221  else
1222  {
1223  this->decelSteps = this->accelSteps; //If top speed is reached before half the total steps are performed, deceleration period should be same length as acceleration period
1224  this->cruiseSteps = steps - this->accelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1225  }
1226  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1227 
1228  if(this->exactDelay.getFloatValue() > 65535.0)
1229  {
1230  this->delay = 0xFFFF;
1231  }
1232  else
1233  {
1234  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1235  }
1236  }
1237 
1238  this->startTimer(); //start timer so we can perform steps
1239  this->enableMotor(); //Enable motor driver
1240 }
1241 
1242 void uStepper::hardStop(bool holdMode)
1243 {
1244  if(this->mode == DROPIN)
1245  {
1246  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1247  }
1248 
1249  this->stopTimer(); //Stop interrupt timer, since we shouldn't perform more steps
1250  this->hold = holdMode;
1251 
1252  if(state != STOP)
1253  {
1254  this->state = STOP; //Set current state to STOP
1255 
1256  this->startTimer();
1257  }
1258 
1259  else
1260  {
1261  if(holdMode == SOFT)
1262  {
1263  this->disableMotor();
1264  }
1265 
1266  else if (holdMode == HARD)
1267  {
1268  this->enableMotor();
1269  }
1270  }
1271 }
1272 
1273 void uStepper::softStop(bool holdMode)
1274 {
1275  float curVel;
1276 
1277  if(this->mode == DROPIN)
1278  {
1279  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1280  }
1281 
1282  this->stopTimer(); //Stop interrupt timer, since we shouldn't perform more steps
1283  this->hold = holdMode;
1284 
1285  if(state != STOP)
1286  {
1287  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
1288 
1289  this->decelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration)); //Number of steps to bring the motor down from current speed to max speed (S = (V^2 - V0^2)/(2*-a)))
1290  this->accelSteps = this->initialDecelSteps = this->cruiseSteps = 0; //Reset amount of steps in the different phases
1291  this->state = DECEL;
1292 
1293  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1294 
1295  if(this->exactDelay.getFloatValue() > 65535.0)
1296  {
1297  this->delay = 0xFFFF;
1298  }
1299  else
1300  {
1301  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1302  }
1303 
1304  this->startTimer();
1305  }
1306 
1307  else
1308  {
1309  if(holdMode == SOFT)
1310  {
1311  this->disableMotor();
1312  }
1313 
1314  else if (holdMode == HARD)
1315  {
1316  this->enableMotor();
1317  }
1318  }
1319 }
1320 
1321 void uStepper::setup( uint8_t mode,
1322  uint8_t microStepping,
1323  float faultTolerance,
1324  float faultHysteresis,
1325  float pTerm,
1326  float iTerm,
1327  float dterm)
1328 {
1329  this->mode = mode;
1330  if(this->mode)
1331  {
1332  if(this->mode == DROPIN)
1333  {
1334  //Set Enable, Step and Dir signal pins from 3dPrinter controller as inputs
1335  pinMode(2,INPUT);
1336  pinMode(3,INPUT);
1337  pinMode(4,INPUT);
1338  //Enable internal pull-up resistors on the above pins
1339  digitalWrite(2,HIGH);
1340  digitalWrite(3,HIGH);
1341  digitalWrite(4,HIGH);
1342  attachInterrupt(digitalPinToInterrupt(2), interrupt0, CHANGE);
1343  attachInterrupt(digitalPinToInterrupt(3), interrupt1, FALLING);
1344  }
1345 
1346  this->stepConversion = ((float)(200*microStepping))/4096.0; //Calculate conversion coefficient from raw encoder data, to actual moved steps
1347  this->tolerance = faultTolerance; //Number of steps missed before controller kicks in
1348  this->hysteresis = faultHysteresis;
1349  this->angleToStep = ((float)(200*microStepping))/360.0; //Calculate conversion coefficient from angle to corresponding number of steps
1350 
1351  //Scale supplied controller coefficents. This is done to enable the user to use easier to manage numbers for these coefficients.
1352  this->pTerm = pTerm/10000.0;
1353  this->iTerm = iTerm/10000.0;
1354  this->dTerm = dTerm/10000.0;
1355  }
1356 
1357  TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1358  TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1359  TCCR2B |= (1 << CS21)| (1 << WGM22); //Enable timer with prescaler 8 - interrupt base frequency ~ 2MHz
1360  TCCR2A |= (1 << WGM21) | (1 << WGM20); //Switch timer 2 to Fast PWM mode, to enable adjustment of interrupt frequency, while being able to use PWM
1361  OCR2A = 70; //Change top value to 70 in order to obtain an interrupt frequency of 28.571kHz
1362  this->enableMotor();
1363  this->encoder.setup(mode);
1364 
1365 }
1366 
1368 {
1369  while(TCNT2); //Wait for timer to overflow, to ensure correct timing.
1370  TIFR2 |= (1 << OCF2A); //Clear compare match interrupt flag, if it is set.
1371  TIMSK2 |= (1 << OCIE2A); //Enable compare match interrupt
1372 
1373  sei();
1374 }
1375 
1377 {
1378  TIMSK2 &= ~(1 << OCIE2A); //disable compare match interrupt
1379 }
1380 
1382 {
1383  PORTB &= ~(1 << 0); //Enable motor driver
1384 }
1385 
1387 {
1388  PORTB |= (1 << 0); //Disable motor driver
1389 }
1390 
1392 {
1393  return this->direction;
1394 }
1395 
1397 {
1398  if(this->state != STOP)
1399  {
1400  return 1; //Motor running
1401  }
1402 
1403  return 0; //Motor not running
1404 }
1405 
1407 {
1408  if(this->direction == CW)
1409  {
1410  return this->stepsSinceReset + this->currentStep;
1411  }
1412  else
1413  {
1414  return this->stepsSinceReset - this->currentStep;
1415  }
1416 }
1417 
1418 void uStepper::pwmD8(float duty)
1419 {
1420  pinMode(8,OUTPUT);
1421  TCCR1A |= (1 << COM1B1);
1422 
1423  if(duty > 100.0)
1424  {
1425  duty = 100.0;
1426  }
1427  else if(duty < 0.0)
1428  {
1429  duty = 0.0;
1430  }
1431 
1432  duty *= 160.0;
1433 
1434  OCR1B = (uint16_t)(duty + 0.5);
1435 }
1436 
1437 void uStepper::pwmD3(float duty)
1438 {
1439  pinMode(3,OUTPUT);
1440  TCCR2A |= (1 << COM2B1);
1441 
1442  if(duty > 100.0)
1443  {
1444  duty = 100.0;
1445  }
1446  else if(duty < 0.0)
1447  {
1448  duty = 0.0;
1449  }
1450 
1451  duty *= 0.7;
1452 
1453  OCR2B = (uint16_t)(duty + 0.5);
1454 }
1455 
1456 void uStepper::updateSetPoint(float setPoint)
1457 {
1458  if(this->mode != DROPIN)
1459  {
1460  return; //This function doesn't make sense in any other configuration than dropin
1461  }
1462 
1463  this->stepCnt = (int32_t)(setPoint*this->angleToStep);
1464 }
1465 
1467 {
1468  static float oldError = 0.0;
1469  float integral;
1470  float output = 1.0;
1471  static float accumError = 0.0;
1472  uint8_t data[2];
1473  uint16_t curAngle;
1474  int16_t deltaAngle;
1475  static uint16_t oldAngle = 0;
1476  static int16_t revolutions = 0;
1477  float error;
1478  static uint32_t speed = 10000;
1479  static uint32_t oldMicros = 0;
1480 
1481  sei();
1482 
1483  I2C.read(ENCODERADDR, ANGLE, 2, data);
1484  cli();
1485  error = (float)this->stepCnt;
1486  if(this->speedValue[0] == oldMicros)
1487  {
1488  speed += 2000;
1489  if(speed > 10000)
1490  {
1491  speed = 10000;
1492  }
1493  }
1494  else
1495  {
1496  speed = this->speedValue[0] - this->speedValue[1];
1497  }
1498  sei();
1499 
1500  curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1501  this->encoder.angle = curAngle;
1502  curAngle -= this->encoder.encoderOffset;
1503  if(curAngle > 4095)
1504  {
1505  curAngle -= 61440;
1506  }
1507 
1508  deltaAngle = (int16_t)oldAngle - (int16_t)curAngle;
1509 
1510  if(deltaAngle < -2047)
1511  {
1512  revolutions--;
1513  deltaAngle += 4096;
1514  }
1515 
1516  else if(deltaAngle > 2047)
1517  {
1518  revolutions++;
1519  deltaAngle -= 4096;
1520  }
1521 
1522  this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)revolutions);
1523  oldAngle = curAngle;
1524 
1525  error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1526 
1527  if(error < -this->tolerance)
1528  {
1529  cli(); //Do Atomic copy, in order to not fuck up variables
1530  this->control = (int32_t)error; //Move current error into control variable, for Timer2 and int0 routines to decide who should provide steps
1531  sei();
1532 
1533  error = -error; //error variable should always be positive when calculating controller output
1534 
1535  integral = error*this->iTerm; //Multiply current error by integral term
1536  accumError += integral; //And accumulate, to get integral action
1537 
1538  //The output of each PID part, should be subtracted from the output variable.
1539  //This is true, since in case of no error the motor should run at a higher speed
1540  //to catch up, and since the speed variable contains the number of microseconds
1541  //between each step, the we need to multiply with a number < 1 to increase speed
1542  output -= this->pTerm*error;
1543  output -= accumError;
1544  output -= this->dTerm*(error - oldError);
1545 
1546  oldError = error; //Save current error for next sample, for use in differential part
1547 
1548  PORTD &= ~(1 << 7); //change direction to CW
1549 
1550  output *= (float)speed; //Calculate stepSpeed
1551 
1552  if(output < 54.0) //If stepSpeed is lower than possible
1553  {
1554  output = 54.0; //Set stepSpeed to lowest possible
1555  accumError -= integral; //and subtract current integral part from accumerror (anti-windup)
1556  }
1557 
1558  cli(); //Atomic copy
1559  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5); //calculate Number of interrupt Timer 2 should do before generating step pulse
1560  sei();
1561 
1562  this->startTimer();
1563  PORTB &= ~(1 << 0);
1564  }
1565 
1566  else if(error > this->tolerance)
1567  {
1568  cli(); //Do Atomic copy, in order to not fuck up variables
1569  this->control = (int32_t)error;
1570  sei();
1571 
1572  integral = error*this->iTerm;
1573  accumError += integral;
1574 
1575  output -= this->pTerm*error;
1576  output -= accumError;
1577  output -= this->dTerm*(error - oldError);
1578 
1579  oldError = error;
1580 
1581  PORTD |= (1 << 7); //change direction to CCW
1582  output *= (float)speed;
1583 
1584  if(output < 54.0)
1585  {
1586  output = 54.0;
1587  accumError -= integral;
1588  }
1589 
1590  cli();
1591  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5);
1592  sei();
1593 
1594  this->startTimer();
1595  PORTB &= ~(1 << 0);
1596  }
1597 
1598  else
1599  {
1600  if(error >= -this->hysteresis && error <= this->hysteresis) //If error is less than 1 step
1601  {
1602  PORTB |= (PIND & 0x04) >> 2; //Set enable pin to whatever is demanded by the 3Dprinter controller
1603 
1604  this->control = 0; //Set control variable to 0, in order to make sure int0 routine generates step pulses
1605  accumError = 0.0; //Clear accumerror
1606 
1607  this->stopTimer(); //Stop timer 2
1608  }
1609  }
1610 }
1611 
1612 void uStepper::pid(void)
1613 {
1614  static float oldError = 0.0;
1615  float integral;
1616  float output = 1.0;
1617  static float accumError = 0.0;
1618  float limit;
1619  uint8_t data[2];
1620  uint16_t curAngle;
1621  int16_t deltaAngle;
1622  static uint16_t oldAngle = 0;
1623  static int16_t revolutions = 0;
1624  float error;
1625  static uint32_t speed = 10000;
1626  static uint32_t oldMicros = 0;
1627 
1628  sei();
1629  if(I2C.getStatus() != I2CFREE)
1630  {
1631  return;
1632  }
1633 
1634  I2C.read(ENCODERADDR, ANGLE, 2, data);
1635  cli();
1636  error = (float)this->stepCnt;
1637  speed = (uint32_t)(this->exactDelay.getFloatValue() * INTPERIOD);
1638  sei();
1639 
1640  curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1641  this->encoder.angle = curAngle;
1642  curAngle -= this->encoder.encoderOffset;
1643  if(curAngle > 4095)
1644  {
1645  curAngle -= 61440;
1646  }
1647 
1648  deltaAngle = (int16_t)oldAngle - (int16_t)curAngle;
1649 
1650  if(deltaAngle < -2047)
1651  {
1652  revolutions--;
1653  deltaAngle += 4096;
1654  }
1655 
1656  else if(deltaAngle > 2047)
1657  {
1658  revolutions++;
1659  deltaAngle -= 4096;
1660  }
1661 
1662  this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)revolutions);
1663  oldAngle = curAngle;
1664 
1665  error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1666 
1667  if(error < -this->tolerance)
1668  {
1669  cli(); //Do Atomic copy, in order to not fuck up variables
1670  this->control = (int32_t)error; //Move current error into control variable, for Timer2 and int0 routines to decide who should provide steps
1671  sei();
1672 
1673  error = -error; //error variable should always be positive when calculating controller output
1674 
1675  integral = error*this->iTerm; //Multiply current error by integral term
1676  accumError += integral; //And accumulate, to get integral action
1677 
1678  //The output of each PID part, should be subtracted from the output variable.
1679  //This is true, since in case of no error the motor should run at a higher speed
1680  //to catch up, and since the speed variable contains the number of microseconds
1681  //between each step, the we need to multiply with a number < 1 to increase speed
1682  output -= this->pTerm*error;
1683  output -= accumError;
1684  output -= this->dTerm*(error - oldError);
1685 
1686  oldError = error; //Save current error for next sample, for use in differential part
1687 
1688  PORTD &= ~(1 << 7); //change direction to CW
1689 
1690  output *= (float)speed; //Calculate stepSpeed
1691 
1692  if(output < 54.0) //If stepSpeed is lower than possible
1693  {
1694  output = 54.0; //Set stepSpeed to lowest possible
1695  accumError -= integral; //and subtract current integral part from accumerror (anti-windup)
1696  }
1697 
1698  cli(); //Atomic copy
1699  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5); //calculate Number of interrupt Timer 2 should do before generating step pulse
1700  sei();
1701 
1702  this->startTimer();
1703  PORTB &= ~(1 << 0);
1704  }
1705 
1706  else if(error > this->tolerance)
1707  {
1708  cli(); //Do Atomic copy, in order to not fuck up variables
1709  this->control = (int32_t)error;
1710  sei();
1711 
1712  integral = error*this->iTerm;
1713  accumError += integral;
1714 
1715  output -= this->pTerm*error;
1716  output -= accumError;
1717  output -= this->dTerm*(error - oldError);
1718 
1719  oldError = error;
1720 
1721  PORTD |= (1 << 7); //change direction to CCW
1722  output *= (float)speed;
1723 
1724  if(output < 54.0)
1725  {
1726  output = 54.0;
1727  accumError -= integral;
1728  }
1729 
1730  cli();
1731  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5);
1732  sei();
1733 
1734  this->startTimer();
1735  PORTB &= ~(1 << 0);
1736  }
1737 
1738  else
1739  {
1740  if(error >= -this->hysteresis && error <= this->hysteresis) //If error is less than 1 step
1741  {
1742  cli();
1743  if(this->hold || this->state)
1744  {
1745  PORTB &= ~(1 << 0);
1746  }
1747  else
1748  {
1749  PORTB |= (1 << 0);
1750  }
1751 
1752  if(this->direction)
1753  {
1754  PORTD |= (1 << 7); //change direction to CCW
1755  }
1756  else
1757  {
1758  PORTD &= ~(1 << 7); //change direction to CW
1759  }
1760  sei();
1761  this->control = 0; //Set control variable to 0, in order to make sure int0 routine generates step pulses
1762  accumError = 0.0; //Clear accumerror
1763 
1764  if(!this->state)
1765  {
1766  this->stopTimer(); //Stop timer 2
1767  }
1768  }
1769  }
1770 }
1771 
1772 void i2cMaster::cmd(uint8_t cmd)
1773 {
1774  uint16_t i = 0;
1775  // send command
1776  TWCR = cmd;
1777  // wait for command to complete
1778  while (!(TWCR & (1 << TWINT)));
1779 
1780  // save status bits
1781  status = TWSR & 0xF8;
1782 }
1783 
1784 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1785 {
1786  uint8_t i, buff[numOfBytes];
1787 
1788  TIMSK1 &= ~(1 << OCIE1A);
1789 
1790  I2C.start(slaveAddr, WRITE);
1791 
1792  I2C.writeByte(regAddr);
1793 
1794  I2C.restart(slaveAddr, READ);
1795 
1796  for(i = 0; i < (numOfBytes - 1); i++)
1797  {
1798  I2C.readByte(ACK, &data[i]);
1799  }
1800 
1801  I2C.readByte(NACK, &data[numOfBytes-1]);
1802 
1803  I2C.stop();
1804 
1805  TIMSK1 |= (1 << OCIE1A);
1806 
1807  return 1;
1808 }
1809 
1810 bool i2cMaster::write(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1811 {
1812  uint8_t i;
1813 
1814  TIMSK1 &= ~(1 << OCIE1A);
1815 
1816  I2C.start(slaveAddr, WRITE);
1817  I2C.writeByte(regAddr);
1818 
1819  for(i = 0; i < numOfBytes; i++)
1820  {
1821  I2C.writeByte(*(data + i));
1822  }
1823  I2C.stop();
1824 
1825  TIMSK1 |= (1 << OCIE1A);
1826 
1827  return 1;
1828 }
1829 
1830 bool i2cMaster::readByte(bool ack, uint8_t *data)
1831 {
1832  if(ack)
1833  {
1834  this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA));
1835  }
1836 
1837  else
1838  {
1839  this->cmd((1 << TWINT) | (1 << TWEN));
1840  }
1841 
1842  *data = TWDR;
1843 
1844  return 1;
1845 }
1846 
1847 bool i2cMaster::start(uint8_t addr, bool RW)
1848 {
1849  // send START condition
1850  this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1851 
1852  if (this->getStatus() != START && this->getStatus() != REPSTART)
1853  {
1854  return false;
1855  }
1856 
1857  // send device address and direction
1858  TWDR = (addr << 1) | RW;
1859  this->cmd((1 << TWINT) | (1 << TWEN));
1860 
1861  if (RW == READ)
1862  {
1863  return this->getStatus() == RXADDRACK;
1864  }
1865 
1866  else
1867  {
1868  return this->getStatus() == TXADDRACK;
1869  }
1870 }
1871 
1872 bool i2cMaster::restart(uint8_t addr, bool RW)
1873 {
1874  return this->start(addr, RW);
1875 }
1876 
1877 bool i2cMaster::writeByte(uint8_t data)
1878 {
1879  TWDR = data;
1880 
1881  this->cmd((1 << TWINT) | (1 << TWEN));
1882 
1883  return this->getStatus() == TXDATAACK;
1884 }
1885 
1887 {
1888  uint16_t i = 0;
1889  // issue stop condition
1890  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1891 
1892  // wait until stop condition is executed and bus released
1893  while (TWCR & (1 << TWSTO));
1894 
1895  status = I2CFREE;
1896 
1897  return 1;
1898 }
1899 
1901 {
1902  return status;
1903 }
1904 
1906 {
1907  // set bit rate register to 12 to obtain 400kHz scl frequency (in combination with no prescaling!)
1908  TWBR = 12;
1909  // no prescaler
1910  TWSR &= 0xFC;
1911 }
1912 
1914 {
1915 
1916 }
#define R
Definition: uStepper.h:209
uStepper(void)
Constructor of uStepper class.
Definition: uStepper.cpp:910
uStepperEncoder(void)
Constructor.
Definition: uStepper.cpp:806
void interrupt0(void)
Used by dropin feature to take in step pulses.
Definition: uStepper.cpp:114
void TIMER1_COMPA_vect(void) __attribute__((signal
Measures angle and speed of motor.
Definition: uStepper.cpp:395
uint16_t getStrength(void)
Measure the strength of the magnet.
Definition: uStepper.cpp:866
#define CRUISE
Definition: uStepper.h:187
#define WRITE
Definition: uStepper.h:215
float getSpeed(void)
Measure the current speed of the motor.
Definition: uStepper.cpp:816
volatile uint16_t counter
Definition: uStepper.h:582
bool write(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
sets up I2C connection to device, writes a number of data bytes and closes the connection ...
Definition: uStepper.cpp:1810
bool getMotorState(void)
Get the current state of the motor.
Definition: uStepper.cpp:1396
volatile int32_t stepCnt
Definition: uStepper.h:585
void pwmD8(float duty)
Generate PWM signal on digital output 8.
Definition: uStepper.cpp:1418
Prototype of class for accessing all features of the uStepper in a single object. ...
Definition: uStepper.h:495
void pid(void)
This method handles the actual PID controller calculations, if enabled.
Definition: uStepper.cpp:1612
volatile float curSpeed
Definition: uStepper.h:380
void enableMotor(void)
Enables the stepper driver output stage.
Definition: uStepper.cpp:1381
bool dropIn
Definition: uStepper.h:559
#define A
Definition: uStepper.h:248
float getMaxAcceleration(void)
Get the value of the maximum motor acceleration.
Definition: uStepper.cpp:961
#define CW
Definition: uStepper.h:194
#define DECEL
Definition: uStepper.h:188
float getTemp(void)
Request a reading of current temperature.
Definition: uStepper.cpp:791
#define INTPIDDELAYCONSTANT
Definition: uStepper.h:193
void updateSetPoint(float setPoint)
Updates setpoint for the motor.
Definition: uStepper.cpp:1456
#define RXADDRACK
Definition: uStepper.h:225
#define C
Definition: uStepper.h:250
bool getCurrentDirection(void)
Returns the direction the motor is currently configured to rotate.
Definition: uStepper.cpp:1391
void pidDropIn(void)
This method handles the actual PID controller calculations for drop-in feature, if enabled...
Definition: uStepper.cpp:1466
float getAngleMoved(void)
Measure the angle moved from reference position.
Definition: uStepper.cpp:811
#define ENCODERADDR
Definition: uStepper.h:202
void hardStop(bool holdMode)
Stop the motor without deceleration.
Definition: uStepper.cpp:1242
void setMaxAcceleration(float accel)
Set the maximum acceleration of the stepper motor.
Definition: uStepper.cpp:941
void startTimer(void)
Starts timer for stepper algorithm.
Definition: uStepper.cpp:1367
void setMaxVelocity(float vel)
Sets the maximum rotational velocity of the motor.
Definition: uStepper.cpp:966
void moveSteps(uint32_t steps, bool dir, bool holdMode)
Make the motor perform a predefined number of steps.
Definition: uStepper.cpp:1089
#define STOP
Definition: uStepper.h:185
float getMaxVelocity(void)
Returns the maximum rotational velocity of the motor.
Definition: uStepper.cpp:999
#define DROPIN
Definition: uStepper.h:182
bool writeByte(uint8_t data)
Writes a byte to a device on the I2C bus.
Definition: uStepper.cpp:1877
i2cMaster(void)
Constructor.
Definition: uStepper.cpp:1913
void pwmD3(float duty)
Generate PWM signal on digital output 3.
Definition: uStepper.cpp:1437
void softStop(bool holdMode)
Stop the motor with deceleration.
Definition: uStepper.cpp:1273
uint8_t detectMagnet(void)
Detect if magnet is present and within range.
Definition: uStepper.cpp:884
uint8_t mode
Definition: uStepper.h:602
uint8_t getStatus(void)
Get current I2C status.
Definition: uStepper.cpp:1900
#define READ
Definition: uStepper.h:213
void cmd(uint8_t cmd)
Sends commands over the I2C bus.
Definition: uStepper.cpp:1772
volatile int32_t angleMoved
Definition: uStepper.h:376
bool restart(uint8_t addr, bool RW)
Restarts connection between arduino and I2C device.
Definition: uStepper.cpp:1872
bool readByte(bool ack, uint8_t *data)
Reads a byte from the I2C bus.
Definition: uStepper.cpp:1830
int64_t stepsSinceReset
Definition: uStepper.h:541
uStepperEncoder encoder
Definition: uStepper.h:671
volatile uint32_t speedValue[2]
Definition: uStepper.h:591
#define HARD
Definition: uStepper.h:196
void stopTimer(void)
Stops the timer for the stepper algorithm.
Definition: uStepper.cpp:1376
uint8_t getAgc(void)
Read the current AGC value of the encoder chip.
Definition: uStepper.cpp:875
bool start(uint8_t addr, bool RW)
sets up connection between arduino and I2C device.
Definition: uStepper.cpp:1847
#define MAGNITUDE
Definition: uStepper.h:207
void runContinous(bool dir)
Make the motor rotate continuously.
Definition: uStepper.cpp:1004
#define INTFREQ
Definition: uStepper.h:190
#define ENCODERSPEEDCONSTANT
Definition: uStepper.h:200
Prototype of class for accessing the TWI (I2C) interface of the AVR (master mode only).
Definition: uStepper.h:967
volatile int32_t control
Definition: uStepper.h:588
#define ANGLE
Definition: uStepper.h:204
void setHome(void)
Define new reference(home) position.
Definition: uStepper.cpp:848
#define SOFT
Definition: uStepper.h:197
float getAngle(void)
Measure the current shaft angle.
Definition: uStepper.cpp:861
#define ACK
Definition: uStepper.h:227
#define B
Definition: uStepper.h:249
bool stop(void)
Closes the I2C connection.
Definition: uStepper.cpp:1886
#define AGC
Definition: uStepper.h:206
void TIMER2_COMPA_vect(void)
Definition: uStepper.cpp:127
#define PID
Definition: uStepper.h:183
#define I2CFREE
Definition: uStepper.h:211
#define REPSTART
Definition: uStepper.h:219
void begin(void)
Setup TWI (I2C) interface.
Definition: uStepper.cpp:1905
uint16_t encoderOffset
Definition: uStepper.h:377
void interrupt1(void)
Used by dropin feature to take in enable signal.
Definition: uStepper.cpp:81
#define TXDATAACK
Definition: uStepper.h:223
#define NACK
Definition: uStepper.h:229
Function prototypes and definitions for the uStepper library.
#define ACCEL
Definition: uStepper.h:186
#define INITDECEL
Definition: uStepper.h:189
void setup(uint8_t mode)
Setup the encoder.
Definition: uStepper.cpp:821
uint8_t state
Definition: uStepper.h:507
uStepperTemp(void)
Constructor.
Definition: uStepper.cpp:786
uint16_t delay
Definition: uStepper.h:551
bool read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
sets up I2C connection to device, reads a number of data bytes and closes the connection ...
Definition: uStepper.cpp:1784
#define START
Definition: uStepper.h:217
void disableMotor(void)
Disables the stepper driver output stage.
Definition: uStepper.cpp:1386
#define TXADDRACK
Definition: uStepper.h:221
int64_t getStepsSinceReset(void)
Get the number of steps applied since reset.
Definition: uStepper.cpp:1406
void setup(uint8_t mode=NORMAL, uint8_t microStepping=SIXTEEN, float faultTolerance=10.0, float faultHysteresis=5.0, float pTerm=1.0, float iTerm=0.02, float dterm=0.006)
Initializes the different parts of the uStepper object.
Definition: uStepper.cpp:1321