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