uStepper
uStepper.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepper.cpp *
3 * Version: 1.2.1 *
4 * date: April 2, 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::pwmD8(double duty)
1296 {
1297  if(!(TCCR1A & (1 << COM1B1)))
1298  {
1299  pinMode(8,OUTPUT);
1300  TCCR1A |= (1 << COM1B1);
1301  }
1302 
1303  if(duty > 100.0)
1304  {
1305  duty = 100.0;
1306  }
1307  else if(duty < 0.0)
1308  {
1309  duty = 0.0;
1310  }
1311 
1312  duty *= 160.0;
1313 
1314  OCR1B = (uint16_t)(duty + 0.5);
1315 }
1316 
1317 void uStepper::pwmD8(int mode)
1318 {
1319  if(mode == PWM)
1320  {
1321  if(!(TCCR1A & (1 << COM1B1)))
1322  {
1323  pinMode(8,OUTPUT);
1324  TCCR1A |= (1 << COM1B1);
1325  }
1326  }
1327  else
1328  {
1329  pinMode(8,INPUT);
1330  TCCR1A &= ~(1 << COM1B1);
1331  }
1332 }
1333 
1334 void uStepper::pwmD3(double duty)
1335 {
1336  if(!(TCCR2A & (1 << COM2B1)))
1337  {
1338  pinMode(3,OUTPUT);
1339  TCCR2A |= (1 << COM2B1);
1340  }
1341 
1342  if(duty > 100.0)
1343  {
1344  duty = 100.0;
1345  }
1346  else if(duty < 0.0)
1347  {
1348  duty = 0.0;
1349  }
1350 
1351  duty *= 0.7;
1352 
1353  OCR2B = (uint16_t)(duty + 0.5);
1354 }
1355 
1356 void uStepper::pwmD3(int mode)
1357 {
1358  if(mode == PWM)
1359  {
1360  if(!(TCCR2A & (1 << COM2B1)))
1361  {
1362  pinMode(3,OUTPUT);
1363  TCCR2A |= (1 << COM2B1);
1364  }
1365  }
1366  else
1367  {
1368  pinMode(3,INPUT);
1369  TCCR2A &= ~(1 << COM2B1);
1370  }
1371 }
1372 
1373 void uStepper::updateSetPoint(float setPoint)
1374 {
1375  if(this->mode != DROPIN)
1376  {
1377  return; //This function doesn't make sense in any other configuration than dropin
1378  }
1379 
1380  this->stepCnt = (int32_t)(setPoint*this->angleToStep);
1381 }
1382 
1384 {
1385  static float oldError = 0.0;
1386  float integral;
1387  float output = 1.0;
1388  static float accumError = 0.0;
1389  uint8_t data[2];
1390  uint16_t curAngle;
1391  int16_t deltaAngle;
1392  float error;
1393  static uint32_t speed = 10000;
1394  static uint32_t oldMicros = 0;
1395 
1396  sei();
1397 
1398  I2C.read(ENCODERADDR, ANGLE, 2, data);
1399  cli();
1400  error = (float)this->stepCnt;
1401  if(this->speedValue[0] == oldMicros)
1402  {
1403  speed += 2000;
1404  if(speed > 10000)
1405  {
1406  speed = 10000;
1407  }
1408  }
1409  else
1410  {
1411  speed = this->speedValue[0] - this->speedValue[1];
1412  }
1413  sei();
1414 
1415  curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1416  this->encoder.angle = curAngle;
1417  curAngle -= this->encoder.encoderOffset;
1418  if(curAngle > 4095)
1419  {
1420  curAngle -= 61440;
1421  }
1422 
1423  deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1424 
1425  if(deltaAngle < -2047)
1426  {
1427  this->encoder.revolutions--;
1428  deltaAngle += 4096;
1429  }
1430 
1431  else if(deltaAngle > 2047)
1432  {
1433  this->encoder.revolutions++;
1434  deltaAngle -= 4096;
1435  }
1436 
1437  this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1438  this->encoder.oldAngle = curAngle;
1439 
1440  error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1441 
1442  if(error < -this->tolerance)
1443  {
1444  cli(); //Do Atomic copy, in order to not fuck up variables
1445  this->control = (int32_t)error; //Move current error into control variable, for Timer2 and int0 routines to decide who should provide steps
1446  sei();
1447 
1448  error = -error; //error variable should always be positive when calculating controller output
1449 
1450  integral = error*this->iTerm; //Multiply current error by integral term
1451  accumError += integral; //And accumulate, to get integral action
1452 
1453  //The output of each PID part, should be subtracted from the output variable.
1454  //This is true, since in case of no error the motor should run at a higher speed
1455  //to catch up, and since the speed variable contains the number of microseconds
1456  //between each step, the we need to multiply with a number < 1 to increase speed
1457  output -= this->pTerm*error;
1458  output -= accumError;
1459  output -= this->dTerm*(error - oldError);
1460 
1461  oldError = error; //Save current error for next sample, for use in differential part
1462 
1463  PORTD &= ~(1 << 7); //change direction to CW
1464 
1465  output *= (float)speed; //Calculate stepSpeed
1466 
1467  if(output < 54.0) //If stepSpeed is lower than possible
1468  {
1469  output = 54.0; //Set stepSpeed to lowest possible
1470  accumError -= integral; //and subtract current integral part from accumerror (anti-windup)
1471  }
1472 
1473  cli(); //Atomic copy
1474  this->delay = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5); //calculate Number of interrupt Timer 2 should do before generating step pulse
1475  sei();
1476 
1477  this->startTimer();
1478  PORTB &= ~(1 << 0);
1479  }
1480 
1481  else if(error > this->tolerance)
1482  {
1483  cli(); //Do Atomic copy, in order to not fuck up variables
1484  this->control = (int32_t)error;
1485  sei();
1486 
1487  integral = error*this->iTerm;
1488  accumError += integral;
1489 
1490  output -= this->pTerm*error;
1491  output -= accumError;
1492  output -= this->dTerm*(error - oldError);
1493 
1494  oldError = error;
1495 
1496  PORTD |= (1 << 7); //change direction to CCW
1497 
1498  output *= (float)speed;
1499  output = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5);
1500 
1501  if(output < cruiseDelay)
1502  {
1503  output = cruiseDelay;
1504  accumError -= integral;
1505  }
1506 
1507  cli();
1508  this->delay = output;
1509  sei();
1510 
1511  this->startTimer();
1512  PORTB &= ~(1 << 0);
1513  }
1514 
1515  else
1516  {
1517  if(error >= -this->hysteresis && error <= this->hysteresis) //If error is less than 1 step
1518  {
1519  PORTB |= (PIND & 0x04) >> 2; //Set enable pin to whatever is demanded by the 3Dprinter controller
1520 
1521  this->control = 0; //Set control variable to 0, in order to make sure int0 routine generates step pulses
1522  accumError = 0.0; //Clear accumerror
1523 
1524  this->stopTimer(); //Stop timer 2
1525  }
1526  }
1527 }
1528 
1529 void uStepper::pid(void)
1530 {
1531  static float oldError = 0.0;
1532  float integral;
1533  float output = 1.0;
1534  static float accumError = 0.0;
1535  float limit;
1536  uint8_t data[2];
1537  uint16_t curAngle;
1538  int16_t deltaAngle;
1539  float error;
1540  static uint32_t speed = 10000;
1541  static uint32_t oldMicros = 0;
1542 
1543  sei();
1544  if(I2C.getStatus() != I2CFREE)
1545  {
1546  return;
1547  }
1548 
1549  I2C.read(ENCODERADDR, ANGLE, 2, data);
1550  cli();
1551  error = (float)this->stepsSinceReset;
1552  if(this->exactDelay.getFloatValue() >= 1.0)
1553  {
1554  speed = (uint32_t)((this->exactDelay.getFloatValue() * INTPERIOD));
1555  }
1556  else
1557  {
1558  speed = 10000;
1559  }
1560 
1561  if(speed > 10000)
1562  {
1563  speed = 10000;
1564  }
1565  sei();
1566 
1567  curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1568  this->encoder.angle = curAngle;
1569  curAngle -= this->encoder.encoderOffset;
1570  if(curAngle > 4095)
1571  {
1572  curAngle -= 61440;
1573  }
1574 
1575  deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1576 
1577  if(deltaAngle < -2047)
1578  {
1579  this->encoder.revolutions--;
1580  deltaAngle += 4096;
1581  }
1582 
1583  else if(deltaAngle > 2047)
1584  {
1585  this->encoder.revolutions++;
1586  deltaAngle -= 4096;
1587  }
1588 
1589  this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1590  this->encoder.oldAngle = curAngle;
1591 
1592  error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1593 
1594  if(error < -this->tolerance)
1595  {
1596  cli(); //Do Atomic copy, in order to not fuck up variables
1597  this->control = (int32_t)error; //Move current error into control variable, for Timer2 and int0 routines to decide who should provide steps
1598  sei();
1599 
1600  error = -error; //error variable should always be positive when calculating controller output
1601 
1602  integral = error*this->iTerm; //Multiply current error by integral term
1603  accumError += integral; //And accumulate, to get integral action
1604 
1605  //The output of each PID part, should be subtracted from the output variable.
1606  //This is true, since in case of no error the motor should run at a higher speed
1607  //to catch up, and since the speed variable contains the number of microseconds
1608  //between each step, the we need to multiply with a number < 1 to increase speed
1609  output -= this->pTerm*error;
1610  output -= accumError;
1611  output -= this->dTerm*(error - oldError);
1612 
1613  oldError = error; //Save current error for next sample, for use in differential part
1614 
1615  PORTD &= ~(1 << 7); //change direction to CW
1616 
1617  output *= (float)speed;
1618  output = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5);
1619 
1620  if(output < cruiseDelay)
1621  {
1622  accumError -= cruiseDelay-output;
1623  output = cruiseDelay;
1624  }
1625 
1626  cli();
1627  this->delay = output;
1628  sei();
1629 
1630  this->startTimer();
1631  PORTB &= ~(1 << 0);
1632  }
1633 
1634  else if(error > this->tolerance)
1635  {
1636  cli(); //Do Atomic copy, in order to not fuck up variables
1637  this->control = (int32_t)error;
1638  sei();
1639 
1640  integral = error*this->iTerm;
1641  accumError += integral;
1642 
1643  output -= this->pTerm*error;
1644  output -= accumError;
1645  output -= this->dTerm*(error - oldError);
1646 
1647  oldError = error;
1648 
1649  PORTD |= (1 << 7); //change direction to CCW
1650 
1651  output *= (float)speed;
1652  output = (uint16_t)((output*INTPIDDELAYCONSTANT) - 0.5);
1653 
1654  if(output < cruiseDelay)
1655  {
1656  accumError -= cruiseDelay-output;
1657  output = cruiseDelay;
1658  }
1659 
1660  cli();
1661  this->delay = output;
1662  sei();
1663 
1664  this->startTimer();
1665  PORTB &= ~(1 << 0);
1666  }
1667 
1668  else
1669  {
1670  if(error >= -this->hysteresis && error <= this->hysteresis) //If error is less than 1 step
1671  {
1672  cli();
1673  if(this->hold || this->state)
1674  {
1675  PORTB &= ~(1 << 0);
1676  }
1677  else
1678  {
1679  PORTB |= (1 << 0);
1680  }
1681 
1682  if(this->direction)
1683  {
1684  PORTD |= (1 << 7); //change direction to CCW
1685  }
1686  else
1687  {
1688  PORTD &= ~(1 << 7); //change direction to CW
1689  }
1690  sei();
1691  this->control = 0; //Set control variable to 0, in order to make sure int0 routine generates step pulses
1692  accumError = 0.0; //Clear accumerror
1693 
1694  if(!this->state)
1695  {
1696  this->stopTimer(); //Stop timer 2
1697  }
1698  }
1699  }
1700 }
1701 
1702 bool i2cMaster::cmd(uint8_t cmd)
1703 {
1704  uint16_t i = 0;
1705  // send command
1706  TWCR = cmd;
1707  // wait for command to complete
1708  while (!(TWCR & (1 << TWINT)))
1709  {
1710  i++;
1711  if(i == 65000)
1712  {
1713  return false;
1714  }
1715  }
1716 
1717  // save status bits
1718  status = TWSR & 0xF8;
1719 
1720  return true;
1721 }
1722 
1723 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1724 {
1725  uint8_t i, buff[numOfBytes];
1726 
1727  TIMSK1 &= ~(1 << OCIE1A);
1728 
1729  if(I2C.start(slaveAddr, WRITE) == false)
1730  {
1731  I2C.stop();
1732  return false;
1733  }
1734 
1735  if(I2C.writeByte(regAddr) == false)
1736  {
1737  I2C.stop();
1738  return false;
1739  }
1740 
1741  if(I2C.restart(slaveAddr, READ) == false)
1742  {
1743  I2C.stop();
1744  return false;
1745  }
1746 
1747  for(i = 0; i < (numOfBytes - 1); i++)
1748  {
1749  if(I2C.readByte(ACK, &data[i]) == false)
1750  {
1751  I2C.stop();
1752  return false;
1753  }
1754  }
1755 
1756  if(I2C.readByte(NACK, &data[numOfBytes-1]) == false)
1757  {
1758  I2C.stop();
1759  return false;
1760  }
1761 
1762  I2C.stop();
1763 
1764  TIMSK1 |= (1 << OCIE1A);
1765 
1766  return 1;
1767 }
1768 
1769 bool i2cMaster::write(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1770 {
1771  uint8_t i;
1772 
1773  TIMSK1 &= ~(1 << OCIE1A);
1774 
1775  if(I2C.start(slaveAddr, WRITE) == false)
1776  {
1777  I2C.stop();
1778  return false;
1779  }
1780 
1781  if(I2C.writeByte(regAddr) == false)
1782  {
1783  I2C.stop();
1784  return false;
1785  }
1786 
1787  for(i = 0; i < numOfBytes; i++)
1788  {
1789  if(I2C.writeByte(*(data + i)) == false)
1790  {
1791  I2C.stop();
1792  return false;
1793  }
1794  }
1795  I2C.stop();
1796 
1797  TIMSK1 |= (1 << OCIE1A);
1798 
1799  return 1;
1800 }
1801 
1802 bool i2cMaster::readByte(bool ack, uint8_t *data)
1803 {
1804  if(ack)
1805  {
1806  if(this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA)) == false)
1807  {
1808  return false;
1809  }
1810 
1811  }
1812 
1813  else
1814  {
1815  if(this->cmd((1 << TWINT) | (1 << TWEN)) == false)
1816  {
1817  return false;
1818  }
1819  }
1820 
1821  *data = TWDR;
1822 
1823  return true;
1824 }
1825 
1826 bool i2cMaster::start(uint8_t addr, bool RW)
1827 {
1828  // send START condition
1829  this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1830 
1831  if (this->getStatus() != START && this->getStatus() != REPSTART)
1832  {
1833  return false;
1834  }
1835 
1836  // send device address and direction
1837  TWDR = (addr << 1) | RW;
1838  this->cmd((1 << TWINT) | (1 << TWEN));
1839 
1840  if (RW == READ)
1841  {
1842 
1843  return this->getStatus() == RXADDRACK;
1844  }
1845 
1846  else
1847  {
1848  return this->getStatus() == TXADDRACK;
1849  }
1850 }
1851 
1852 bool i2cMaster::restart(uint8_t addr, bool RW)
1853 {
1854  return this->start(addr, RW);
1855 }
1856 
1857 bool i2cMaster::writeByte(uint8_t data)
1858 {
1859  TWDR = data;
1860 
1861  this->cmd((1 << TWINT) | (1 << TWEN));
1862 
1863  return this->getStatus() == TXDATAACK;
1864 }
1865 
1867 {
1868  uint16_t i = 0;
1869  // issue stop condition
1870  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1871 
1872  // wait until stop condition is executed and bus released
1873  while (TWCR & (1 << TWSTO));
1874 
1875  status = I2CFREE;
1876 
1877  return 1;
1878 }
1879 
1881 {
1882  return status;
1883 }
1884 
1886 {
1887  // set bit rate register to 12 to obtain 400kHz scl frequency (in combination with no prescaling!)
1888  TWBR = 12;
1889  // no prescaler
1890  TWSR &= 0xFC;
1891 }
1892 
1894 {
1895 
1896 }
#define R
Definition: uStepper.h:251
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:215
#define WRITE
Definition: uStepper.h:260
float getSpeed(void)
Measure the current speed of the motor.
Definition: uStepper.cpp:690
volatile uint16_t counter
Definition: uStepper.h:701
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:1769
bool getMotorState(void)
Get the current state of the motor.
Definition: uStepper.cpp:1273
volatile int32_t stepCnt
Definition: uStepper.h:706
Prototype of class for accessing all features of the uStepper in a single object. ...
Definition: uStepper.h:568
void pid(void)
This method handles the actual PID controller calculations, if enabled.
Definition: uStepper.cpp:1529
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:453
#define PWM
Definition: uStepper.h:204
void enableMotor(void)
Enables the stepper driver output stage.
Definition: uStepper.cpp:1258
bool dropIn
Definition: uStepper.h:665
#define A
Definition: uStepper.h:300
float getMaxAcceleration(void)
Get the value of the maximum motor acceleration.
Definition: uStepper.cpp:837
#define CW
Definition: uStepper.h:227
#define DECEL
Definition: uStepper.h:217
float getTemp(void)
Request a reading of current temperature.
Definition: uStepper.cpp:665
#define INTPIDDELAYCONSTANT
Definition: uStepper.h:225
void updateSetPoint(float setPoint)
Updates setpoint for the motor.
Definition: uStepper.cpp:1373
#define RXADDRACK
Definition: uStepper.h:275
#define C
Definition: uStepper.h:304
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:1383
float getAngleMoved(void)
Measure the angle moved from reference position.
Definition: uStepper.cpp:685
#define ENCODERADDR
Definition: uStepper.h:240
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:1702
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:211
float getMaxVelocity(void)
Returns the maximum rotational velocity of the motor.
Definition: uStepper.cpp:875
#define DROPIN
Definition: uStepper.h:206
bool writeByte(uint8_t data)
Writes a byte to a device on the I2C bus.
Definition: uStepper.cpp:1857
i2cMaster(void)
Constructor.
Definition: uStepper.cpp:1893
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:641
uint8_t mode
Definition: uStepper.h:735
uint8_t getStatus(void)
Get current I2C status.
Definition: uStepper.cpp:1880
#define READ
Definition: uStepper.h:257
#define STATUS
Definition: uStepper.h:244
void pwmD8(double duty)
Generate PWM signal on digital output 8.
Definition: uStepper.cpp:1295
volatile int32_t angleMoved
Definition: uStepper.h:432
bool restart(uint8_t addr, bool RW)
Restarts connection between arduino and I2C device.
Definition: uStepper.cpp:1852
bool readByte(bool ack, uint8_t *data)
Reads a byte from the I2C bus.
Definition: uStepper.cpp:1802
uStepperEncoder encoder
Definition: uStepper.h:805
volatile uint32_t speedValue[2]
Definition: uStepper.h:716
#define HARD
Definition: uStepper.h:231
volatile uint16_t oldAngle
Definition: uStepper.h:440
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:1826
#define MAGNITUDE
Definition: uStepper.h:248
void runContinous(bool dir)
Make the motor rotate continuously.
Definition: uStepper.cpp:880
#define INTFREQ
Definition: uStepper.h:221
#define ENCODERSPEEDCONSTANT
Definition: uStepper.h:238
Prototype of class for accessing the TWI (I2C) interface of the AVR (master mode only).
Definition: uStepper.h:1122
volatile int32_t control
Definition: uStepper.h:711
#define ANGLE
Definition: uStepper.h:242
void setHome(void)
Define new reference(home) position.
Definition: uStepper.cpp:722
volatile uint16_t angle
Definition: uStepper.h:444
volatile int16_t revolutions
Definition: uStepper.h:449
#define SOFT
Definition: uStepper.h:233
float getAngle(void)
Measure the current shaft angle.
Definition: uStepper.cpp:737
#define ACK
Definition: uStepper.h:278
#define B
Definition: uStepper.h:302
#define INTPERIOD
Definition: uStepper.h:223
bool stop(void)
Closes the I2C connection.
Definition: uStepper.cpp:1866
#define AGC
Definition: uStepper.h:246
i2cMaster I2C
Definition: uStepper.cpp:78
#define PID
Definition: uStepper.h:208
#define I2CFREE
Definition: uStepper.h:254
#define REPSTART
Definition: uStepper.h:266
void begin(void)
Setup TWI (I2C) interface.
Definition: uStepper.cpp:1885
uint16_t encoderOffset
Definition: uStepper.h:435
void interrupt1(void)
Used by dropin feature to take in enable signal.
Definition: uStepper.cpp:82
#define TXDATAACK
Definition: uStepper.h:272
#define NACK
Definition: uStepper.h:281
Function prototypes and definitions for the uStepper library.
#define ACCEL
Definition: uStepper.h:213
#define INITDECEL
Definition: uStepper.h:219
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:1334
uint8_t state
Definition: uStepper.h:585
uStepperTemp(void)
Constructor.
Definition: uStepper.cpp:660
uint16_t delay
Definition: uStepper.h:661
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:1723
#define START
Definition: uStepper.h:263
void disableMotor(void)
Disables the stepper driver output stage.
Definition: uStepper.cpp:1263
#define TXADDRACK
Definition: uStepper.h:269
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