uStepper
uStepper.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepper.cpp *
3 * Version: 1.2.0 *
4 * date: March 22, 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  I2C.read(ENCODERADDR, ANGLE, 2, data);
714  this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
715  this->oldAngle = 0;
716  this->angleMoved = 0;
717  this->revolutions = 0;
718 
719  sei();
720 }
721 
723 {
724  cli();
725  uint8_t data[2];
726  I2C.read(ENCODERADDR, ANGLE, 2, data);
727  this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
728 
729  this->angle = 0;
730  this->oldAngle = 0;
731  this->angleMoved = 0;
732  this->revolutions = 0;
733  sei();
734 }
735 
737 {
738  return (float)this->angle*0.087890625;
739 }
740 
742 {
743  uint8_t data[2];
744 
745  I2C.read(ENCODERADDR, MAGNITUDE, 2, data);
746 
747  return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
748 }
749 
751 {
752  uint8_t data;
753 
754  I2C.read(ENCODERADDR, AGC, 1, &data);
755 
756  return data;
757 }
758 
760 {
761  uint8_t data;
762 
763  I2C.read(ENCODERADDR, STATUS, 1, &data);
764 
765  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 !
766 
767  if(data == 0x08)
768  {
769  return 1; //magnet too strong
770  }
771 
772  else if(data == 0x10)
773  {
774  return 2; //magnet too weak
775  }
776 
777  else if(data == 0x20)
778  {
779  return 0; //magnet detected and within limits
780  }
781 
782  return 3; //Something went horribly wrong !
783 }
784 
786 {
787 
788  this->state = STOP;
789 
790  this->setMaxAcceleration(1000.0);
791  this->setMaxVelocity(1000.0);
792 
793  p = &(this->control);
794  pointer = this;
795 
796  DDRD |= (1 << 7); //set direction pin to output
797  DDRD |= (1 << 4); //set step pin to output
798  DDRB |= (1 << 0); //set enable pin to output
799 }
800 
801 uStepper::uStepper(float accel, float vel)
802 {
803  this->state = STOP;
804 
805  this->setMaxVelocity(vel);
806  this->setMaxAcceleration(accel);
807 
808  p = &(this->control);
809  pointer = this;
810 
811  DDRD |= (1 << 7); //set direction pin to output
812  DDRD |= (1 << 4); //set step pin to output
813  DDRB |= (1 << 0); //set enable pin to output
814 }
815 
817 {
818  this->acceleration = accel;
819 
820  this->stopTimer(); //Stop timer so we dont fuck up stuff !
821  this->multiplier.setValue((this->acceleration/(INTFREQ*INTFREQ))); //Recalculate multiplier variable, used by the acceleration algorithm since acceleration has changed!
822 
823  if(this->state != STOP)
824  {
825  if(this->continous == 1) //If motor was running continously
826  {
827  this->runContinous(this->direction); //We should make it run continously again
828  }
829  else //If motor still needs to perform some steps
830  {
831  this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold); //we should make sure the motor gets to execute the remaining steps
832  }
833  }
834 }
835 
837 {
838  return this->acceleration;
839 }
840 
842 {
843  if(vel < 0.5005)
844  {
845  this->velocity = 0.5005; //Limit velocity in order to not overflow delay variable
846  }
847 
848  else if(vel > 28000.0)
849  {
850  this->velocity = 28000.0; //limit velocity in order to not underflow delay variable
851  }
852 
853  else
854  {
855  this->velocity = vel;
856  }
857 
858  this->stopTimer(); //Stop timer so we dont fuck up stuff !
859  this->cruiseDelay = (uint16_t)((INTFREQ/this->velocity) - 0.5); //Calculate cruise delay, so we dont have to recalculate this in the interrupt routine
860 
861  if(this->state != STOP) //If motor was running, we should make sure it runs again
862  {
863  if(this->continous == 1) //If motor was running continously
864  {
865  this->runContinous(this->direction); //We should make it run continously again
866  }
867  else //If motor still needs to perform some steps
868  {
869  this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold); //we should make sure it gets to execute these steps
870  }
871  }
872 }
873 
875 {
876  return this->velocity;
877 }
878 
880 {
881  float curVel;
882 
883  if(this->mode == DROPIN)
884  {
885  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
886  }
887 
888  this->stopTimer(); //Stop interrupt timer, so we don't fuck up stuff !
889  this->continous = 1; //Set continous variable to 1, in order to let the interrupt routine now, that the motor should run continously
890 
891  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.
892  {
893  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
894  if(dir != digitalRead(DIR)) //If motor is currently running the opposite direction as desired
895  {
896  this->direction = dir;
897  this->state = INITDECEL; //We should decelerate the motor to full stop before accelerating the speed in the opposite direction
898  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)))
899  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)))
900 
901  this->exactDelay.setValue(INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration)); //number of interrupts before the first step should be performed.
902 
903  if(this->exactDelay.getFloatValue() >= 65535.5)
904  {
905  this->delay = 0xFFFF;
906  }
907  else
908  {
909  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
910  }
911  }
912  else //If the motor is currently rotating the same direction as the desired direction
913  {
914  if(curVel > this->velocity) //If current velocity is greater than desired velocity
915  {
916  this->state = INITDECEL; //We need to decelerate the motor to desired velocity
917  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)))
918  this->accelSteps = 0; //No acceleration phase is needed
919  }
920 
921  else if(curVel < this->velocity) //If the current velocity is less than the desired velocity
922  {
923  this->state = ACCEL; //Start accelerating
924  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
925  }
926 
927  else //If motor is currently running at desired speed
928  {
929  this->state = CRUISE; //We should just run at cruise speed
930  }
931  }
932  }
933 
934  else //If motor is currently stopped (state = STOP)
935  {
936  this->direction = dir;
937  this->state = ACCEL; //Start accelerating
938  if(dir) //Set the motor direction pin to the desired setting
939  {
940  PORTD |= (1 << 7);
941  }
942  else
943  {
944  PORTD &= ~(1 << 7);
945  }
946  this->accelSteps = (velocity*velocity)/(2.0*acceleration); //Number of steps to bring the motor to max speed (S = (V^2 - V0^2)/(2*a)))
947 
948  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
949 
950  if(this->exactDelay.getFloatValue() > 65535.0)
951  {
952  this->delay = 0xFFFF;
953  }
954  else
955  {
956  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
957  }
958  }
959 
960  this->startTimer(); //start timer so we can perform steps
961  this->enableMotor(); //Enable motor
962 }
963 
964 void uStepper::moveSteps(uint32_t steps, bool dir, bool holdMode)
965 {
966  float curVel;
967 
968  if(this->mode == DROPIN)
969  {
970  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
971  }
972 
973  this->stopTimer(); //Stop interrupt timer so we dont fuck stuff up !
974  steps--;
975  this->direction = dir; //Set direction variable to the desired direction of rotation for the interrupt routine
976  this->hold = holdMode; //Set the hold variable to desired hold mode (block motor or release motor after end movement) for the interrupt routine
977  this->totalSteps = steps; //Load the desired number of steps into the totalSteps variable for the interrupt routine
978  this->continous = 0; //Set continous variable to 0, since the motor should not run continous
979 
980  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.
981  {
982  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
983 
984  if(dir != digitalRead(DIR)) //If current direction is different from desired direction
985  {
986  this->state = INITDECEL; //We should decelerate the motor to full stop
987  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)))
988  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)))
989  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
990 
991  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
992  {
993  this->accelSteps = this->decelSteps = (this->totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
994  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
995  }
996  else
997  {
998  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
999  this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps; //Perform remaining steps, as cruise steps
1000  }
1001 
1002  this->exactDelay.setValue(INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1003 
1004  if(this->exactDelay.getFloatValue() >= 65535.5)
1005  {
1006  this->delay = 0xFFFF;
1007  }
1008  else
1009  {
1010  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1011  }
1012  }
1013  else //If the motor is currently rotating the same direction as desired, we dont necessarily need to decelerate
1014  {
1015  if(curVel > this->velocity) //If current velocity is greater than desired velocity
1016  {
1017  this->state = INITDECEL; //We need to decelerate the motor to desired velocity
1018  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)))
1019  this->accelSteps = 0; //No acceleration phase is needed
1020  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
1021  this->exactDelay.setValue((INTFREQ/sqrt((curVel*curVel) + 2*this->acceleration)));
1022 
1023  if(this->totalSteps <= (this->initialDecelSteps + this->decelSteps))
1024  {
1025  this->cruiseSteps = 0;
1026  }
1027  else
1028  {
1029  this->cruiseSteps = steps - this->initialDecelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1030  }
1031 
1032 
1033  }
1034 
1035  else if(curVel < this->velocity) //If current velocity is less than desired velocity
1036  {
1037  this->state = ACCEL; //Start accelerating
1038  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
1039 
1040  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
1041  {
1042  this->accelSteps = this->decelSteps = (this->totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1043  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
1044  this->cruiseSteps = 0;
1045  }
1046  else
1047  {
1048  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
1049  this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps; //Perform remaining steps, as cruise steps
1050  }
1051 
1052  this->cruiseSteps = steps - this->accelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1053  this->initialDecelSteps = 0; //No initial deceleration phase needed
1054  }
1055 
1056  else //If current velocity is equal to desired velocity
1057  {
1058  this->state = CRUISE; //We are already at desired speed, therefore we start at cruise phase
1059  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
1060  this->accelSteps = 0; //No acceleration phase needed
1061  this->initialDecelSteps = 0; //No initial deceleration phase needed
1062 
1063  if(this->decelSteps >= this->totalSteps)
1064  {
1065  this->cruiseSteps = 0;
1066  }
1067  else
1068  {
1069  this->cruiseSteps = steps - this->decelSteps; //Perform remaining steps as cruise steps
1070  }
1071  }
1072  }
1073  }
1074 
1075  else //If motor is currently at full stop (state = STOP)
1076  {
1077  if(dir) //Set the motor direction pin to the desired setting
1078  {
1079  PORTD |= (1 << 7);
1080  }
1081  else
1082  {
1083  PORTD &= ~(1 << 7);
1084  }
1085  this->state = ACCEL;
1086  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)))
1087  this->initialDecelSteps = 0; //No initial deceleration phase needed
1088 
1089  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
1090  {
1091  this->cruiseSteps = 0; //No cruise phase needed
1092  this->accelSteps = this->decelSteps = (steps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1093  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
1094  }
1095 
1096  else
1097  {
1098  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
1099  this->cruiseSteps = steps - this->accelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1100  }
1101  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1102 
1103  if(this->exactDelay.getFloatValue() > 65535.0)
1104  {
1105  this->delay = 0xFFFF;
1106  }
1107  else
1108  {
1109  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1110  }
1111  }
1112 
1113  this->startTimer(); //start timer so we can perform steps
1114  this->enableMotor(); //Enable motor driver
1115 }
1116 
1117 void uStepper::hardStop(bool holdMode)
1118 {
1119  if(this->mode == DROPIN)
1120  {
1121  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1122  }
1123 
1124  this->stopTimer(); //Stop interrupt timer, since we shouldn't perform more steps
1125  this->hold = holdMode;
1126 
1127  if(state != STOP)
1128  {
1129  this->state = STOP; //Set current state to STOP
1130 
1131  this->startTimer();
1132  }
1133 
1134  else
1135  {
1136  if(holdMode == SOFT)
1137  {
1138  this->disableMotor();
1139  }
1140 
1141  else if (holdMode == HARD)
1142  {
1143  this->enableMotor();
1144  }
1145  }
1146 }
1147 
1148 void uStepper::softStop(bool holdMode)
1149 {
1150  float curVel;
1151 
1152  if(this->mode == DROPIN)
1153  {
1154  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1155  }
1156 
1157  this->stopTimer(); //Stop interrupt timer, since we shouldn't perform more steps
1158  this->hold = holdMode;
1159 
1160  if(state != STOP)
1161  {
1162  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
1163 
1164  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)))
1165  this->accelSteps = this->initialDecelSteps = this->cruiseSteps = 0; //Reset amount of steps in the different phases
1166  this->state = DECEL;
1167 
1168  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1169 
1170  if(this->exactDelay.getFloatValue() > 65535.0)
1171  {
1172  this->delay = 0xFFFF;
1173  }
1174  else
1175  {
1176  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1177  }
1178 
1179  this->startTimer();
1180  }
1181 
1182  else
1183  {
1184  if(holdMode == SOFT)
1185  {
1186  this->disableMotor();
1187  }
1188 
1189  else if (holdMode == HARD)
1190  {
1191  this->enableMotor();
1192  }
1193  }
1194 }
1195 
1196 void uStepper::setup( uint8_t mode,
1197  uint8_t microStepping,
1198  float faultTolerance,
1199  float faultHysteresis,
1200  float pTerm,
1201  float iTerm,
1202  float dterm)
1203 {
1204  this->mode = mode;
1205  if(this->mode)
1206  {
1207  if(this->mode == DROPIN)
1208  {
1209  //Set Enable, Step and Dir signal pins from 3dPrinter controller as inputs
1210  pinMode(2,INPUT);
1211  pinMode(3,INPUT);
1212  pinMode(4,INPUT);
1213  //Enable internal pull-up resistors on the above pins
1214  digitalWrite(2,HIGH);
1215  digitalWrite(3,HIGH);
1216  digitalWrite(4,HIGH);
1217  attachInterrupt(digitalPinToInterrupt(2), interrupt0, CHANGE);
1218  attachInterrupt(digitalPinToInterrupt(3), interrupt1, FALLING);
1219  }
1220 
1221  this->stepConversion = ((float)(200*microStepping))/4096.0; //Calculate conversion coefficient from raw encoder data, to actual moved steps
1222  this->tolerance = faultTolerance; //Number of steps missed before controller kicks in
1223  this->hysteresis = faultHysteresis;
1224  this->angleToStep = ((float)(200*microStepping))/360.0; //Calculate conversion coefficient from angle to corresponding number of steps
1225 
1226  //Scale supplied controller coefficents. This is done to enable the user to use easier to manage numbers for these coefficients.
1227  this->pTerm = pTerm/10000.0;
1228  this->iTerm = iTerm/10000.0;
1229  this->dTerm = dTerm/10000.0;
1230  }
1231 
1232  TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1233  TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1234  TCCR2B |= (1 << CS21)| (1 << WGM22); //Enable timer with prescaler 8 - interrupt base frequency ~ 2MHz
1235  TCCR2A |= (1 << WGM21) | (1 << WGM20); //Switch timer 2 to Fast PWM mode, to enable adjustment of interrupt frequency, while being able to use PWM
1236  OCR2A = 70; //Change top value to 70 in order to obtain an interrupt frequency of 28.571kHz
1237  OCR2B = 70;
1238  this->enableMotor();
1239  this->encoder.setup(mode);
1240 
1241 }
1242 
1244 {
1245  while(TCNT2); //Wait for timer to overflow, to ensure correct timing.
1246  TIFR2 |= (1 << OCF2A); //Clear compare match interrupt flag, if it is set.
1247  TIMSK2 |= (1 << OCIE2A); //Enable compare match interrupt
1248 
1249  sei();
1250 }
1251 
1253 {
1254  TIMSK2 &= ~(1 << OCIE2A); //disable compare match interrupt
1255 }
1256 
1258 {
1259  PORTB &= ~(1 << 0); //Enable motor driver
1260 }
1261 
1263 {
1264  PORTB |= (1 << 0); //Disable motor driver
1265 }
1266 
1268 {
1269  return this->direction;
1270 }
1271 
1273 {
1274  if(this->state != STOP)
1275  {
1276  return 1; //Motor running
1277  }
1278 
1279  return 0; //Motor not running
1280 }
1281 
1283 {
1284  if(this->direction == CW)
1285  {
1286  return this->stepsSinceReset;
1287  }
1288  else
1289  {
1290  return this->stepsSinceReset;
1291  }
1292 }
1293 
1294 void uStepper::pwmD8(double duty)
1295 {
1296  if(!(TCCR1A & (1 << COM1B1)))
1297  {
1298  pinMode(8,OUTPUT);
1299  TCCR1A |= (1 << COM1B1);
1300  }
1301 
1302  if(duty > 100.0)
1303  {
1304  duty = 100.0;
1305  }
1306  else if(duty < 0.0)
1307  {
1308  duty = 0.0;
1309  }
1310 
1311  duty *= 160.0;
1312 
1313  OCR1B = (uint16_t)(duty + 0.5);
1314 }
1315 
1316 void uStepper::pwmD8(int mode)
1317 {
1318  if(mode == PWM)
1319  {
1320  if(!(TCCR1A & (1 << COM1B1)))
1321  {
1322  pinMode(8,OUTPUT);
1323  TCCR1A |= (1 << COM1B1);
1324  }
1325  }
1326  else
1327  {
1328  pinMode(8,INPUT);
1329  TCCR1A &= ~(1 << COM1B1);
1330  }
1331 }
1332 
1333 void uStepper::pwmD3(double duty)
1334 {
1335  if(!(TCCR2A & (1 << COM2B1)))
1336  {
1337  pinMode(3,OUTPUT);
1338  TCCR2A |= (1 << COM2B1);
1339  }
1340 
1341  if(duty > 100.0)
1342  {
1343  duty = 100.0;
1344  }
1345  else if(duty < 0.0)
1346  {
1347  duty = 0.0;
1348  }
1349 
1350  duty *= 0.7;
1351 
1352  OCR2B = (uint16_t)(duty + 0.5);
1353 }
1354 
1355 void uStepper::pwmD3(int mode)
1356 {
1357  if(mode == PWM)
1358  {
1359  if(!(TCCR2A & (1 << COM2B1)))
1360  {
1361  pinMode(3,OUTPUT);
1362  TCCR2A |= (1 << COM2B1);
1363  }
1364  }
1365  else
1366  {
1367  pinMode(3,INPUT);
1368  TCCR2A &= ~(1 << COM2B1);
1369  }
1370 }
1371 
1372 void uStepper::updateSetPoint(float setPoint)
1373 {
1374  if(this->mode != DROPIN)
1375  {
1376  return; //This function doesn't make sense in any other configuration than dropin
1377  }
1378 
1379  this->stepCnt = (int32_t)(setPoint*this->angleToStep);
1380 }
1381 
1383 {
1384  static float oldError = 0.0;
1385  float integral;
1386  float output = 1.0;
1387  static float accumError = 0.0;
1388  uint8_t data[2];
1389  uint16_t curAngle;
1390  int16_t deltaAngle;
1391  float error;
1392  static uint32_t speed = 10000;
1393  static uint32_t oldMicros = 0;
1394 
1395  sei();
1396 
1397  I2C.read(ENCODERADDR, ANGLE, 2, data);
1398  cli();
1399  error = (float)this->stepCnt;
1400  if(this->speedValue[0] == oldMicros)
1401  {
1402  speed += 2000;
1403  if(speed > 10000)
1404  {
1405  speed = 10000;
1406  }
1407  }
1408  else
1409  {
1410  speed = this->speedValue[0] - this->speedValue[1];
1411  }
1412  sei();
1413 
1414  curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1415  this->encoder.angle = curAngle;
1416  curAngle -= this->encoder.encoderOffset;
1417  if(curAngle > 4095)
1418  {
1419  curAngle -= 61440;
1420  }
1421 
1422  deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1423 
1424  if(deltaAngle < -2047)
1425  {
1426  this->encoder.revolutions--;
1427  deltaAngle += 4096;
1428  }
1429 
1430  else if(deltaAngle > 2047)
1431  {
1432  this->encoder.revolutions++;
1433  deltaAngle -= 4096;
1434  }
1435 
1436  this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1437  this->encoder.oldAngle = curAngle;
1438 
1439  error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1440 
1441  if(error < -this->tolerance)
1442  {
1443  cli(); //Do Atomic copy, in order to not fuck up variables
1444  this->control = (int32_t)error; //Move current error into control variable, for Timer2 and int0 routines to decide who should provide steps
1445  sei();
1446 
1447  error = -error; //error variable should always be positive when calculating controller output
1448 
1449  integral = error*this->iTerm; //Multiply current error by integral term
1450  accumError += integral; //And accumulate, to get integral action
1451 
1452  //The output of each PID part, should be subtracted from the output variable.
1453  //This is true, since in case of no error the motor should run at a higher speed
1454  //to catch up, and since the speed variable contains the number of microseconds
1455  //between each step, the we need to multiply with a number < 1 to increase speed
1456  output -= this->pTerm*error;
1457  output -= accumError;
1458  output -= this->dTerm*(error - oldError);
1459 
1460  oldError = error; //Save current error for next sample, for use in differential part
1461 
1462  PORTD &= ~(1 << 7); //change direction to CW
1463 
1464  output *= (float)speed; //Calculate stepSpeed
1465 
1466  if(output < 54.0) //If stepSpeed is lower than possible
1467  {
1468  output = 54.0; //Set stepSpeed to lowest possible
1469  accumError -= integral; //and subtract current integral part from accumerror (anti-windup)
1470  }
1471 
1472  cli(); //Atomic copy
1473  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5); //calculate Number of interrupt Timer 2 should do before generating step pulse
1474  sei();
1475 
1476  this->startTimer();
1477  PORTB &= ~(1 << 0);
1478  }
1479 
1480  else if(error > this->tolerance)
1481  {
1482  cli(); //Do Atomic copy, in order to not fuck up variables
1483  this->control = (int32_t)error;
1484  sei();
1485 
1486  integral = error*this->iTerm;
1487  accumError += integral;
1488 
1489  output -= this->pTerm*error;
1490  output -= accumError;
1491  output -= this->dTerm*(error - oldError);
1492 
1493  oldError = error;
1494 
1495  PORTD |= (1 << 7); //change direction to CCW
1496  output *= (float)speed;
1497 
1498  if(output < 54.0)
1499  {
1500  output = 54.0;
1501  accumError -= integral;
1502  }
1503 
1504  cli();
1505  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5);
1506  sei();
1507 
1508  this->startTimer();
1509  PORTB &= ~(1 << 0);
1510  }
1511 
1512  else
1513  {
1514  if(error >= -this->hysteresis && error <= this->hysteresis) //If error is less than 1 step
1515  {
1516  PORTB |= (PIND & 0x04) >> 2; //Set enable pin to whatever is demanded by the 3Dprinter controller
1517 
1518  this->control = 0; //Set control variable to 0, in order to make sure int0 routine generates step pulses
1519  accumError = 0.0; //Clear accumerror
1520 
1521  this->stopTimer(); //Stop timer 2
1522  }
1523  }
1524 }
1525 
1526 void uStepper::pid(void)
1527 {
1528  static float oldError = 0.0;
1529  float integral;
1530  float output = 1.0;
1531  static float accumError = 0.0;
1532  float limit;
1533  uint8_t data[2];
1534  uint16_t curAngle;
1535  int16_t deltaAngle;
1536  float error;
1537  static uint32_t speed = 10000;
1538  static uint32_t oldMicros = 0;
1539 
1540  sei();
1541  if(I2C.getStatus() != I2CFREE)
1542  {
1543  return;
1544  }
1545 
1546  I2C.read(ENCODERADDR, ANGLE, 2, data);
1547  cli();
1548  error = (float)this->stepsSinceReset;
1549  if(this->exactDelay.getFloatValue() >= 1.0)
1550  {
1551  speed = (uint32_t)((this->exactDelay.getFloatValue() * INTPERIOD));
1552  }
1553  else
1554  {
1555  speed = 10000;
1556  }
1557 
1558  if(speed > 10000)
1559  {
1560  speed = 10000;
1561  }
1562  sei();
1563 
1564  curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1565  this->encoder.angle = curAngle;
1566  curAngle -= this->encoder.encoderOffset;
1567  if(curAngle > 4095)
1568  {
1569  curAngle -= 61440;
1570  }
1571 
1572  deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1573 
1574  if(deltaAngle < -2047)
1575  {
1576  this->encoder.revolutions--;
1577  deltaAngle += 4096;
1578  }
1579 
1580  else if(deltaAngle > 2047)
1581  {
1582  this->encoder.revolutions++;
1583  deltaAngle -= 4096;
1584  }
1585 
1586  this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1587  this->encoder.oldAngle = curAngle;
1588 
1589  error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1590 
1591  if(error < -this->tolerance)
1592  {
1593  cli(); //Do Atomic copy, in order to not fuck up variables
1594  this->control = (int32_t)error; //Move current error into control variable, for Timer2 and int0 routines to decide who should provide steps
1595  sei();
1596 
1597  error = -error; //error variable should always be positive when calculating controller output
1598 
1599  integral = error*this->iTerm; //Multiply current error by integral term
1600  accumError += integral; //And accumulate, to get integral action
1601 
1602  //The output of each PID part, should be subtracted from the output variable.
1603  //This is true, since in case of no error the motor should run at a higher speed
1604  //to catch up, and since the speed variable contains the number of microseconds
1605  //between each step, the we need to multiply with a number < 1 to increase speed
1606  output -= this->pTerm*error;
1607  output -= accumError;
1608  output -= this->dTerm*(error - oldError);
1609 
1610  oldError = error; //Save current error for next sample, for use in differential part
1611 
1612  PORTD &= ~(1 << 7); //change direction to CW
1613 
1614  output *= (float)speed; //Calculate stepSpeed
1615 
1616  if(output < 54.0) //If stepSpeed is lower than possible
1617  {
1618  output = 54.0; //Set stepSpeed to lowest possible
1619  accumError -= integral; //and subtract current integral part from accumerror (anti-windup)
1620  }
1621 
1622  cli(); //Atomic copy
1623  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5); //calculate Number of interrupt Timer 2 should do before generating step pulse
1624  sei();
1625 
1626  this->startTimer();
1627  PORTB &= ~(1 << 0);
1628  }
1629 
1630  else if(error > this->tolerance)
1631  {
1632  cli(); //Do Atomic copy, in order to not fuck up variables
1633  this->control = (int32_t)error;
1634  sei();
1635 
1636  integral = error*this->iTerm;
1637  accumError += integral;
1638 
1639  output -= this->pTerm*error;
1640  output -= accumError;
1641  output -= this->dTerm*(error - oldError);
1642 
1643  oldError = error;
1644 
1645  PORTD |= (1 << 7); //change direction to CCW
1646  output *= (float)speed;
1647 
1648  if(output < 54.0)
1649  {
1650  output = 54.0;
1651  accumError -= integral;
1652  }
1653 
1654  cli();
1655  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5);
1656  sei();
1657 
1658  this->startTimer();
1659  PORTB &= ~(1 << 0);
1660  }
1661 
1662  else
1663  {
1664  if(error >= -this->hysteresis && error <= this->hysteresis) //If error is less than 1 step
1665  {
1666  cli();
1667  if(this->hold || this->state)
1668  {
1669  PORTB &= ~(1 << 0);
1670  }
1671  else
1672  {
1673  PORTB |= (1 << 0);
1674  }
1675 
1676  if(this->direction)
1677  {
1678  PORTD |= (1 << 7); //change direction to CCW
1679  }
1680  else
1681  {
1682  PORTD &= ~(1 << 7); //change direction to CW
1683  }
1684  sei();
1685  this->control = 0; //Set control variable to 0, in order to make sure int0 routine generates step pulses
1686  accumError = 0.0; //Clear accumerror
1687 
1688  if(!this->state)
1689  {
1690  this->stopTimer(); //Stop timer 2
1691  }
1692  }
1693  }
1694 }
1695 
1696 bool i2cMaster::cmd(uint8_t cmd)
1697 {
1698  uint16_t i = 0;
1699  // send command
1700  TWCR = cmd;
1701  // wait for command to complete
1702  while (!(TWCR & (1 << TWINT)))
1703  {
1704  i++;
1705  if(i == 65000)
1706  {
1707  return false;
1708  }
1709  }
1710 
1711  // save status bits
1712  status = TWSR & 0xF8;
1713 
1714  return true;
1715 }
1716 
1717 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1718 {
1719  uint8_t i, buff[numOfBytes];
1720 
1721  TIMSK1 &= ~(1 << OCIE1A);
1722 
1723  if(I2C.start(slaveAddr, WRITE) == false)
1724  {
1725  I2C.stop();
1726  return false;
1727  }
1728 
1729  if(I2C.writeByte(regAddr) == false)
1730  {
1731  I2C.stop();
1732  return false;
1733  }
1734 
1735  if(I2C.restart(slaveAddr, READ) == false)
1736  {
1737  I2C.stop();
1738  return false;
1739  }
1740 
1741  for(i = 0; i < (numOfBytes - 1); i++)
1742  {
1743  if(I2C.readByte(ACK, &data[i]) == false)
1744  {
1745  I2C.stop();
1746  return false;
1747  }
1748  }
1749 
1750  if(I2C.readByte(NACK, &data[numOfBytes-1]) == false)
1751  {
1752  I2C.stop();
1753  return false;
1754  }
1755 
1756  I2C.stop();
1757 
1758  TIMSK1 |= (1 << OCIE1A);
1759 
1760  return 1;
1761 }
1762 
1763 bool i2cMaster::write(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1764 {
1765  uint8_t i;
1766 
1767  TIMSK1 &= ~(1 << OCIE1A);
1768 
1769  if(I2C.start(slaveAddr, WRITE) == false)
1770  {
1771  I2C.stop();
1772  return false;
1773  }
1774 
1775  if(I2C.writeByte(regAddr) == false)
1776  {
1777  I2C.stop();
1778  return false;
1779  }
1780 
1781  for(i = 0; i < numOfBytes; i++)
1782  {
1783  if(I2C.writeByte(*(data + i)) == false)
1784  {
1785  I2C.stop();
1786  return false;
1787  }
1788  }
1789  I2C.stop();
1790 
1791  TIMSK1 |= (1 << OCIE1A);
1792 
1793  return 1;
1794 }
1795 
1796 bool i2cMaster::readByte(bool ack, uint8_t *data)
1797 {
1798  if(ack)
1799  {
1800  if(this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA)) == false)
1801  {
1802  return false;
1803  }
1804 
1805  }
1806 
1807  else
1808  {
1809  if(this->cmd((1 << TWINT) | (1 << TWEN)) == false)
1810  {
1811  return false;
1812  }
1813  }
1814 
1815  *data = TWDR;
1816 
1817  return true;
1818 }
1819 
1820 bool i2cMaster::start(uint8_t addr, bool RW)
1821 {
1822  // send START condition
1823  this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1824 
1825  if (this->getStatus() != START && this->getStatus() != REPSTART)
1826  {
1827  return false;
1828  }
1829 
1830  // send device address and direction
1831  TWDR = (addr << 1) | RW;
1832  this->cmd((1 << TWINT) | (1 << TWEN));
1833 
1834  if (RW == READ)
1835  {
1836 
1837  return this->getStatus() == RXADDRACK;
1838  }
1839 
1840  else
1841  {
1842  return this->getStatus() == TXADDRACK;
1843  }
1844 }
1845 
1846 bool i2cMaster::restart(uint8_t addr, bool RW)
1847 {
1848  return this->start(addr, RW);
1849 }
1850 
1851 bool i2cMaster::writeByte(uint8_t data)
1852 {
1853  TWDR = data;
1854 
1855  this->cmd((1 << TWINT) | (1 << TWEN));
1856 
1857  return this->getStatus() == TXDATAACK;
1858 }
1859 
1861 {
1862  uint16_t i = 0;
1863  // issue stop condition
1864  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1865 
1866  // wait until stop condition is executed and bus released
1867  while (TWCR & (1 << TWSTO));
1868 
1869  status = I2CFREE;
1870 
1871  return 1;
1872 }
1873 
1875 {
1876  return status;
1877 }
1878 
1880 {
1881  // set bit rate register to 12 to obtain 400kHz scl frequency (in combination with no prescaling!)
1882  TWBR = 12;
1883  // no prescaler
1884  TWSR &= 0xFC;
1885 }
1886 
1888 {
1889 
1890 }
#define R
Definition: uStepper.h:247
uStepper(void)
Constructor of uStepper class.
Definition: uStepper.cpp:785
uStepperEncoder(void)
Constructor.
Definition: uStepper.cpp:680
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:741
#define CRUISE
Definition: uStepper.h:211
#define WRITE
Definition: uStepper.h:256
float getSpeed(void)
Measure the current speed of the motor.
Definition: uStepper.cpp:690
volatile uint16_t counter
Definition: uStepper.h:697
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:1763
bool getMotorState(void)
Get the current state of the motor.
Definition: uStepper.cpp:1272
volatile int32_t stepCnt
Definition: uStepper.h:702
Prototype of class for accessing all features of the uStepper in a single object. ...
Definition: uStepper.h:564
void pid(void)
This method handles the actual PID controller calculations, if enabled.
Definition: uStepper.cpp:1526
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:449
#define PWM
Definition: uStepper.h:200
void enableMotor(void)
Enables the stepper driver output stage.
Definition: uStepper.cpp:1257
bool dropIn
Definition: uStepper.h:661
#define A
Definition: uStepper.h:296
float getMaxAcceleration(void)
Get the value of the maximum motor acceleration.
Definition: uStepper.cpp:836
#define CW
Definition: uStepper.h:223
#define DECEL
Definition: uStepper.h:213
float getTemp(void)
Request a reading of current temperature.
Definition: uStepper.cpp:665
#define INTPIDDELAYCONSTANT
Definition: uStepper.h:221
void updateSetPoint(float setPoint)
Updates setpoint for the motor.
Definition: uStepper.cpp:1372
#define RXADDRACK
Definition: uStepper.h:271
#define C
Definition: uStepper.h:300
bool getCurrentDirection(void)
Returns the direction the motor is currently configured to rotate.
Definition: uStepper.cpp:1267
void pidDropIn(void)
This method handles the actual PID controller calculations for drop-in feature, if enabled...
Definition: uStepper.cpp:1382
float getAngleMoved(void)
Measure the angle moved from reference position.
Definition: uStepper.cpp:685
#define ENCODERADDR
Definition: uStepper.h:236
void hardStop(bool holdMode)
Stop the motor without deceleration.
Definition: uStepper.cpp:1117
void setMaxAcceleration(float accel)
Set the maximum acceleration of the stepper motor.
Definition: uStepper.cpp:816
void startTimer(void)
Starts timer for stepper algorithm.
Definition: uStepper.cpp:1243
bool cmd(uint8_t cmd)
Sends commands over the I2C bus.
Definition: uStepper.cpp:1696
void setMaxVelocity(float vel)
Sets the maximum rotational velocity of the motor.
Definition: uStepper.cpp:841
void moveSteps(uint32_t steps, bool dir, bool holdMode)
Make the motor perform a predefined number of steps.
Definition: uStepper.cpp:964
#define STOP
Definition: uStepper.h:207
float getMaxVelocity(void)
Returns the maximum rotational velocity of the motor.
Definition: uStepper.cpp:874
#define DROPIN
Definition: uStepper.h:202
bool writeByte(uint8_t data)
Writes a byte to a device on the I2C bus.
Definition: uStepper.cpp:1851
i2cMaster(void)
Constructor.
Definition: uStepper.cpp:1887
void softStop(bool holdMode)
Stop the motor with deceleration.
Definition: uStepper.cpp:1148
uint8_t detectMagnet(void)
Detect if magnet is present and within range.
Definition: uStepper.cpp:759
volatile int32_t stepsSinceReset
Definition: uStepper.h:637
uint8_t mode
Definition: uStepper.h:731
uint8_t getStatus(void)
Get current I2C status.
Definition: uStepper.cpp:1874
#define READ
Definition: uStepper.h:253
#define STATUS
Definition: uStepper.h:240
void pwmD8(double duty)
Generate PWM signal on digital output 8.
Definition: uStepper.cpp:1294
volatile int32_t angleMoved
Definition: uStepper.h:428
bool restart(uint8_t addr, bool RW)
Restarts connection between arduino and I2C device.
Definition: uStepper.cpp:1846
bool readByte(bool ack, uint8_t *data)
Reads a byte from the I2C bus.
Definition: uStepper.cpp:1796
uStepperEncoder encoder
Definition: uStepper.h:800
volatile uint32_t speedValue[2]
Definition: uStepper.h:712
#define HARD
Definition: uStepper.h:227
volatile uint16_t oldAngle
Definition: uStepper.h:436
void stopTimer(void)
Stops the timer for the stepper algorithm.
Definition: uStepper.cpp:1252
uint8_t getAgc(void)
Read the current AGC value of the encoder chip.
Definition: uStepper.cpp:750
bool start(uint8_t addr, bool RW)
sets up connection between arduino and I2C device.
Definition: uStepper.cpp:1820
#define MAGNITUDE
Definition: uStepper.h:244
void runContinous(bool dir)
Make the motor rotate continuously.
Definition: uStepper.cpp:879
#define INTFREQ
Definition: uStepper.h:217
#define ENCODERSPEEDCONSTANT
Definition: uStepper.h:234
Prototype of class for accessing the TWI (I2C) interface of the AVR (master mode only).
Definition: uStepper.h:1117
volatile int32_t control
Definition: uStepper.h:707
#define ANGLE
Definition: uStepper.h:238
void setHome(void)
Define new reference(home) position.
Definition: uStepper.cpp:722
volatile uint16_t angle
Definition: uStepper.h:440
volatile int16_t revolutions
Definition: uStepper.h:445
#define SOFT
Definition: uStepper.h:229
float getAngle(void)
Measure the current shaft angle.
Definition: uStepper.cpp:736
#define ACK
Definition: uStepper.h:274
#define B
Definition: uStepper.h:298
#define INTPERIOD
Definition: uStepper.h:219
bool stop(void)
Closes the I2C connection.
Definition: uStepper.cpp:1860
#define AGC
Definition: uStepper.h:242
i2cMaster I2C
Definition: uStepper.cpp:78
#define PID
Definition: uStepper.h:204
#define I2CFREE
Definition: uStepper.h:250
#define REPSTART
Definition: uStepper.h:262
void begin(void)
Setup TWI (I2C) interface.
Definition: uStepper.cpp:1879
uint16_t encoderOffset
Definition: uStepper.h:431
void interrupt1(void)
Used by dropin feature to take in enable signal.
Definition: uStepper.cpp:82
#define TXDATAACK
Definition: uStepper.h:268
#define NACK
Definition: uStepper.h:277
Function prototypes and definitions for the uStepper library.
#define ACCEL
Definition: uStepper.h:209
#define INITDECEL
Definition: uStepper.h:215
int32_t getStepsSinceReset(void)
Get the number of steps applied since reset.
Definition: uStepper.cpp:1282
void setup(uint8_t mode)
Setup the encoder.
Definition: uStepper.cpp:695
void pwmD3(double duty)
Generate PWM signal on digital output 3.
Definition: uStepper.cpp:1333
uint8_t state
Definition: uStepper.h:581
uStepperTemp(void)
Constructor.
Definition: uStepper.cpp:660
uint16_t delay
Definition: uStepper.h:657
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:1717
#define START
Definition: uStepper.h:259
void disableMotor(void)
Disables the stepper driver output stage.
Definition: uStepper.cpp:1262
#define TXADDRACK
Definition: uStepper.h:265
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:1196