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