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