uStepper
 All Classes Files Functions Variables Macros Pages
uStepper.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepper.cpp *
3 * Version: 0.4.0 *
4 * date: July 30th, 2016 *
5 * Author: Thomas Hørring Olsen *
6 * *
7 *********************************************************************************************
8 * uStepper class *
9 * *
10 * This file contains the implementation of the class methods, incorporated in the *
11 * uStepper arduino library. The library is used by instantiating an uStepper object *
12 * by calling either of the two overloaded constructors: *
13 * *
14 * example: *
15 * *
16 * uStepper stepper; *
17 * *
18 * OR *
19 * *
20 * uStepper stepper(500, 2000); *
21 * *
22 * The first instantiation above creates a uStepper object with default acceleration *
23 * and maximum speed (1000 steps/s^2 and 1000steps/s respectively). *
24 * The second instantiation overwrites the default settings of acceleration and *
25 * maximum speed (in this case 500 steps/s^2 and 2000 steps/s, respectively); *
26 * *
27 * after instantiation of the object, the object setup function should be called within *
28 * arduino's setup function: *
29 * *
30 * example: *
31 * *
32 * uStepper stepper; *
33 * *
34 * void setup() *
35 * { *
36 * stepper.setup(); *
37 * } *
38 * *
39 * void loop() *
40 * { *
41 * *
42 * } *
43 * *
44 * After this, the library is ready to control the motor! *
45 * *
46 *********************************************************************************************
47 * (C) 2016 *
48 * *
49 * ON Development IVS *
50 * www.on-development.com *
51 * administration@on-development.com *
52 * *
53 * The code contained in this file is released under the following open source license: *
54 * *
55 * Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International *
56 * *
57 * The code in this file is provided without warranty of any kind - use at own risk! *
58 * neither ON Development IVS nor the author, can be held responsible for any damage *
59 * caused by the use of the code contained in this file ! *
60 * *
61 ********************************************************************************************/
69 #include <uStepper.h>
70 #include <math.h>
71 
72 uStepper *pointer;
74 
75 volatile uint16_t i = 0;
76 volatile int32_t stepCnt = 0, control = 0;
77 
78 extern "C" {
79 
80  void INT1_vect(void)
81  {
82  if((PIND & (0x20))) //CCW
83  {
84  if(control == 0)
85  {
86  PORTD |= (1 << 7); //Set dir to CCW
87 
88  }
89  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)
90  }
91  else //CW
92  {
93  if(control == 0)
94  {
95 
96  PORTD &= ~(1 << 7); //Set dir to CW
97  }
98 
99  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)
100  }
101 
102  if(control == 0) //If no steps are lost, redirect step pulses
103  {
104  PORTD |= (1 << 4); //generate step pulse
105  delayMicroseconds(1); //wait a small time
106  PORTD &= ~(1 << 4); //pull step pin low again
107  }
108  }
109 
110  void INT0_vect(void)
111  {
112 
113  if(PIND & 0x04)
114  {
115 
116  PORTB |= (1 << 0);
117  }
118  else
119  {
120 
121  PORTB &= ~(1 << 0);
122  }
123  }
124 
125  void TIMER2_COMPA_vect(void)
126  {
127  asm volatile("push r16 \n\t");
128  asm volatile("in r16,0x3F \n\t");
129  asm volatile("push r16 \n\t");
130  asm volatile("push r30 \n\t");
131  asm volatile("push r31 \n\t");
132  asm volatile("lds r30,pointer \n\t");
133  asm volatile("lds r31,pointer+1 \n\t");
134  asm volatile("ldd r16,z+56 \n\t");
135  asm volatile("sbrs r16,0 \n\t");
136  asm volatile("jmp _AccelerationAlgorithm \n\t"); //Execute the acceleration profile algorithm
137 
138  asm volatile("push r0 \n\t");
139  asm volatile("push r1 \n\t");
140  asm volatile("push r2 \n\t");
141  asm volatile("push r3 \n\t");
142  asm volatile("push r4 \n\t");
143  asm volatile("push r5 \n\t");
144  asm volatile("push r6 \n\t");
145  asm volatile("push r7 \n\t");
146  asm volatile("push r8 \n\t");
147  asm volatile("push r9 \n\t");
148  asm volatile("push r10 \n\t");
149  asm volatile("push r11 \n\t");
150  asm volatile("push r12 \n\t");
151  asm volatile("push r13 \n\t");
152  asm volatile("push r14 \n\t");
153  asm volatile("push r15 \n\t");
154  asm volatile("push r17 \n\t");
155  asm volatile("push r18 \n\t");
156  asm volatile("push r19 \n\t");
157  asm volatile("push r20 \n\t");
158  asm volatile("push r21 \n\t");
159  asm volatile("push r22 \n\t");
160  asm volatile("push r23 \n\t");
161  asm volatile("push r24 \n\t");
162  asm volatile("push r25 \n\t");
163  asm volatile("push r26 \n\t");
164  asm volatile("push r27 \n\t");
165  asm volatile("push r28 \n\t");
166  asm volatile("push r29 \n\t");
167 
168  if(i < pointer->faultStepDelay) //This value defines the speed at which the motor rotates when compensating for lost steps. This value should be tweaked to the application
169  {
170 
171  i++;
172  }
173  else
174  {
175  PORTD |= (1 << 4);
176  delayMicroseconds(1);
177  PORTD &= ~(1 << 4);
178  if(control < 0)
179  {
180  control += 1;
181  }
182 
183  else if(control > 0)
184  {
185  control -= 1;
186  }
187  i = 0;
188  }
189 
190  asm volatile("pop r29 \n\t");
191  asm volatile("pop r28 \n\t");
192  asm volatile("pop r27 \n\t");
193  asm volatile("pop r26 \n\t");
194  asm volatile("pop r25 \n\t");
195  asm volatile("pop r24 \n\t");
196  asm volatile("pop r23 \n\t");
197  asm volatile("pop r22 \n\t");
198  asm volatile("pop r21 \n\t");
199  asm volatile("pop r20 \n\t");
200  asm volatile("pop r19 \n\t");
201  asm volatile("pop r18 \n\t");
202  asm volatile("pop r17 \n\t");
203  asm volatile("pop r15 \n\t");
204  asm volatile("pop r14 \n\t");
205  asm volatile("pop r13 \n\t");
206  asm volatile("pop r12 \n\t");
207  asm volatile("pop r11 \n\t");
208  asm volatile("pop r10 \n\t");
209  asm volatile("pop r9 \n\t");
210  asm volatile("pop r8 \n\t");
211  asm volatile("pop r7 \n\t");
212  asm volatile("pop r6 \n\t");
213  asm volatile("pop r5 \n\t");
214  asm volatile("pop r4 \n\t");
215  asm volatile("pop r3 \n\t");
216  asm volatile("pop r2 \n\t");
217  asm volatile("pop r1 \n\t");
218  asm volatile("pop r0 \n\t");
219  asm volatile("pop r31 \n\t");
220  asm volatile("pop r30 \n\t");
221  asm volatile("pop r16 \n\t");
222  asm volatile("out 0x3F,r16 \n\t");
223  asm volatile("pop r16 \n\t");
224  asm volatile("reti \n\t");
225  }
226 
227  void TIMER1_COMPA_vect(void)
228  {
229  float error;
230  float deltaAngle, newSpeed;
231  static float curAngle, oldAngle = 0.0, deltaSpeedAngle = 0.0, oldSpeed = 0.0;
232  static uint8_t loops = 0;
233  static float revolutions = 0.0;
234  uint8_t data[2];
235 
236  sei();
237  if(I2C.getStatus() != I2CFREE)
238  {
239  return;
240  }
241  I2C.read(ENCODERADDR, ANGLE, 2, data);
242  curAngle = (float)((((uint16_t)data[0]) << 8 )| (uint16_t)data[1])*0.087890625;
243  pointer->encoder.angle = curAngle;
244  curAngle -= pointer->encoder.encoderOffset;
245 
246  curAngle = fmod(curAngle + 360.0, 360.0);
247 
248  deltaAngle = (oldAngle - curAngle);
249  //count number of revolutions, on angle overflow
250  if(deltaAngle < -180.0)
251  {
252  revolutions -= 1.0;
253  deltaAngle += 360;
254  }
255 
256  else if(deltaAngle > 180.0)
257  {
258  revolutions += 1.0;
259  deltaAngle -= 360.0;
260  }
261 
262  if( loops < 10)
263  {
264  loops++;
265  deltaSpeedAngle += deltaAngle;
266  }
267  else
268  {
269  newSpeed = (deltaSpeedAngle*ENCODERSPEEDCONSTANT);
270  newSpeed = oldSpeed*ALPHA + newSpeed*BETA; //Filter
271  oldSpeed = newSpeed;
272  pointer->encoder.curSpeed = newSpeed;
273  loops = 0;
274  deltaSpeedAngle = 0.0;
275  }
276 
277  pointer->encoder.angleMoved = curAngle + (360.0*revolutions);
278  oldAngle = curAngle;
279  pointer->encoder.oldAngle = curAngle;
280 
281  if(pointer->dropIn)
282  {
283  cli();
284  error = (float)stepCnt; //atomic read to ensure no fuckups happen from the external interrupt routine
285  sei();
286  error *= pointer->stepResolution;
287  error -= pointer->encoder.angleMoved;
288  if(error > pointer->tolerance)
289  {
290  PORTB &= ~(1 << 0);
291  control = (int32_t)(error/pointer->stepResolution);
292 
293  PORTD |= (1 << 7);
294  pointer->startTimer();
295  }
296  else if(error < -pointer->tolerance)
297  {
298  PORTB &= ~(1 << 0);
299  control = (int32_t)(error/pointer->stepResolution);
300  PORTD &= ~(1 << 7);
301 
302  pointer->startTimer();
303  }
304  else
305  {
306  PORTB |= (PIND & 0x04) >> 2;
307  control = 0;
308  pointer->stopTimer();
309  }
310  }
311  }
312 }
313 
314 float2::float2(void)
315 {
316 
317 }
318 
319 float float2::getFloatValue(void)
320 {
321  union
322  {
323  float f;
324  uint32_t i;
325  } a;
326 
327  a.i = (uint32_t)(this->value >> 25);
328 
329  return a.f;
330 }
331 
332 uint64_t float2::getRawValue(void)
333 {
334  return this->value;
335 }
336 
337 void float2::setValue(float val)
338 {
339  union
340  {
341  float f;
342  uint32_t i;
343  } a;
344 
345  a.f = val;
346 
347  this->value = ((uint64_t)a.i) << 25;
348 }
349 
350 bool float2::operator<=(const float &value)
351 {
352  if(this->getFloatValue() > value)
353  {
354  return 0;
355  }
356 
357  if(this->getFloatValue() == value)
358  {
359  if((this->value & 0x0000000000007FFF) > 0)
360  {
361  return 0;
362  }
363  }
364 
365  return 1;
366 }
367 
368 bool float2::operator<=(const float2 &value)
369 {
370  if((this->value >> 56) > (value.value >> 56)) // sign bit of "this" bigger than sign bit of "value"?
371  {
372  return 1; //"This" is negative while "value" is not. ==> "this" < "value"
373  }
374 
375  if((this->value >> 56) == (value.value >> 56)) //Sign bit of "this" == sign bit of "value"?
376  {
377  if( (this->value >> 48) < (value.value >> 48) ) //Exponent of "this" < exponent of "value"?
378  {
379  return 1; //==> "this" is smaller than "value"
380  }
381 
382  if( (this->value >> 48) == (value.value >> 48) ) //Exponent of "this" == exponent of "value"?
383  {
384  if((this->value & 0x0000FFFFFFFFFFFF) <= (value.value & 0x0000FFFFFFFFFFFF)) //mantissa of "this" <= mantissa of "value"?
385  {
386  return 1; //==> "this" <= "value"
387  }
388  }
389  }
390 
391  return 0; //"this" > "value"
392 }
393 
394 float2 & float2::operator=(const float &value)
395 {
396  this->setValue(value);
397 
398  return *this;
399 }
400 
401 float2 & float2::operator+=(const float &value)
402 {
403 
404 }
405 
406 float2 & float2::operator+=(const float2 &value)
407 {
408  float2 temp = value;
409  uint64_t tempMant, tempExp;
410  uint8_t cnt; //how many times should we shift the mantissa of the smallest number to add the two mantissa's
411 
412  if((this->value >> 56) == (temp.value >> 56))
413  {
414  if(*this <= temp)
415  {
416  cnt = (temp.value >> 48) - (this->value >> 48);
417  if(cnt < 48)
418  {
419  tempExp = (temp.value >> 48);
420 
421  this->value &= 0x0000FFFFFFFFFFFF;
422  this->value |= 0x0001000000000000;
423  this->value >>= cnt;
424 
425  tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
426  tempMant += this->value;
427 
428  while(tempMant > 0x2000000000000)
429  {
430  tempMant >>= 1;
431  tempExp++;
432  }
433 
434  tempMant &= 0x0000FFFFFFFFFFFF;
435  this->value = (tempExp << 48) | tempMant;
436  }
437  else
438  {
439  this->value = temp.value;
440  }
441  }
442 
443  else
444  {
445  cnt = (this->value >> 48) - (temp.value >> 48);
446 
447  if(cnt < 48)
448  {
449  tempExp = (this->value >> 48);
450 
451  temp.value &= 0x0000FFFFFFFFFFFF;
452  temp.value |= 0x0001000000000000;
453  temp.value >>= cnt;
454 
455  tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
456  tempMant += temp.value;
457 
458  while(tempMant > 0x2000000000000)
459  {
460  tempMant >>= 1;
461  tempExp++;
462  }
463 
464  tempMant &= 0x0000FFFFFFFFFFFF;
465  this->value = (tempExp << 48) | tempMant;
466  }
467  }
468  }
469 
470  else if((this->value >> 56) == 1)
471  {
472  this->value &= 0x00FFFFFFFFFFFFFF; //clear sign bit, to consider absolute value
473 
474  if(*this <= temp)
475  {
476  cnt = (temp.value >> 48) - (this->value >> 48);
477 
478  if(cnt < 48)
479  {
480  tempExp = (temp.value >> 48);
481 
482  this->value &= 0x0000FFFFFFFFFFFF;
483  this->value |= 0x0001000000000000;
484  this->value >>= cnt;
485 
486  tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
487 
488  tempMant -= this->value;
489 
490  if(tempMant > 0x8000000000000000)
491  {
492 
493  tempMant &= 0x0000FFFFFFFFFFFF;
494  tempExp--;
495  }
496 
497  while(tempMant < 0x1000000000000)
498  {
499  tempMant <<= 1;
500  tempExp--;
501  }
502 
503  tempMant &= 0x0000FFFFFFFFFFFF;
504 
505  this->value = (tempExp << 48) | tempMant;
506  }
507 
508  else
509  {
510  this->value = temp.value;
511  }
512  }
513 
514  else
515  {
516  cnt = (this->value >> 48) - (temp.value >> 48);
517  if(cnt < 48)
518  {
519  tempExp = (this->value >> 48);
520 
521  temp.value &= 0x0000FFFFFFFFFFFF;
522  temp.value |= 0x0001000000000000;
523  temp.value >>= cnt;
524 
525  tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
526 
527  tempMant -= temp.value;
528 
529  if(tempMant > 0x8000000000000000)
530  {
531  tempMant &= 0x0000FFFFFFFFFFFF;
532  tempExp--;
533  }
534 
535  while(tempMant < 0x1000000000000)
536  {
537  tempMant <<= 1;
538  tempExp--;
539  }
540 
541  tempMant &= 0x0000FFFFFFFFFFFF;
542 
543  this->value = (tempExp << 48) | tempMant;
544  this->value |= 0x0100000000000000;
545  }
546  }
547  }
548 
549  else
550  {
551  temp.value &= 0x00FFFFFFFFFFFFFF; //clear sign bit, to consider absolute value
552 
553  if(temp <= *this)
554  {
555  cnt = (this->value >> 48) - (temp.value >> 48);
556  if(cnt < 48)
557  {
558  tempExp = (this->value >> 48);
559 
560  temp.value &= 0x0000FFFFFFFFFFFF;
561  temp.value |= 0x0001000000000000;
562  temp.value >>= cnt;
563 
564  tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
565 
566  tempMant -= temp.value;
567 
568  if(tempMant > 0x8000000000000000)
569  {
570  tempMant &= 0x0000FFFFFFFFFFFF;
571  tempExp--;
572  }
573 
574  while(tempMant < 0x1000000000000)
575  {
576  tempMant <<= 1;
577  tempExp--;
578  }
579 
580  tempMant &= 0x0000FFFFFFFFFFFF;
581 
582  this->value = (tempExp << 48) | tempMant;
583  }
584  }
585 
586  else
587  {
588  cnt = (temp.value >> 48) - (this->value >> 48);
589  if(cnt < 48)
590  {
591  tempExp = (temp.value >> 48);
592 
593  this->value &= 0x0000FFFFFFFFFFFF;
594  this->value |= 0x0001000000000000;
595  this->value >>= cnt;
596 
597  tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
598 
599  tempMant -= this->value;
600 
601  if(tempMant > 0x8000000000000000)
602  {
603  tempMant &= 0x0000FFFFFFFFFFFF;
604  tempExp--;
605  }
606 
607  while(tempMant < 0x1000000000000)
608  {
609  tempMant <<= 1;
610  tempExp--;
611  }
612 
613  tempMant &= 0x0000FFFFFFFFFFFF;
614 
615  this->value = (tempExp << 48) | tempMant;
616  this->value |= 0x0100000000000000;
617  }
618 
619  else
620  {
621  this->value = temp.value;
622  this->value |= 0x0100000000000000;
623  }
624  }
625  }
626 
627  return *this;
628 
629 }
630 
632 {
633 
634 }
635 
637 {
638  float T = 0.0;
639  float Vout = 0.0;
640  float NTC = 0.0;
641 
642  Vout = analogRead(TEMP)*0.0048828125; //0.0048828125 = 5V/1024
643  NTC = ((R*5.0)/Vout)-R; //the current NTC resistance
644  NTC = log(NTC);
645  T = A + B*NTC + C*NTC*NTC*NTC; //Steinhart-Hart equation
646  T = 1.0/T;
647 
648  return T - 273.15;
649 }
650 
652 {
653  I2C.begin();
654 }
655 
657 {/*
658  float deltaAngle, angle;
659 
660  TIMSK1 &= ~(1 << OCIE1A);
661 
662  angle = this->getAngle() - this->encoderOffset;
663 
664 
665  if(angle < 0.0)
666  {
667  angle += 360.0;
668  }
669 
670  deltaAngle = this->oldAngle - angle;
671 
672  if(deltaAngle < -180.0)
673  {
674  pointer->encoder.angleMoved += (deltaAngle + 360.0);
675  }
676 
677  else if(deltaAngle > 180.0)
678  {
679  pointer->encoder.angleMoved -= (360.0 - deltaAngle);
680  }
681 
682  else
683  {
684  this->angleMoved += deltaAngle;
685  }
686 
687  this->oldAngle = angle;
688 
689  TIMSK1 |= (1 << OCIE1A);*/
690 
691  return this->angleMoved;
692 }
693 
695 {
696  return this->curSpeed;
697 }
698 
700 {
701  TCCR1A = 0;
702  TCNT1 = 0;
703  OCR1A = 16000;
704  TIFR1 = 0;
705  TIMSK1 = (1 << OCIE1A);
706  TCCR1B = (1 << WGM12) | (1 << CS10);
707  this->encoderOffset = this->getAngle();
708 
709  this->oldAngle = 0.0;
710  this->angleMoved = 0.0;
711 
712  sei();
713 
714 }
715 
717 {
718  this->encoderOffset = this->getAngle();
719 
720  this->oldAngle = 0.0;
721  this->angleMoved = 0.0;
722  this->angleMoved = 0.0;
723 }
724 
726 {
727  return this->angle;
728 }
729 
731 {
732  uint8_t data[2];
733 
734  I2C.read(ENCODERADDR, MAGNITUDE, 2, data);
735 
736  return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
737 }
738 
740 {
741  uint8_t data;
742 
743  I2C.read(ENCODERADDR, MAGNITUDE, 1, &data);
744 
745  return data;
746 }
747 
749 {
750  uint8_t data;
751 
752  I2C.read(ENCODERADDR, AGC, 1, &data);
753 
754  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 !
755 
756  if(data == 0x08)
757  {
758  return 1; //magnet too strong
759  }
760 
761  else if(data == 0x10)
762  {
763  return 2; //magnet too weak
764  }
765 
766  else if(data == 0x20)
767  {
768  return 0; //magnet detected and within limits
769  }
770 
771  return 3; //Something went horribly wrong !
772 }
773 
775 {
776 
777  this->state = STOP;
778 
779  this->setMaxAcceleration(1000.0);
780  this->setMaxVelocity(1000.0);
781 
782  pointer = this;
783 
784  DDRD |= (1 << 7); //set direction pin to output
785  DDRD |= (1 << 4); //set step pin to output
786  DDRB |= (1 << 0); //set enable pin to output
787 }
788 
789 uStepper::uStepper(float accel, float vel)
790 {
791  this->state = STOP;
792 
793  this->setMaxVelocity(vel);
794  this->setMaxAcceleration(accel);
795 
796  pointer = this;
797 
798  DDRD |= (1 << 7); //set direction pin to output
799  DDRD |= (1 << 4); //set step pin to output
800  DDRB |= (1 << 0); //set enable pin to output
801 }
802 
804 {
805  this->acceleration = accel;
806 
807  this->stopTimer(); //Stop timer so we dont fuck up stuff !
808  this->multiplier.setValue((this->acceleration/(INTFREQ*INTFREQ))); //Recalculate multiplier variable, used by the acceleration algorithm since acceleration has changed!
809 
810  if(this->state != STOP)
811  {
812  if(this->continous == 1) //If motor was running continously
813  {
814  this->runContinous(this->direction); //We should make it run continously again
815  }
816  else //If motor still needs to perform some steps
817  {
818  this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold); //we should make sure the motor gets to execute the remaining steps
819  }
820  }
821 }
822 
824 {/*
825  Serial.print(this->exactDelay.getFloatValue(),15);
826  Serial.print("\t");
827  Serial.print(this->delay);
828  Serial.print("\t");
829  Serial.print((uint8_t)((this->exactDelay.value >> 48) & 0x00000000000000FF),HEX);
830  Serial.print(" ");
831  Serial.print((uint8_t)((this->exactDelay.value >> 40) & 0x00000000000000FF),HEX);
832  Serial.print(" ");
833  Serial.print((uint8_t)((this->exactDelay.value >> 32) & 0x00000000000000FF),HEX);
834  Serial.print(" ");
835  Serial.print((uint8_t)((this->exactDelay.value >> 24) & 0x00000000000000FF),HEX);
836  Serial.print(" ");
837  Serial.print((uint8_t)((this->exactDelay.value >> 16) & 0x00000000000000FF),HEX);
838  Serial.print(" ");
839  Serial.print((uint8_t)((this->exactDelay.value >> 8) & 0x00000000000000FF),HEX);
840  Serial.print(" ");
841  Serial.print((uint8_t)((this->exactDelay.value) & 0x00000000000000FF),HEX);
842  Serial.print("\t");
843  Serial.print(this->delay);
844  Serial.print("\t");
845  Serial.println(this->totalSteps);*/
846  /*cli();
847  float tmp=this->exactDelay.getFloatValue();
848  sei();
849  return tmp;*/
850  return this->acceleration;
851 }
852 
854 {
855  if(vel < 0.5005)
856  {
857  this->velocity = 0.5005; //Limit velocity in order to not overflow delay variable
858  }
859 
860  else if(vel > 32000.0)
861  {
862  this->velocity = 32000.0; //limit velocity in order to not underflow delay variable
863  }
864 
865  else
866  {
867  this->velocity = vel;
868  }
869 
870  this->stopTimer(); //Stop timer so we dont fuck up stuff !
871  this->cruiseDelay = (uint16_t)((INTFREQ/this->velocity) - 0.5); //Calculate cruise delay, so we dont have to recalculate this in the interrupt routine
872 
873  if(this->state != STOP) //If motor was running, we should make sure it runs again
874  {
875  if(this->continous == 1) //If motor was running continously
876  {
877  this->runContinous(this->direction); //We should make it run continously again
878  }
879  else //If motor still needs to perform some steps
880  {
881  this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold); //we should make sure it gets to execute these steps
882  }
883  }
884 }
885 
887 {
888  return this->velocity;
889 }
890 
892 {
893  float curVel;
894 
895  if(this->dropIn)
896  {
897  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
898  }
899 
900  this->continous = 1; //Set continous variable to 1, in order to let the interrupt routine now, that the motor should run continously
901 
902  this->stopTimer(); //Stop interrupt timer, so we don't fuck up stuff !
903 
904  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.
905  {
906  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
907 
908  if(dir != digitalRead(DIR)) //If motor is currently running the opposite direction as desired
909  {
910  this->state = INITDECEL; //We should decelerate the motor to full stop before accelerating the speed in the opposite direction
911  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)))
912  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)))
913  this->direction = dir;
914  this->exactDelay.setValue(INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration)); //number of interrupts before the first step should be performed.
915 
916  if(this->exactDelay.getFloatValue() >= 65535.5)
917  {
918  this->delay = 0xFFFF;
919  }
920  else
921  {
922  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
923  }
924  }
925  else //If the motor is currently rotating the same direction as the desired direction
926  {
927  if(curVel > this->velocity) //If current velocity is greater than desired velocity
928  {
929  this->state = INITDECEL; //We need to decelerate the motor to desired velocity
930  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)))
931  this->accelSteps = 0; //No acceleration phase is needed
932  }
933 
934  else if(curVel < this->velocity) //If the current velocity is less than the desired velocity
935  {
936  this->state = ACCEL; //Start accelerating
937  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
938  }
939 
940  else //If motor is currently running at desired speed
941  {
942  this->state = CRUISE; //We should just run at cruise speed
943  }
944  }
945  }
946 
947  else //If motor is currently stopped (state = STOP)
948  {
949  this->state = ACCEL; //Start accelerating
950  if(dir) //Set the motor direction pin to the desired setting
951  {
952  PORTD |= (1 << 7);
953  }
954  else
955  {
956  PORTD &= ~(1 << 7);
957  }
958  this->accelSteps = (velocity*velocity)/(2.0*acceleration); //Number of steps to bring the motor to max speed (S = (V^2 - V0^2)/(2*a)))
959 
960  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
961 
962  if(this->exactDelay.getFloatValue() > 65535.0)
963  {
964  this->delay = 0xFFFF;
965  }
966  else
967  {
968  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
969  }
970  }
971 
972  this->startTimer(); //start timer so we can perform steps
973  this->enableMotor(); //Enable motor
974 }
975 
976 void uStepper::moveSteps(uint32_t steps, bool dir, bool holdMode)
977 {
978  float curVel;
979 
980  if(this->dropIn)
981  {
982  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
983  }
984 
985  this->stopTimer(); //Stop interrupt timer so we dont fuck stuff up !
986  steps--;
987  this->direction = dir; //Set direction variable to the desired direction of rotation for the interrupt routine
988  this->hold = holdMode; //Set the hold variable to desired hold mode (block motor or release motor after end movement) for the interrupt routine
989  this->totalSteps = steps; //Load the desired number of steps into the totalSteps variable for the interrupt routine
990  this->continous = 0; //Set continous variable to 0, since the motor should not run continous
991 
992  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.
993  {
994  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
995 
996  if(dir != digitalRead(DIR)) //If current direction is different from desired direction
997  {
998  this->state = INITDECEL; //We should decelerate the motor to full stop
999  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)))
1000  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)))
1001  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
1002 
1003  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
1004  {
1005  this->accelSteps = this->decelSteps = (this->totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1006  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
1007  }
1008  else
1009  {
1010  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
1011  this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps; //Perform remaining steps, as cruise steps
1012  }
1013 
1014  this->exactDelay.setValue(INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1015 
1016  if(this->exactDelay.getFloatValue() >= 65535.5)
1017  {
1018  this->delay = 0xFFFF;
1019  }
1020  else
1021  {
1022  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1023  }
1024  }
1025  else //If the motor is currently rotating the same direction as desired, we dont necessarily need to decelerate
1026  {
1027  if(curVel > this->velocity) //If current velocity is greater than desired velocity
1028  {
1029  this->state = INITDECEL; //We need to decelerate the motor to desired velocity
1030  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)))
1031  this->accelSteps = 0; //No acceleration phase is needed
1032  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
1033  this->exactDelay.setValue((INTFREQ/sqrt((curVel*curVel) + 2*this->acceleration)));
1034 
1035  if(this->totalSteps <= (this->initialDecelSteps + this->decelSteps))
1036  {
1037  this->cruiseSteps = 0;
1038  }
1039  else
1040  {
1041  this->cruiseSteps = steps - this->initialDecelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1042  }
1043 
1044 
1045  }
1046 
1047  else if(curVel < this->velocity) //If current velocity is less than desired velocity
1048  {
1049  this->state = ACCEL; //Start accelerating
1050  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
1051 
1052  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
1053  {
1054  this->accelSteps = this->decelSteps = (this->totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1055  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
1056  this->cruiseSteps = 0;
1057  }
1058  else
1059  {
1060  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
1061  this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps; //Perform remaining steps, as cruise steps
1062  }
1063 
1064  this->cruiseSteps = steps - this->accelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1065  this->initialDecelSteps = 0; //No initial deceleration phase needed
1066  }
1067 
1068  else //If current velocity is equal to desired velocity
1069  {
1070  this->state = CRUISE; //We are already at desired speed, therefore we start at cruise phase
1071  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
1072  this->accelSteps = 0; //No acceleration phase needed
1073  this->initialDecelSteps = 0; //No initial deceleration phase needed
1074 
1075  if(this->decelSteps >= this->totalSteps)
1076  {
1077  this->cruiseSteps = 0;
1078  }
1079  else
1080  {
1081  this->cruiseSteps = steps - this->decelSteps; //Perform remaining steps as cruise steps
1082  }
1083  }
1084  }
1085  }
1086 
1087  else //If motor is currently at full stop (state = STOP)
1088  {
1089  if(dir) //Set the motor direction pin to the desired setting
1090  {
1091  PORTD |= (1 << 7);
1092  }
1093  else
1094  {
1095  PORTD &= ~(1 << 7);
1096  }
1097  this->state = ACCEL;
1098  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)))
1099  this->initialDecelSteps = 0; //No initial deceleration phase needed
1100 
1101  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
1102  {
1103  this->cruiseSteps = 0; //No cruise phase needed
1104  this->accelSteps = this->decelSteps = (steps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1105  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
1106  }
1107 
1108  else
1109  {
1110  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
1111  this->cruiseSteps = steps - this->accelSteps - this->decelSteps; //Perform remaining steps as cruise steps
1112  }
1113  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1114 
1115  if(this->exactDelay.getFloatValue() > 65535.0)
1116  {
1117  this->delay = 0xFFFF;
1118  }
1119  else
1120  {
1121  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1122  }
1123  }
1124 
1125  this->startTimer(); //start timer so we can perform steps
1126  this->enableMotor(); //Enable motor driver
1127 }
1128 
1129 void uStepper::hardStop(bool holdMode)
1130 {
1131  if(this->dropIn)
1132  {
1133  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1134  }
1135 
1136  this->stopTimer(); //Stop interrupt timer, since we shouldn't perform more steps
1137  this->hold = holdMode;
1138 
1139  if(state != STOP)
1140  {
1141  this->state = STOP; //Set current state to STOP
1142 
1143  this->startTimer();
1144  }
1145 
1146  else
1147  {
1148  if(holdMode == SOFT)
1149  {
1150  this->disableMotor();
1151  }
1152 
1153  else if (holdMode == HARD)
1154  {
1155  this->enableMotor();
1156  }
1157  }
1158 }
1159 
1160 void uStepper::softStop(bool holdMode)
1161 {
1162  float curVel;
1163 
1164  if(this->dropIn)
1165  {
1166  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1167  }
1168 
1169  this->stopTimer(); //Stop interrupt timer, since we shouldn't perform more steps
1170  this->hold = holdMode;
1171 
1172  if(state != STOP)
1173  {
1174  curVel = INTFREQ/this->exactDelay.getFloatValue(); //Use this to calculate current velocity
1175 
1176  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)))
1177  this->accelSteps = this->initialDecelSteps = this->cruiseSteps = 0; //Reset amount of steps in the different phases
1178  this->state = DECEL;
1179 
1180  this->exactDelay.setValue(INTFREQ/sqrt(2.0*this->acceleration)); //number of interrupts before the first step should be performed.
1181 
1182  if(this->exactDelay.getFloatValue() > 65535.0)
1183  {
1184  this->delay = 0xFFFF;
1185  }
1186  else
1187  {
1188  this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
1189  }
1190 
1191  this->startTimer();
1192  }
1193 
1194  else
1195  {
1196  if(holdMode == SOFT)
1197  {
1198  this->disableMotor();
1199  }
1200 
1201  else if (holdMode == HARD)
1202  {
1203  this->enableMotor();
1204  }
1205  }
1206 }
1207 
1208 void uStepper::setup(bool mode, uint8_t microStepping, float faultSpeed, uint32_t faultTolerance)
1209 {
1210  this->dropIn = mode;
1211  if(mode)
1212  {
1213  pinMode(2,INPUT);
1214  pinMode(3,INPUT);
1215  pinMode(4,INPUT);
1216  digitalWrite(2,HIGH);
1217  digitalWrite(3,HIGH);
1218  digitalWrite(4,HIGH);
1219  this->stepResolution = 360.0/((float)(200*microStepping));
1220  this->tolerance = ((float)faultTolerance)*this->stepResolution;
1221  this->faultStepDelay = (uint16_t)((INTFREQ/faultSpeed) - 1);
1222  EICRA = 0x0D; //int0 generates interrupt on any change and int1 generates interrupt on rising edge
1223  EIMSK = 0x03; //enable int0 and int1 interrupt requests
1224  }
1225 
1226  TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1227  TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1228  TCCR2B |= (1 << CS21); //Enable timer with prescaler 8. interrupt base frequency ~ 7.8125kHz
1229  TCCR2A |= (1 << WGM21); //Switch timer 2 to CTC mode, to adjust interrupt frequency
1230  OCR2A = 70; //Change top value to 60 in order to obtain an interrupt frequency of 33.333kHz
1231  this->encoder.setup();
1232 }
1233 
1235 {
1236  TCNT2 = 0; //Clear counter value, to make sure we get correct timing
1237  TIFR2 |= (1 << OCF2A); //Clear compare match interrupt flag, if it is set.
1238  TIMSK2 |= (1 << OCIE2A); //Enable compare match interrupt
1239 
1240  sei();
1241 }
1242 
1244 {
1245  TIMSK2 &= ~(1 << OCF2A); //disable compare match interrupt
1246 }
1247 
1249 {
1250  PORTB &= ~(1 << 0); //Enable motor driver
1251 }
1252 
1254 {
1255  PORTB |= (1 << 0); //Disable motor driver
1256 }
1257 
1259 {
1260  return this->direction;
1261 }
1262 
1264 {
1265  if(this->state != STOP)
1266  {
1267  return 1; //Motor running
1268  }
1269 
1270  return 0; //Motor not running
1271 }
1272 
1274 {
1275  if(this->direction == CW)
1276  {
1277  return this->stepsSinceReset + this->currentStep;
1278  }
1279  else
1280  {
1281  return this->stepsSinceReset - this->currentStep;
1282  }
1283 }
1284 
1285 void i2cMaster::cmd(uint8_t cmd)
1286 {
1287  uint16_t i = 0;
1288  // send command
1289  TWCR = cmd;
1290  // wait for command to complete
1291  while (!(TWCR & (1 << TWINT)));
1292 
1293  // save status bits
1294  status = TWSR & 0xF8;
1295 }
1296 
1297 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1298 {
1299  uint8_t i, buff[numOfBytes];
1300 
1301  TIMSK1 &= ~(1 << OCIE1A);
1302 
1303  I2C.start(slaveAddr, WRITE);
1304 
1305  I2C.writeByte(regAddr);
1306 
1307  I2C.restart(slaveAddr, READ);
1308 
1309  for(i = 0; i < (numOfBytes - 1); i++)
1310  {
1311  I2C.readByte(ACK, &data[i]);
1312  }
1313 
1314  I2C.readByte(NACK, &data[numOfBytes-1]);
1315 
1316  I2C.stop();
1317 
1318  TIMSK1 |= (1 << OCIE1A);
1319 
1320  return 1;
1321 }
1322 
1323 bool i2cMaster::write(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1324 {
1325  uint8_t i;
1326 
1327  TIMSK1 &= ~(1 << OCIE1A);
1328 
1329  I2C.start(slaveAddr, WRITE);
1330  I2C.writeByte(regAddr);
1331 
1332  for(i = 0; i < numOfBytes; i++)
1333  {
1334  I2C.writeByte(*(data + i));
1335  }
1336  I2C.stop();
1337 
1338  TIMSK1 |= (1 << OCIE1A);
1339 
1340  return 1;
1341 }
1342 
1343 bool i2cMaster::readByte(bool ack, uint8_t *data)
1344 {
1345  if(ack)
1346  {
1347  this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA));
1348  }
1349 
1350  else
1351  {
1352  this->cmd((1 << TWINT) | (1 << TWEN));
1353  }
1354 
1355  *data = TWDR;
1356 
1357  return 1;
1358 }
1359 
1360 bool i2cMaster::start(uint8_t addr, bool RW)
1361 {
1362  // send START condition
1363  this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1364 
1365  if (this->getStatus() != START && this->getStatus() != REPSTART)
1366  {
1367  return false;
1368  }
1369 
1370  // send device address and direction
1371  TWDR = (addr << 1) | RW;
1372  this->cmd((1 << TWINT) | (1 << TWEN));
1373 
1374  if (RW == READ)
1375  {
1376  return this->getStatus() == RXADDRACK;
1377  }
1378 
1379  else
1380  {
1381  return this->getStatus() == TXADDRACK;
1382  }
1383 }
1384 
1385 bool i2cMaster::restart(uint8_t addr, bool RW)
1386 {
1387  return this->start(addr, RW);
1388 }
1389 
1390 bool i2cMaster::writeByte(uint8_t data)
1391 {
1392  TWDR = data;
1393 
1394  this->cmd((1 << TWINT) | (1 << TWEN));
1395 
1396  return this->getStatus() == TXDATAACK;
1397 }
1398 
1400 {
1401  uint16_t i = 0;
1402  // issue stop condition
1403  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1404 
1405  // wait until stop condition is executed and bus released
1406  while (TWCR & (1 << TWSTO));
1407 
1408  status = I2CFREE;
1409 
1410  return 1;
1411 }
1412 
1414 {
1415  return status;
1416 }
1417 
1419 {
1420  // set bit rate register to 12 to obtain 400kHz scl frequency (in combination with no prescaling!)
1421  TWBR = 12;
1422  // no prescaler
1423  TWSR &= 0xFC;
1424 }
1425 
1427 {
1428 
1429 }
#define R
Definition: uStepper.h:185
uStepper(void)
Constructor of uStepper class.
Definition: uStepper.cpp:774
uStepperEncoder(void)
Constructor.
Definition: uStepper.cpp:651
void setup(bool mode=NORMAL, uint8_t microStepping=SIXTEEN, float faultSpeed=3000.0, uint32_t faultTolerance=20)
Initializes the different parts of the uStepper object.
Definition: uStepper.cpp:1208
uint32_t currentStep
Definition: uStepper.h:445
uint16_t getStrength(void)
Measure the strength of the magnet.
Definition: uStepper.cpp:730
#define CRUISE
Definition: uStepper.h:166
#define WRITE
Definition: uStepper.h:191
float getSpeed(void)
Measure the current speed of the motor.
Definition: uStepper.cpp:694
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:1323
bool getMotorState(void)
Get the current state of the motor.
Definition: uStepper.cpp:1263
float2 multiplier
Definition: uStepper.h:433
float acceleration
Definition: uStepper.h:465
bool continous
Definition: uStepper.h:449
Prototype of class for accessing all features of the uStepper in a single object. ...
Definition: uStepper.h:427
volatile float curSpeed
Definition: uStepper.h:416
void enableMotor(void)
Enables the stepper driver output stage.
Definition: uStepper.cpp:1248
#define A
Definition: uStepper.h:223
float getMaxAcceleration(void)
Get the value of the maximum motor acceleration.
Definition: uStepper.cpp:823
#define CW
Definition: uStepper.h:170
#define DECEL
Definition: uStepper.h:167
float getTemp(void)
Request a reading of current temperature.
Definition: uStepper.cpp:636
#define RXADDRACK
Definition: uStepper.h:201
uint32_t cruiseSteps
Definition: uStepper.h:443
uint32_t totalSteps
Definition: uStepper.h:447
#define C
Definition: uStepper.h:225
bool getCurrentDirection(void)
Returns the direction the motor is currently configured to rotate.
Definition: uStepper.cpp:1258
float getAngleMoved(void)
Measure the angle moved from reference position.
Definition: uStepper.cpp:656
uint32_t accelSteps
Definition: uStepper.h:437
#define ENCODERADDR
Definition: uStepper.h:175
void hardStop(bool holdMode)
Stop the motor without deceleration.
Definition: uStepper.cpp:1129
void setMaxAcceleration(float accel)
Set the maximum acceleration of the stepper motor.
Definition: uStepper.cpp:803
void startTimer(void)
Starts timer for stepper algorithm.
Definition: uStepper.cpp:1234
void setMaxVelocity(float vel)
Sets the maximum rotational velocity of the motor.
Definition: uStepper.cpp:853
void moveSteps(uint32_t steps, bool dir, bool holdMode)
Make the motor perform a predefined number of steps.
Definition: uStepper.cpp:976
#define STOP
Definition: uStepper.h:164
float getMaxVelocity(void)
Returns the maximum rotational velocity of the motor.
Definition: uStepper.cpp:886
bool writeByte(uint8_t data)
Writes a byte to a device on the I2C bus.
Definition: uStepper.cpp:1390
i2cMaster(void)
Constructor.
Definition: uStepper.cpp:1426
void softStop(bool holdMode)
Stop the motor with deceleration.
Definition: uStepper.cpp:1160
uint8_t status
Definition: uStepper.h:712
uint8_t detectMagnet(void)
Detect if magnet is present and within range.
Definition: uStepper.cpp:748
uint8_t getStatus(void)
Get current I2C status.
Definition: uStepper.cpp:1413
#define READ
Definition: uStepper.h:189
void cmd(uint8_t cmd)
Sends commands over the I2C bus.
Definition: uStepper.cpp:1285
bool restart(uint8_t addr, bool RW)
Restarts connection between arduino and I2C device.
Definition: uStepper.cpp:1385
bool readByte(bool ack, uint8_t *data)
Reads a byte from the I2C bus.
Definition: uStepper.cpp:1343
int64_t stepsSinceReset
Definition: uStepper.h:455
uStepperEncoder encoder
Definition: uStepper.h:515
#define HARD
Definition: uStepper.h:172
void stopTimer(void)
Stops the timer for the stepper algorithm.
Definition: uStepper.cpp:1243
uint8_t getAgc(void)
Read the current AGC value of the encoder chip.
Definition: uStepper.cpp:739
bool start(uint8_t addr, bool RW)
sets up connection between arduino and I2C device.
Definition: uStepper.cpp:1360
#define MAGNITUDE
Definition: uStepper.h:180
void runContinous(bool dir)
Make the motor rotate continuously.
Definition: uStepper.cpp:891
#define INTFREQ
Definition: uStepper.h:169
#define ENCODERSPEEDCONSTANT
Definition: uStepper.h:183
Prototype of class for accessing the TWI (I2C) interface of the AVR (master mode only).
Definition: uStepper.h:709
#define ANGLE
Definition: uStepper.h:177
void setHome(void)
Define new reference(home) position.
Definition: uStepper.cpp:716
#define SOFT
Definition: uStepper.h:173
uint16_t cruiseDelay
Definition: uStepper.h:431
float getAngle(void)
Measure the current shaft angle.
Definition: uStepper.cpp:725
#define ACK
Definition: uStepper.h:203
#define B
Definition: uStepper.h:224
bool stop(void)
Closes the I2C connection.
Definition: uStepper.cpp:1399
#define AGC
Definition: uStepper.h:179
i2cMaster I2C
Definition: uStepper.cpp:73
volatile float oldAngle
Definition: uStepper.h:414
#define I2CFREE
Definition: uStepper.h:187
#define REPSTART
Definition: uStepper.h:195
void begin(void)
Setup TWI (I2C) interface.
Definition: uStepper.cpp:1418
#define TXDATAACK
Definition: uStepper.h:199
#define NACK
Definition: uStepper.h:205
Function prototypes and definitions for the uStepper library.
#define ACCEL
Definition: uStepper.h:165
#define INITDECEL
Definition: uStepper.h:168
float velocity
Definition: uStepper.h:463
float2 exactDelay
Definition: uStepper.h:457
volatile float angleMoved
Definition: uStepper.h:417
uint8_t state
Definition: uStepper.h:435
uStepperTemp(void)
Constructor.
Definition: uStepper.cpp:631
uint16_t delay
Definition: uStepper.h:459
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:1297
uint32_t decelSteps
Definition: uStepper.h:439
bool direction
Definition: uStepper.h:453
bool hold
Definition: uStepper.h:451
#define START
Definition: uStepper.h:193
float encoderOffset
Definition: uStepper.h:413
void disableMotor(void)
Disables the stepper driver output stage.
Definition: uStepper.cpp:1253
#define TXADDRACK
Definition: uStepper.h:197
int64_t getStepsSinceReset(void)
Get the number of steps applied since reset.
Definition: uStepper.cpp:1273
void setup(void)
Setup the encoder.
Definition: uStepper.cpp:699
uint32_t initialDecelSteps
Definition: uStepper.h:441