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.3.0 *
4 * date: May 7th, 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 * TO DO: *
48 * - Implement Doxygen comments *
49 * - Review comments *
50 * - Implement a function to read the actual speed of the motor, using the encoder *
51 * *
52 *********************************************************************************************
53 * (C) 2016 *
54 * *
55 * ON Development IVS *
56 * www.on-development.com *
57 * administration@on-development.com *
58 * *
59 * The code contained in this file is released under the following open source license: *
60 * *
61 * Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International *
62 * *
63 * The code in this file is provided without warranty of any kind - use at own risk! *
64 * neither ON Development IVS nor the author, can be held responsible for any damage *
65 * caused by the use of the code contained in this file ! *
66 * *
67 ********************************************************************************************/
75 #include <uStepper.h>
76 #include <Arduino.h>
77 #include <Wire.h>
78 #include <util/delay.h>
79 #include <math.h>
80 
81 uStepper *pointer;
83 
84 extern "C" {
85  void TIMER2_COMPA_vect(void)
86  {
87  asm volatile("jmp _AccelerationAlgorithm \n\t"); //Execute the acceleration profile algorithm
88  }
89 
90  void TIMER1_COMPA_vect(void)
91  {
92  float deltaAngle;
93  static float curAngle, oldAngle = 0.0, loops = 1.0;
94 
95  sei();
96  if(I2C.getStatus() != I2CFREE)
97  {
98  return;
99  }
100 
101  curAngle = fmod(pointer->encoder.getAngle() - pointer->encoder.encoderOffset + 360.0, 360.0);
102 
103  deltaAngle = (oldAngle - curAngle);
104 
105  if(deltaAngle < -50.0)
106  {
107  deltaAngle += 360.0;
108  }
109  else if(deltaAngle > 50.0)
110  {
111  deltaAngle -= 360;
112  }
113 
114  if((deltaAngle >= 10.0) || (deltaAngle <= -10.0))
115  {
116  pointer->encoder.curSpeed = deltaAngle*ENCODERSPEEDCONSTANT;
117  pointer->encoder.curSpeed /= loops;
118  loops = 1.0;
119  oldAngle = curAngle;
120  }
121  else
122  {
123  loops += 1.0;
124  if(loops >= 50000.0)
125  {
126  pointer->encoder.curSpeed = 0.0;
127  }
128  }
129 
130  deltaAngle = pointer->encoder.oldAngle - curAngle;
131 
132  if(deltaAngle < -50.0)
133  {
134  pointer->encoder.angleMoved += (deltaAngle + 360.0);
135  }
136 
137  else if(deltaAngle > 50.0)
138  {
139  pointer->encoder.angleMoved -= (360.0 - deltaAngle);
140  }
141  else
142  {
143  pointer->encoder.angleMoved += deltaAngle;
144  }
145 
146  pointer->encoder.oldAngle = curAngle;
147  }
148 
149 }
150 
152 {
153 
154 }
155 
157 {
158  float T = 0.0;
159  float Vout = 0.0;
160  float NTC = 0.0;
161 
162  Vout = analogRead(TEMP)*0.0048828125; //0.0048828125 = 5V/1024
163  NTC = ((R*5.0)/Vout)-R; //the current NTC resistance
164  NTC = log(NTC);
165  T = A + B*NTC + C*NTC*NTC*NTC; //Steinhart-Hart equation
166  T = 1.0/T;
167 
168  return T - 273.15;
169 }
170 
172 {
173  I2C.begin();
174 }
175 
177 {
178  float deltaAngle, angle;
179 
180  TIMSK1 &= ~(1 << OCIE1A);
181 
182  angle = fmod(this->getAngle() - this->encoderOffset + 360.0, 360.0);
183  deltaAngle = this->oldAngle - angle;
184 
185  if(deltaAngle < -50.0)
186  {
187  pointer->encoder.angleMoved += (deltaAngle + 360.0);
188  }
189 
190  else if(deltaAngle > 50.0)
191  {
192  pointer->encoder.angleMoved -= (360.0 - deltaAngle);
193  }
194 
195  else
196  {
197  this->angleMoved += deltaAngle;
198  }
199 
200  this->oldAngle = angle;
201 
202  TIMSK1 |= (1 << OCIE1A);
203 
204  return this->angleMoved;
205 }
206 
208 {
209  return this->curSpeed;
210 }
211 
213 {
214  TCCR1A = 0;
215  TCNT1 = 0;
216  OCR1A = 65535;
217  TIFR1 = 0;
218  TIMSK1 = (1 << OCIE1A);
219  TCCR1B = (1 << WGM12) | (1 << CS10);
220  this->encoderOffset = this->getAngle();
221 
222  this->oldAngle = 0.0;
223  this->angleMoved = 0.0;
224 
225  sei();
226 
227 }
228 
230 {
231  this->encoderOffset = this->getAngle();
232 
233  this->oldAngle = 0.0;
234  this->angleMoved = 0.0;
235  this->angleMoved = 0.0;
236 
237 }
238 
240 {
241  float angle;
242  uint8_t data[2];
243 
244  I2C.read(ENCODERADDR, ANGLE, 2, data);
245 
246  angle = (float)((((uint16_t)data[0]) << 8 )| (uint16_t)data[1])*0.087890625;
247 
248  return angle;
249 }
250 
252 {
253  uint8_t data[2];
254 
255  I2C.read(ENCODERADDR, MAGNITUDE, 2, data);
256 
257  return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
258 }
259 
261 {
262  uint8_t data;
263 
264  I2C.read(ENCODERADDR, MAGNITUDE, 1, &data);
265 
266  return data;
267 }
268 
270 {
271  uint8_t data;
272 
273  I2C.read(ENCODERADDR, AGC, 1, &data);
274 
275  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 !
276 
277  if(data == 0x08)
278  {
279  return 1; //magnet too strong
280  }
281 
282  else if(data == 0x10)
283  {
284  return 2; //magnet too weak
285  }
286 
287  else if(data == 0x20)
288  {
289  return 0; //magnet detected and within limits
290  }
291 
292  return 3; //Something went horribly wrong !
293 }
294 
296 {
297  this->state = STOP;
298 
299  this->setMaxAcceleration(1000.0);
300  this->setMaxVelocity(1000.0);
301 
302  pointer = this;
303 
304  pinMode(DIR, OUTPUT);
305  pinMode(STEP, OUTPUT);
306  pinMode(ENA, OUTPUT);
307 }
308 
309 uStepper::uStepper(float accel, float vel)
310 {
311  this->state = STOP;
312 
313  this->setMaxVelocity(vel);
314  this->setMaxAcceleration(accel);
315 
316  pointer = this;
317 
318  pinMode(DIR, OUTPUT);
319  pinMode(STEP, OUTPUT);
320  pinMode(ENA, OUTPUT);
321 }
322 
324 {
325  this->acceleration = accel;
326 
327  this->stopTimer(); //Stop timer so we dont fuck up stuff !
328  this->multiplier = (this->acceleration/(INTFREQ*INTFREQ)); //Recalculate multiplier variable, used by the acceleration algorithm since acceleration has changed!
329  if(this->state != STOP)
330  {
331  if(this->continous == 1) //If motor was running continously
332  {
333  this->runContinous(this->direction); //We should make it run continously again
334  }
335  else //If motor still needs to perform some steps
336  {
337  this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold); //we should make sure the motor gets to execute the remaining steps
338  }
339  }
340 }
341 
343 {
344  return this->acceleration;
345 }
346 
348 {
349  if(vel < 0.5005)
350  {
351  this->velocity = 0.5005; //Limit velocity in order to not overflow delay variable
352  }
353 
354  else if(vel > 32000.0)
355  {
356  this->velocity = 32000.0; //limit velocity in order to not underflow delay variable
357  }
358 
359  else
360  {
361  this->velocity = vel;
362  }
363 
364  this->stopTimer(); //Stop timer so we dont fuck up stuff !
365  this->cruiseDelay = (uint16_t)((INTFREQ/this->velocity) - 0.5); //Calculate cruise delay, so we dont have to recalculate this in the interrupt routine
366 
367  if(this->state != STOP) //If motor was running, we should make sure it runs again
368  {
369  if(this->continous == 1) //If motor was running continously
370  {
371  this->runContinous(this->direction); //We should make it run continously again
372  }
373  else //If motor still needs to perform some steps
374  {
375  this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold); //we should make sure it gets to execute these steps
376  }
377  }
378 }
379 
381 {
382  return this->velocity;
383 }
384 
386 {
387  float curVel;
388 
389  this->continous = 1; //Set continous variable to 1, in order to let the interrupt routine now, that the motor should run continously
390 
391  this->stopTimer(); //Stop interrupt timer, so we don't fuck up stuff !
392 
393  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.
394  {
395  curVel = INTFREQ/this->exactDelay; //Use this to calculate current velocity
396 
397  if(dir != digitalRead(DIR)) //If motor is currently running the opposite direction as desired
398  {
399  this->state = INITDECEL; //We should decelerate the motor to full stop before accelerating the speed in the opposite direction
400  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)))
401  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)))
402  this->direction = dir;
403  this->exactDelay = INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration); //number of interrupts before the first step should be performed.
404 
405  if(this->exactDelay >= 65535.5)
406  {
407  this->delay = 0xFFFF;
408  }
409  else
410  {
411  this->delay = (uint16_t)(this->exactDelay - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
412  }
413  }
414  else //If the motor is currently rotating the same direction as the desired direction
415  {
416  if(curVel > this->velocity) //If current velocity is greater than desired velocity
417  {
418  this->state = INITDECEL; //We need to decelerate the motor to desired velocity
419  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)))
420  this->accelSteps = 0; //No acceleration phase is needed
421  }
422 
423  else if(curVel < this->velocity) //If the current velocity is less than the desired velocity
424  {
425  this->state = ACCEL; //Start accelerating
426  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
427  }
428 
429  else //If motor is currently running at desired speed
430  {
431  this->state = CRUISE; //We should just run at cruise speed
432  }
433  }
434  }
435 
436  else //If motor is currently stopped (state = STOP)
437  {
438  this->state = ACCEL; //Start accelerating
439  digitalWrite(DIR, dir); //Set the motor direction pin to the desired setting
440  this->accelSteps = (velocity*velocity)/(2.0*acceleration); //Number of steps to bring the motor to max speed (S = (V^2 - V0^2)/(2*a)))
441 
442  this->exactDelay = INTFREQ/sqrt(2.0*this->acceleration); //number of interrupts before the first step should be performed.
443 
444  if(this->exactDelay > 65535.0)
445  {
446  this->delay = 0xFFFF;
447  }
448  else
449  {
450  this->delay = (uint16_t)(this->exactDelay - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
451  }
452  }
453 
454  this->startTimer(); //start timer so we can perform steps
455  this->enableMotor(); //Enable motor
456 }
457 
458 void uStepper::moveSteps(uint32_t steps, bool dir, bool holdMode)
459 {
460  float curVel;
461 
462  this->stopTimer(); //Stop interrupt timer so we dont fuck stuff up !
463  steps--;
464  this->direction = dir; //Set direction variable to the desired direction of rotation for the interrupt routine
465  this->hold = holdMode; //Set the hold variable to desired hold mode (block motor or release motor after end movement) for the interrupt routine
466  this->totalSteps = steps; //Load the desired number of steps into the totalSteps variable for the interrupt routine
467  this->continous = 0; //Set continous variable to 0, since the motor should not run continous
468 
469  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.
470  {
471  curVel = INTFREQ/this->exactDelay; //Use this to calculate current velocity
472 
473  if(dir != digitalRead(DIR)) //If current direction is different from desired direction
474  {
475  this->state = INITDECEL; //We should decelerate the motor to full stop
476  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)))
477  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)))
478  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
479  this->exactDelayDecel = (INTFREQ/sqrt(this->velocity*this->velocity + 2*this->acceleration));
480 
481  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
482  {
483  this->accelSteps = this->decelSteps = (this->totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
484  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
485  }
486  else
487  {
488  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
489  this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps; //Perform remaining steps, as cruise steps
490  }
491 
492  this->exactDelay = INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration); //number of interrupts before the first step should be performed.
493 
494  if(this->exactDelay >= 65535.5)
495  {
496  this->delay = 0xFFFF;
497  }
498  else
499  {
500  this->delay = (uint16_t)(this->exactDelay - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
501  }
502  }
503  else //If the motor is currently rotating the same direction as desired, we dont necessarily need to decelerate
504  {
505  if(curVel > this->velocity) //If current velocity is greater than desired velocity
506  {
507  this->state = INITDECEL; //We need to decelerate the motor to desired velocity
508  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)))
509  this->accelSteps = 0; //No acceleration phase is needed
510  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
511  this->exactDelayDecel = (INTFREQ/sqrt(this->velocity*this->velocity + 2*this->acceleration));
512  this->exactDelay = (INTFREQ/sqrt((curVel*curVel) + 2*this->acceleration));
513 
514  if(this->totalSteps <= (this->initialDecelSteps + this->decelSteps))
515  {
516  this->cruiseSteps = 0;
517  }
518  else
519  {
520  this->cruiseSteps = steps - this->initialDecelSteps - this->decelSteps; //Perform remaining steps as cruise steps
521  }
522 
523 
524  }
525 
526  else if(curVel < this->velocity) //If current velocity is less than desired velocity
527  {
528  this->state = ACCEL; //Start accelerating
529  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
530  this->exactDelayDecel = (INTFREQ/sqrt(this->velocity*this->velocity + 2*this->acceleration));
531  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
532  {
533  this->accelSteps = this->decelSteps = (this->totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
534  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
535  this->cruiseSteps = 0;
536  }
537  else
538  {
539  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
540  this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps; //Perform remaining steps, as cruise steps
541  }
542 
543  this->cruiseSteps = steps - this->accelSteps - this->decelSteps; //Perform remaining steps as cruise steps
544  this->initialDecelSteps = 0; //No initial deceleration phase needed
545  }
546 
547  else //If current velocity is equal to desired velocity
548  {
549  this->state = CRUISE; //We are already at desired speed, therefore we start at cruise phase
550  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
551  this->accelSteps = 0; //No acceleration phase needed
552  this->initialDecelSteps = 0; //No initial deceleration phase needed
553  this->exactDelayDecel = (INTFREQ/sqrt(this->velocity*this->velocity + 2*this->acceleration));
554  if(this->decelSteps >= this->totalSteps)
555  {
556  this->cruiseSteps = 0;
557  }
558  else
559  {
560  this->cruiseSteps = steps - this->decelSteps; //Perform remaining steps as cruise steps
561  }
562  }
563  }
564  }
565 
566  else //If motor is currently at full stop (state = STOP)
567  {
568  digitalWrite(DIR, dir); //Set direction pin to desired direction
569  this->state = ACCEL;
570  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)))
571  this->initialDecelSteps = 0; //No initial deceleration phase needed
572 
573  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
574  {
575  this->cruiseSteps = 0; //No cruise phase needed
576  this->accelSteps = this->decelSteps = (steps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
577  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
578  this->exactDelayDecel = (INTFREQ/sqrt(2*this->acceleration*this->accelSteps));
579  }
580 
581  else
582  {
583  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
584  this->cruiseSteps = steps - this->accelSteps - this->decelSteps; //Perform remaining steps as cruise steps
585  this->exactDelayDecel = (INTFREQ/sqrt(this->velocity*this->velocity + 2*this->acceleration));
586  }
587  this->exactDelay = INTFREQ/sqrt(2.0*this->acceleration); //number of interrupts before the first step should be performed.
588 
589  if(this->exactDelay > 65535.0)
590  {
591  this->delay = 0xFFFF;
592  }
593  else
594  {
595  this->delay = (uint16_t)(this->exactDelay - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
596  }
597  }
598 
599  this->startTimer(); //start timer so we can perform steps
600  this->enableMotor(); //Enable motor driver
601 }
602 
603 void uStepper::hardStop(bool holdMode)
604 {
605  this->stopTimer(); //Stop interrupt timer, since we shouldn't perform more steps
606  this->hold = holdMode;
607 
608  if(state != STOP)
609  {
610  this->state = STOP; //Set current state to STOP
611 
612  this->startTimer();
613  }
614 
615  else
616  {
617  if(holdMode == SOFT)
618  {
619  this->disableMotor();
620  }
621 
622  else if (holdMode == HARD)
623  {
624  this->enableMotor();
625  }
626  }
627 }
628 
629 void uStepper::softStop(bool holdMode)
630 {
631  float curVel;
632 
633  this->stopTimer(); //Stop interrupt timer, since we shouldn't perform more steps
634  this->hold = holdMode;
635 
636  if(state != STOP)
637  {
638  curVel = INTFREQ/this->exactDelay; //Use this to calculate current velocity
639 
640  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)))
641  this->accelSteps = this->initialDecelSteps = this->cruiseSteps = 0; //Reset amount of steps in the different phases
642  this->state = DECEL;
643 
644  this->exactDelay = INTFREQ/sqrt(2.0*this->acceleration); //number of interrupts before the first step should be performed.
645 
646  if(this->exactDelay > 65535.0)
647  {
648  this->delay = 0xFFFF;
649  }
650  else
651  {
652  this->delay = (uint16_t)(this->exactDelay - 0.5); //Truncate the exactDelay variable, since we cant perform fractional steps
653  }
654 
655  this->startTimer();
656  }
657 
658  else
659  {
660  if(holdMode == SOFT)
661  {
662  this->disableMotor();
663  }
664 
665  else if (holdMode == HARD)
666  {
667  this->enableMotor();
668  }
669  }
670 }
671 
672 void uStepper::setup(void)
673 {
674 
675  TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
676  TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
677  TCCR2B |= (1 << CS21); //Enable timer with prescaler 8. interrupt base frequency ~ 7.8125kHz
678  TCCR2A |= (1 << WGM21); //Switch timer 2 to CTC mode, to adjust interrupt frequency
679  OCR2A = 60; //Change top value to 60 in order to obtain an interrupt frequency of 33.333kHz
680  this->encoder.setup();
681 }
682 
684 {
685  TCNT2 = 0; //Clear counter value, to make sure we get correct timing
686  TIFR2 |= (1 << OCF2A); //Clear compare match interrupt flag, if it is set.
687  TIMSK2 |= (1 << OCIE2A); //Enable compare match interrupt
688 
689  sei();
690 }
691 
693 {
694  TIMSK2 &= ~(1 << OCF2A); //disable compare match interrupt
695 }
696 
698 {
699  digitalWrite(ENA, LOW); //Enable motor driver
700 }
701 
703 {
704  digitalWrite(ENA, HIGH); //Disable motor driver
705 }
706 
708 {
709  return this->direction;
710 }
711 
713 {
714  if(this->state != STOP)
715  {
716  return 1; //Motor running
717  }
718 
719  return 0; //Motor not running
720 }
721 
723 {
724  if(this->direction == CW)
725  {
726  return this->stepsSinceReset + this->currentStep;
727  }
728  else
729  {
730  return this->stepsSinceReset - this->currentStep;
731  }
732 }
733 
734 void i2cMaster::cmd(uint8_t cmd)
735 {
736  uint16_t i = 0;
737  // send command
738  TWCR = cmd;
739  // wait for command to complete
740  while (!(TWCR & (1 << TWINT)));
741 
742  // save status bits
743  status = TWSR & 0xF8;
744 }
745 
746 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
747 {
748  uint8_t i, buff[numOfBytes];
749 
750  TIMSK1 &= ~(1 << OCIE1A);
751 
752  I2C.start(slaveAddr, WRITE);
753 
754  I2C.writeByte(regAddr);
755 
756  I2C.restart(slaveAddr, READ);
757 
758  for(i = 0; i < (numOfBytes - 1); i++)
759  {
760  I2C.readByte(ACK, &data[i]);
761  }
762 
763  I2C.readByte(NACK, &data[numOfBytes-1]);
764 
765  I2C.stop();
766 
767  TIMSK1 |= (1 << OCIE1A);
768 
769  return 1;
770 }
771 
772 bool i2cMaster::write(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
773 {
774  uint8_t i;
775 
776  TIMSK1 &= ~(1 << OCIE1A);
777 
778  I2C.start(slaveAddr, WRITE);
779  I2C.writeByte(regAddr);
780 
781  for(i = 0; i < numOfBytes; i++)
782  {
783  I2C.writeByte(*(data + i));
784  }
785  I2C.stop();
786 
787  TIMSK1 |= (1 << OCIE1A);
788 
789  return 1;
790 }
791 
792 bool i2cMaster::readByte(bool ack, uint8_t *data)
793 {
794  if(ack)
795  {
796  this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA));
797  }
798 
799  else
800  {
801  this->cmd((1 << TWINT) | (1 << TWEN));
802  }
803 
804  *data = TWDR;
805 
806  return 1;
807 }
808 
809 bool i2cMaster::start(uint8_t addr, bool RW)
810 {
811  // send START condition
812  this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
813 
814  if (this->getStatus() != START && this->getStatus() != REPSTART)
815  {
816  return false;
817  }
818 
819  // send device address and direction
820  TWDR = (addr << 1) | RW;
821  this->cmd((1 << TWINT) | (1 << TWEN));
822 
823  if (RW == READ)
824  {
825  return this->getStatus() == RXADDRACK;
826  }
827 
828  else
829  {
830  return this->getStatus() == TXADDRACK;
831  }
832 }
833 
834 bool i2cMaster::restart(uint8_t addr, bool RW)
835 {
836  return this->start(addr, RW);
837 }
838 
839 bool i2cMaster::writeByte(uint8_t data)
840 {
841  TWDR = data;
842 
843  this->cmd((1 << TWINT) | (1 << TWEN));
844 
845  return this->getStatus() == TXDATAACK;
846 }
847 
848 bool i2cMaster::stop(void)
849 {
850  uint16_t i = 0;
851  // issue stop condition
852  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
853 
854  // wait until stop condition is executed and bus released
855  while (TWCR & (1 << TWSTO));
856 
857  status = I2CFREE;
858 
859  return 1;
860 }
861 
862 uint8_t i2cMaster::getStatus(void)
863 {
864  return status;
865 }
866 
868 {
869  // set bit rate register to 12 to obtain 400kHz scl frequency (in combination with no prescaling!)
870  TWBR = 12;
871  // no prescaler
872  TWSR &= 0xFC;
873 }
874 
876 {
877 
878 }
#define R
Definition: uStepper.h:165
uStepper(void)
Constructor of uStepper class.
Definition: uStepper.cpp:295
uStepperEncoder(void)
Constructor.
Definition: uStepper.cpp:171
uint32_t currentStep
Definition: uStepper.h:406
uint16_t getStrength(void)
Measure the strength of the magnet.
Definition: uStepper.cpp:251
#define CRUISE
Definition: uStepper.h:146
#define WRITE
Definition: uStepper.h:171
float getSpeed(void)
Measure the current speed of the motor.
Definition: uStepper.cpp:207
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:772
bool getMotorState(void)
Get the current state of the motor.
Definition: uStepper.cpp:712
float acceleration
Definition: uStepper.h:388
bool continous
Definition: uStepper.h:410
Prototype of class for accessing all features of the uStepper in a single object. ...
Definition: uStepper.h:380
volatile float curSpeed
Definition: uStepper.h:369
void enableMotor(void)
Enables the stepper driver output stage.
Definition: uStepper.cpp:697
#define A
Definition: uStepper.h:203
float getMaxAcceleration(void)
Get the value of the maximum motor acceleration.
Definition: uStepper.cpp:342
#define CW
Definition: uStepper.h:150
#define DECEL
Definition: uStepper.h:147
float exactDelay
Definition: uStepper.h:418
float getTemp(void)
Request a reading of current temperature.
Definition: uStepper.cpp:156
float exactDelayDecel
Definition: uStepper.h:394
#define RXADDRACK
Definition: uStepper.h:181
uint32_t cruiseSteps
Definition: uStepper.h:404
uint32_t totalSteps
Definition: uStepper.h:408
#define C
Definition: uStepper.h:205
bool getCurrentDirection(void)
Returns the direction the motor is currently configured to rotate.
Definition: uStepper.cpp:707
float getAngleMoved(void)
Measure the angle moved from reference position.
Definition: uStepper.cpp:176
uint32_t accelSteps
Definition: uStepper.h:398
#define ENCODERADDR
Definition: uStepper.h:155
void hardStop(bool holdMode)
Stop the motor without deceleration.
Definition: uStepper.cpp:603
void setMaxAcceleration(float accel)
Set the maximum acceleration of the stepper motor.
Definition: uStepper.cpp:323
void startTimer(void)
Starts timer for stepper algorithm.
Definition: uStepper.cpp:683
void setMaxVelocity(float vel)
Sets the maximum rotational velocity of the motor.
Definition: uStepper.cpp:347
void moveSteps(uint32_t steps, bool dir, bool holdMode)
Make the motor perform a predefined number of steps.
Definition: uStepper.cpp:458
#define STOP
Definition: uStepper.h:144
float getMaxVelocity(void)
Returns the maximum rotational velocity of the motor.
Definition: uStepper.cpp:380
float multiplier
Definition: uStepper.h:392
bool writeByte(uint8_t data)
Writes a byte to a device on the I2C bus.
Definition: uStepper.cpp:839
i2cMaster(void)
Constructor.
Definition: uStepper.cpp:875
void softStop(bool holdMode)
Stop the motor with deceleration.
Definition: uStepper.cpp:629
uint8_t status
Definition: uStepper.h:660
uint8_t detectMagnet(void)
Detect if magnet is present and within range.
Definition: uStepper.cpp:269
uint8_t getStatus(void)
Get current I2C status.
Definition: uStepper.cpp:862
#define READ
Definition: uStepper.h:169
void cmd(uint8_t cmd)
Sends commands over the I2C bus.
Definition: uStepper.cpp:734
bool restart(uint8_t addr, bool RW)
Restarts connection between arduino and I2C device.
Definition: uStepper.cpp:834
bool readByte(bool ack, uint8_t *data)
Reads a byte from the I2C bus.
Definition: uStepper.cpp:792
int64_t stepsSinceReset
Definition: uStepper.h:416
uStepperEncoder encoder
Definition: uStepper.h:463
#define HARD
Definition: uStepper.h:152
void stopTimer(void)
Stops the timer for the stepper algorithm.
Definition: uStepper.cpp:692
uint8_t getAgc(void)
Read the current AGC value of the encoder chip.
Definition: uStepper.cpp:260
bool start(uint8_t addr, bool RW)
sets up connection between arduino and I2C device.
Definition: uStepper.cpp:809
#define MAGNITUDE
Definition: uStepper.h:160
void runContinous(bool dir)
Make the motor rotate continuously.
Definition: uStepper.cpp:385
#define INTFREQ
Definition: uStepper.h:149
#define ENCODERSPEEDCONSTANT
Definition: uStepper.h:163
Prototype of class for accessing the TWI (I2C) interface of the AVR (master mode only).
Definition: uStepper.h:657
#define ANGLE
Definition: uStepper.h:157
void setHome(void)
Define new reference(home) position.
Definition: uStepper.cpp:229
#define SOFT
Definition: uStepper.h:153
uint16_t cruiseDelay
Definition: uStepper.h:384
float getAngle(void)
Measure the current shaft angle.
Definition: uStepper.cpp:239
#define ACK
Definition: uStepper.h:183
#define B
Definition: uStepper.h:204
bool stop(void)
Closes the I2C connection.
Definition: uStepper.cpp:848
#define AGC
Definition: uStepper.h:159
i2cMaster I2C
Definition: uStepper.cpp:82
volatile float oldAngle
Definition: uStepper.h:368
#define I2CFREE
Definition: uStepper.h:167
#define REPSTART
Definition: uStepper.h:175
void begin(void)
Setup TWI (I2C) interface.
Definition: uStepper.cpp:867
#define TXDATAACK
Definition: uStepper.h:179
#define NACK
Definition: uStepper.h:185
Function prototypes and definitions for the uStepper library.
#define ACCEL
Definition: uStepper.h:145
#define INITDECEL
Definition: uStepper.h:148
float velocity
Definition: uStepper.h:386
volatile float angleMoved
Definition: uStepper.h:370
void setup(void)
Initializes the different parts of the uStepper object.
Definition: uStepper.cpp:672
uint8_t state
Definition: uStepper.h:396
uStepperTemp(void)
Constructor.
Definition: uStepper.cpp:151
uint16_t delay
Definition: uStepper.h:390
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:746
uint32_t decelSteps
Definition: uStepper.h:400
bool direction
Definition: uStepper.h:414
bool hold
Definition: uStepper.h:412
#define START
Definition: uStepper.h:173
float encoderOffset
Definition: uStepper.h:367
void disableMotor(void)
Disables the stepper driver output stage.
Definition: uStepper.cpp:702
#define TXADDRACK
Definition: uStepper.h:177
int64_t getStepsSinceReset(void)
Get the number of steps applied since reset.
Definition: uStepper.cpp:722
void setup(void)
Setup the encoder.
Definition: uStepper.cpp:212
uint32_t initialDecelSteps
Definition: uStepper.h:402