uStepper S-lite
uStepperSLite.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepperSLite.cpp *
3 * Version: 1.2.0 *
4 * Date: Jan 18, 2020 *
5 * Author: Thomas Hørring Olsen *
6 * *
7 *********************************************************************************************
8 * uStepper S-lite class *
9 * *
10 * This file contains the implementation of the class methods, incorporated in the *
11 * uStepper S-lite arduino library. The library is used by instantiating an uStepper *
12 * S-lite object by calling either of the two overloaded constructors: *
13 * *
14 * example: *
15 * *
16 * uStepperSLite stepper; *
17 * *
18 * OR *
19 * *
20 * uStepperSLite stepper(500, 2000); *
21 * *
22 * The first instantiation above creates a uStepper S-lite object with default *
23 * acceleration 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 * uStepperSLite 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) 2018 *
48 * *
49 * uStepper ApS *
50 * www.ustepper.com *
51 * administration@ustepper.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 uStepper ApS nor the author, can be held responsible for any damage *
59 * caused by the use of the code contained in this file ! *
60 * *
61 ********************************************************************************************/
71 #include <uStepperSLite.h>
72 #include <math.h>
73 uStepperSLite *pointer;
74 volatile int32_t *p __attribute__((used));
76 extern "C" {
77 
78 void INT0_vect(void)
79 {
80  if(PIND & 0x04)
81  {
82  PORTD |= (1 << 4);
83  }
84  else
85  {
86  PORTD &= ~(1 << 4);
87  }
88  if((PINB & (0x08))) //CCW
89  {
90  if(!pointer->invertPidDropinDirection)
91  {
92  pointer->stepCnt--; //DIR is set to CCW, therefore we subtract 1 step from step count (negative values = number of steps in CCW direction from initial postion)
93  }
94  else
95  {
96  pointer->stepCnt++; //DIR is set to CW, therefore we add 1 step to step count (positive values = number of steps in CW direction from initial postion)
97  }
98 
99  }
100  else //CW
101  {
102  if(!pointer->invertPidDropinDirection)
103  {
104  pointer->stepCnt++; //DIR is set to CW, therefore we add 1 step to step count (positive values = number of steps in CW direction from initial postion)
105  }
106  else
107  {
108  pointer->stepCnt--; //DIR is set to CCW, therefore we subtract 1 step from step count (negative values = number of steps in CCW direction from initial postion)
109  }
110  }
111 }
112 
113 void INT1_vect(void)
114 {
115  if(PIND & 0x04)
116  {
117  PORTD |= (1 << 4);
118  }
119  else
120  {
121  PORTD &= ~(1 << 4);
122  }
123 }
124 
125 void TIMER3_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,p \n\t");
133  asm volatile("lds r31,p+1 \n\t");
134 
135  asm volatile("jmp _stepGenerator \n\t"); //Execute the acceleration profile algorithm
136 
137 }
138 
139 void TIMER1_COMPA_vect(void)
140  {
141  uint8_t data[2];
142  uint16_t curAngle;
143  int16_t deltaAngle;
144  float posError = 0.0;
145  static float posEst = 0.0;
146  static float velIntegrator = 0.0;
147  static float velEst = 0.0;
148  uint32_t temp;
149  int32_t stepCntTemp;
150  int32_t pidTargetPositionTruncated;
151  int32_t *stepsSinceResetPointer;
152  float tempFloat;
153  sei();
154 
155  if(I2C.getStatus() != I2CFREE)
156  {
157  return;
158  }
159 
160  I2C.read(ENCODERADDR, ANGLE, 2, data);
161 
162  curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
163  pointer->encoder.angle = curAngle;
164  curAngle -= pointer->encoder.encoderOffset;
165 
166  if(curAngle > 4095)
167  {
168  curAngle -= 61440;
169  }
170 
171  deltaAngle = (int16_t)pointer->encoder.oldAngle - (int16_t)curAngle;
172 
173  if(deltaAngle < -2047)
174  {
175  deltaAngle += 4096;
176  }
177 
178  else if(deltaAngle > 2047)
179  {
180  deltaAngle -= 4096;
181  }
182 
183  pointer->encoder.angleMoved -= deltaAngle; //The AS5600 encoder counts in the opposite direction
184  //as the AEAT8800 (the encoder on uStepper S).
185  //Therefore it is subtracted here, so the two boards
186  //behave similar
187 
188  pointer->encoder.oldAngle = curAngle;
189 
190  if(pointer->mode == DROPIN)
191  {
192  cli();
193  stepCntTemp = pointer->stepCnt;
194  sei();
195 
196  // Speed filter
197  posEst += velEst * ENCODERINTSAMPLETIME;
198  posError = (float)stepCntTemp - posEst;
199  velIntegrator += posError * PULSEFILTERKI;
200  velEst = (posError * PULSEFILTERKP) + velIntegrator;
201  pointer->currentPidSpeed = velIntegrator;
202 
203  posError = (float)stepCntTemp - ((float)pointer->encoder.angleMoved * pointer->stepConversion);
204 
205  pointer->pidDropin(posError);
206  return;
207  }
208  else
209  {
210 
211  /*
212  // Speed filter
213  posEst += velEst * ENCODERINTSAMPLETIME;
214  posError = (float)pointer->encoder.angleMoved - posEst;
215  velIntegrator += posError * PULSEFILTERKI;
216  velEst = (posError * PULSEFILTERKP) + velIntegrator;
217  pointer->encoder.curSpeed = velIntegrator * pointer->stepConversion;
218  */
219 
220 
221 
222 
223  //stepgenerator targetposition integrator
224  pointer->pidTargetPosition += pointer->currentPidSpeed * ENCODERINTSAMPLETIME;
225 
226  if(pointer->mode == PID)
227  {
228  pidTargetPositionTruncated = (int32_t)pointer->pidTargetPosition;
229  stepsSinceResetPointer = &pidTargetPositionTruncated;
230  }
231  else
232  {
233  stepsSinceResetPointer = &pointer->stepsSinceReset;
234  }
235 
236  if(pointer->continous == 1)
237  {
238  pointer->targetPosition = (float)pointer->pidTargetPosition;
239  }
240  else
241  {
242  if(pointer->targetPosition < 0)
243  {
244  if(pointer->pidTargetPosition < (float)pointer->targetPosition)
245  {
246  pointer->pidTargetPosition = (float)pointer->targetPosition;
247  }
248  }
249  else
250  {
251  if(pointer->pidTargetPosition > (float)pointer->targetPosition)
252  {
253  pointer->pidTargetPosition = (float)pointer->targetPosition;
254  }
255  }
256  }
257 
258  //acceleration profile generator
259  if(pointer->state == INITDECEL)
260  {
261  if(pointer->direction == CW && *stepsSinceResetPointer > 0 && pointer->direction != pointer->stepGeneratorDirection)
262  {
263  pointer->currentPidAcceleration = pointer->acceleration * ENCODERINTSAMPLETIME;
264  if(*stepsSinceResetPointer <= pointer->decelToAccelThreshold)
265  {
266  pointer->stepGeneratorDirection = pointer->direction;
267  if(pointer->decelToAccelThreshold == pointer->accelToCruiseThreshold)
268  {
269  pointer->state = CRUISE;
270  }
271  else
272  {
273  pointer->state = ACCEL;
274  }
275  }
276  }
277  else if(pointer->direction == CW && *stepsSinceResetPointer < 0 && pointer->direction != pointer->stepGeneratorDirection)
278  {
279  pointer->currentPidAcceleration = pointer->acceleration * ENCODERINTSAMPLETIME;
280  if(*stepsSinceResetPointer <= pointer->decelToAccelThreshold)
281  {
282  pointer->stepGeneratorDirection = pointer->direction;
283  if(pointer->decelToAccelThreshold == pointer->accelToCruiseThreshold)
284  {
285  pointer->state = CRUISE;
286  }
287  else
288  {
289  pointer->state = ACCEL;
290  }
291  }
292  }
293  else if(pointer->direction == CCW && *stepsSinceResetPointer < 0 && pointer->direction != pointer->stepGeneratorDirection)
294  {
295  pointer->currentPidAcceleration = -pointer->acceleration * ENCODERINTSAMPLETIME;
296  if(*stepsSinceResetPointer >= pointer->decelToAccelThreshold)
297  {
298  pointer->stepGeneratorDirection = pointer->direction;
299  if(pointer->decelToAccelThreshold == pointer->accelToCruiseThreshold)
300  {
301  pointer->state = CRUISE;
302  }
303  else
304  {
305  pointer->state = ACCEL;
306  }
307  }
308  }
309  else if(pointer->direction == CCW && *stepsSinceResetPointer > 0 && pointer->direction != pointer->stepGeneratorDirection)
310  {
311  pointer->currentPidAcceleration = -pointer->acceleration * ENCODERINTSAMPLETIME;
312  if(*stepsSinceResetPointer >= pointer->decelToAccelThreshold)
313  {
314  pointer->stepGeneratorDirection = pointer->direction;
315  if(pointer->decelToAccelThreshold == pointer->accelToCruiseThreshold)
316  {
317  pointer->state = CRUISE;
318  }
319  else
320  {
321  pointer->state = ACCEL;
322  }
323  }
324  }
325  else if(pointer->direction == CW && *stepsSinceResetPointer > 0 )
326  {
327  pointer->currentPidAcceleration = -pointer->acceleration * ENCODERINTSAMPLETIME;
328  if(*stepsSinceResetPointer >= pointer->decelToAccelThreshold)
329  {
330  pointer->stepGeneratorDirection = pointer->direction;
331  if(pointer->decelToAccelThreshold == pointer->accelToCruiseThreshold)
332  {
333  pointer->state = CRUISE;
334  }
335  else
336  {
337  pointer->state = ACCEL;
338  }
339  }
340  }
341  else if(pointer->direction == CW && *stepsSinceResetPointer < 0)
342  {
343  pointer->currentPidAcceleration = -pointer->acceleration * ENCODERINTSAMPLETIME;
344  if(*stepsSinceResetPointer >= pointer->decelToAccelThreshold)
345  {
346  pointer->stepGeneratorDirection = pointer->direction;
347  if(pointer->decelToAccelThreshold == pointer->accelToCruiseThreshold)
348  {
349  pointer->state = CRUISE;
350  }
351  else
352  {
353  pointer->state = ACCEL;
354  }
355  }
356  }
357  else if(pointer->direction == CCW && *stepsSinceResetPointer < 0)
358  {
359  pointer->currentPidAcceleration = pointer->acceleration * ENCODERINTSAMPLETIME;
360  if(*stepsSinceResetPointer <= pointer->decelToAccelThreshold)
361  {
362  pointer->stepGeneratorDirection = pointer->direction;
363  if(pointer->decelToAccelThreshold == pointer->accelToCruiseThreshold)
364  {
365  pointer->state = CRUISE;
366  }
367  else
368  {
369  pointer->state = ACCEL;
370  }
371  }
372  }
373  else if(pointer->direction == CCW && *stepsSinceResetPointer > 0)
374  {
375  pointer->currentPidAcceleration = pointer->acceleration * ENCODERINTSAMPLETIME;
376  if(*stepsSinceResetPointer <= pointer->decelToAccelThreshold)
377  {
378  pointer->stepGeneratorDirection = pointer->direction;
379  if(pointer->decelToAccelThreshold == pointer->accelToCruiseThreshold)
380  {
381  pointer->state = CRUISE;
382  }
383  else
384  {
385  pointer->state = ACCEL;
386  }
387  }
388  }
389 
390  }
391  else if(pointer->state == ACCEL)
392  {
393  pointer->stepGeneratorDirection = pointer->direction;
394  if(pointer->direction == CCW)
395  {
396  if(*stepsSinceResetPointer <= pointer->accelToCruiseThreshold)
397  {
398  pointer->state = CRUISE;
399  }
400  pointer->currentPidAcceleration = -(pointer->acceleration * ENCODERINTSAMPLETIME);
401  }
402  else
403  {
404  if(*stepsSinceResetPointer >= pointer->accelToCruiseThreshold)
405  {
406  pointer->state = CRUISE;
407  }
408  pointer->currentPidAcceleration = pointer->acceleration * ENCODERINTSAMPLETIME;
409  }
410  }
411  else if(pointer->state == CRUISE)
412  {
413  pointer->stepGeneratorDirection = pointer->direction;
414  if(pointer->continous == 1)
415  {
416  pointer->state = CRUISE;
417  }
418  else
419  {
420  if(pointer->direction == CCW)
421  {
422  if(*stepsSinceResetPointer <= pointer->cruiseToDecelThreshold)
423  {
424  pointer->state = DECEL;
425  }
426  }
427  else
428  {
429  if(*stepsSinceResetPointer >= pointer->cruiseToDecelThreshold)
430  {
431  pointer->state = DECEL;
432  }
433  }
434  }
435 
436  pointer->currentPidAcceleration = 0;
437 
438  }
439  else if(pointer->state == DECEL)
440  {
441  pointer->stepGeneratorDirection = pointer->direction;
442  if(pointer->direction == CW)
443  {
444  if(*stepsSinceResetPointer >= pointer->decelToStopThreshold)
445  {
446  pointer->state = STOP;
447  }
448  pointer->currentPidAcceleration = -(pointer->acceleration * ENCODERINTSAMPLETIME);
449  }
450  else
451  {
452  if(*stepsSinceResetPointer <= pointer->decelToStopThreshold)
453  {
454  pointer->state = STOP;
455  }
456  pointer->currentPidAcceleration = pointer->acceleration * ENCODERINTSAMPLETIME;
457  }
458  }
459  else if(pointer->state == STOP)
460  {
461  pointer->stepGeneratorDirection = pointer->direction;
462  pointer->currentPidAcceleration = 0.0;
463  pointer->currentPidSpeed = 0.0;
464  TCCR3B &= ~(1 << CS30);
465  if(pointer->mode == NORMAL)
466  {
467  if(pointer->brake == BRAKEON)
468  {
469  PORTD &= ~(1 << 4);
470  }
471  else
472  {
473  PORTD |= (1 << 4);
474  }
475  }
476  }
477 
478  if(pointer->mode == PID && !pointer->pidDisabled)
479  {
480  tempFloat = (float)pointer->encoder.angleMoved * pointer->stepConversion;
481  pointer->stepsSinceReset = (int32_t)(tempFloat);
482  pointer->pid((float)pointer->pidTargetPosition - tempFloat);
483  }
484  if(pointer->mode == NORMAL || pointer->pidDisabled)
485  {
486  if(pointer->currentPidSpeed > 5.0)
487  {
488  temp = (uint32_t)((STEPGENERATORFREQUENCY/(pointer->currentPidSpeed)) + 0.5);
489  cli();
490  pointer->stepDelay = temp;
491  sei();
492  }
493  else if(pointer->currentPidSpeed < -5.0)
494  {
495  temp = (uint32_t)((STEPGENERATORFREQUENCY/(-pointer->currentPidSpeed)) + 0.5);
496  cli();
497  pointer->stepDelay = temp;
498  sei();
499  }
500  else
501  {
502  cli();
503  pointer->stepDelay = 20000;
504  sei();
505  }
506  }
507  //stepGenerator speed integrator
508  pointer->currentPidSpeed += pointer->currentPidAcceleration;
509  if(pointer->direction == CW)
510  {
511  if(pointer->currentPidAcceleration > 0)
512  {
513  if(pointer->currentPidSpeed >= pointer->velocity && pointer->state != INITDECEL)
514  {
515  pointer->currentPidSpeed = pointer->velocity;
516  }
517  }
518  else if(pointer->currentPidAcceleration < 0)
519  {
520  if(pointer->currentPidSpeed <= 0.0&& pointer->state != INITDECEL)
521  {
522  pointer->currentPidSpeed = 0.0;
523  }
524  else if(pointer->currentPidSpeed <= pointer->velocity&& pointer->state != INITDECEL && pointer->state != DECEL)
525  {
526  pointer->currentPidSpeed = pointer->velocity;
527  }
528  }
529  else if(pointer->currentPidSpeed < 0.0)
530  {
531  //pointer->currentPidSpeed = 0.0;
532  }
533  }
534  else
535  {
536  if(pointer->currentPidAcceleration < 0)
537  {
538  if(pointer->currentPidSpeed <= -pointer->velocity && pointer->state != INITDECEL)
539  {
540  pointer->currentPidSpeed = -pointer->velocity;
541  }
542  }
543  else if(pointer->currentPidAcceleration > 0)
544  {
545  if(pointer->currentPidSpeed >= 0 && pointer->state != INITDECEL)
546  {
547  pointer->currentPidSpeed = 0;
548  }
549  else if(pointer->currentPidSpeed >= -pointer->velocity && pointer->state != INITDECEL && pointer->state != DECEL)
550  {
551  pointer->currentPidSpeed = -pointer->velocity;
552  }
553  }
554  else if(pointer->currentPidSpeed > 0.0)
555  {
556  //pointer->currentPidSpeed = 0.0;
557  }
558  }
559 
560  pointer->detectStall();
561  }
562  }
563 }
564 
566 {
567  I2C.begin();
568 }
569 
571 {
572  return (float)this->angleMoved*0.087890625;
573 }
574 
576 {
577  if(unit == RPM)
578  {
579  return this->curSpeed * pointer->stepsPerSecondToRPM;
580  }
581  else //StepsPerSecond
582  {
583  return this->curSpeed;
584  }
585 }
586 
588 {
589  TCNT1 = 0;
590  ICR1 = 32000;
591  TIFR1 = 0;
592  TIMSK1 = (1 << OCIE1A);
593  TCCR1A = (1 << WGM11);
594  TCCR1B = (1 << WGM12) | (1 << WGM13) | (1 << CS10);
595 }
596 
598 {
599  cli();
600  uint8_t data[2];
601  TIMSK1 &= ~(1 << OCIE1A);
602  I2C.read(ENCODERADDR, ANGLE, 2, data);
603  TIMSK1 |= (1 << OCIE1A);
604  this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
605  pointer->stepsSinceReset = 0;
606  this->angle = 0;
607  this->oldAngle = 0;
608  this->angleMoved = 0;
609  pointer->pidTargetPosition = 0.0;
610  pointer->targetPosition = 0;
611  pointer->pidError = 0;
612  sei();
613 }
614 
616 {
617  return (float)this->angle*0.087890625;
618 }
619 
621 {
622  uint8_t data[2];
623 
624  TIMSK1 &= ~(1 << OCIE1A);
625  I2C.read(ENCODERADDR, MAGNITUDE, 2, data);
626  TIMSK1 |= (1 << OCIE1A);
627 
628  return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
629 }
630 
632 {
633  uint8_t data;
634  TIMSK1 &= ~(1 << OCIE1A);
635  I2C.read(ENCODERADDR, AGC, 1, &data);
636  TIMSK1 |= (1 << OCIE1A);
637  return data;
638 }
639 
641 {
642  uint8_t data;
643  TIMSK1 &= ~(1 << OCIE1A);
644  I2C.read(ENCODERADDR, STATUS, 1, &data);
645  TIMSK1 |= (1 << OCIE1A);
646  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 !
647 
648  if(data == 0x08)
649  {
650  return 1; //magnet too strong
651  }
652 
653  else if(data == 0x10)
654  {
655  return 2; //magnet too weak
656  }
657 
658  else if(data == 0x20)
659  {
660  return 0; //magnet detected and within limits
661  }
662 
663  return 3; //Something went horribly wrong !
664 }
665 
666 uStepperSLite::uStepperSLite(float accel, float vel)
667 {
668  this->state = STOP;
669 
670  this->setMaxVelocity(vel);
671  this->setMaxAcceleration(accel);
672 
673  pointer = this;
674 
675  DDRB |= (1 << 2); //set direction pin to output
676  DDRD |= (1 << 7); //set step pin to output
677  DDRD |= (1 << 4); //set enable pin to output
678 }
679 
681 {
682  this->acceleration = accel;
683 
684  if(this->state != STOP)
685  {
686  if(this->continous == 1) //If motor was running continously
687  {
688  this->runContinous(this->direction); //We should make it run continously again
689  }
690  else //If motor still needs to perform some steps
691  {
692  this->moveSteps(abs(this->targetPosition - this->stepsSinceReset), this->direction, this->brake); //we should make sure the motor gets to execute the remaining steps
693  }
694  }
695 }
696 
698 {
699 
700  if(vel < 0.5005)
701  {
702  this->velocity = 0.5005; //Limit velocity in order to not overflow delay variable
703 
704  }
705 
706  else if(vel > 100000.0)
707  {
708  this->velocity = 100000.0; //limit velocity in order to not underflow delay variable
709  }
710 
711  else
712  {
713  this->velocity = vel;
714  }
715 
716  if(this->state != STOP) //If motor was running, we should make sure it runs again
717  {
718 
719  if(this->continous == 1) //If motor was running continously
720  {
721  this->runContinous(this->direction); //We should make it run continously again
722  }
723  else //If motor still needs to perform some steps
724  {
725  this->moveSteps(abs(this->targetPosition - this->stepsSinceReset), this->direction, this->brake); //we should make sure the motor gets to execute the remaining steps
726  }
727  }
728 }
729 
731 {
732  float curVel, startVelocity = 0;
733  uint8_t tempState;
734  uint32_t accelSteps;
735  uint32_t initialDecelSteps;
736 
737  if(this->mode == DROPIN)
738  {
739  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
740  }
741 
742  cli();
743  curVel = this->currentPidSpeed;
744  sei();
745 
746  if(this->state == STOP) //If motor is currently running at desired speed
747  {
748  initialDecelSteps = 0;
749  tempState = ACCEL; //We should just run at cruise speed
750  startVelocity = 0.0;//sqrt(2.0*this->acceleration); //number of interrupts before the first step should be performed.
751  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)))
752 
753  }
754  else if((dir == CW && curVel < 0) || (dir == CCW && curVel > 0)) //If motor turns CCW and should turn CW, or if motor turns CW and shoúld turn CCW
755  {
756  tempState = INITDECEL; //We should decelerate the motor to full stop
757  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)))
758  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)))
759  startVelocity = curVel;//sqrt((curVel*curVel) + 2.0*this->acceleration); //number of interrupts before the first step should be performed.
760  }
761  else if((dir == CW && curVel > 0) || (dir == CCW && curVel < 0)) //If the motor is currently rotating the same direction as the desired direction
762  {
763  startVelocity = curVel;
764  if(abs(curVel) > this->velocity) //If current velocity is greater than desired velocity
765  {
766  tempState = INITDECEL; //We need to decelerate the motor to desired velocity
767  initialDecelSteps = (uint32_t)(((abs(curVel)*abs(curVel)) - (this->velocity*this->velocity))/(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)))
768  accelSteps = 0; //No acceleration phase is needed
769  }
770 
771  else if(abs(curVel) < this->velocity) //If the current velocity is less than the desired velocity
772  {
773  tempState = ACCEL; //Start accelerating
774  accelSteps = (uint32_t)(((this->velocity*this->velocity) - (abs(curVel)*abs(curVel)))/(2.0*this->acceleration)); //Number of Steps needed to accelerate from current velocity to full speed
775  initialDecelSteps = 0;
776  }
777 
778  else //If motor is currently running at desired speed
779  {
780  initialDecelSteps = 0;
781  tempState = CRUISE; //We should just run at cruise speed
782  accelSteps = 0; //No acceleration phase is needed
783  }
784  }
785  else
786  {
787  initialDecelSteps = 0;
788  tempState = ACCEL; //We should just run at cruise speed
789  startVelocity = 0.0;//sqrt(2.0*this->acceleration); //number of interrupts before the first step should be performed.
790  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)))
791  }
792  cli();
793  this->direction = dir;
794 
795  this->continous = 1;
796 
797  if(dir == CW)
798  {
799  if(this->direction != this->stepGeneratorDirection)
800  {
801  this->decelToAccelThreshold = this->stepsSinceReset - initialDecelSteps;
802  }
803  else
804  {
805  this->decelToAccelThreshold = this->stepsSinceReset + initialDecelSteps;
806  }
807 
808  this->accelToCruiseThreshold = this->decelToAccelThreshold + accelSteps;
809  if(tempState != INITDECEL)
810  {
811  PORTB |= (1 << 2);
812 
813  }
814  else
815  {
816  PORTB &= ~(1 << 2);
817  }
818 
819 
820  }
821  else
822  {
823  if(this->direction != this->stepGeneratorDirection)
824  {
825  this->decelToAccelThreshold = this->stepsSinceReset + initialDecelSteps;
826  }
827  else
828  {
829  this->decelToAccelThreshold = this->stepsSinceReset - initialDecelSteps;
830  }
831 
832 
833  this->accelToCruiseThreshold = this->decelToAccelThreshold - accelSteps;
834  if(tempState != INITDECEL)
835  {
836  PORTB &= ~(1 << 2);
837  }
838  else
839  {
840  PORTB |= (1 << 2);
841  }
842 
843 
844  }
845  this->currentPidSpeed = startVelocity;
846  if(pointer->currentPidSpeed > 5.0)
847  {
848  pointer->stepDelay = (uint32_t)((STEPGENERATORFREQUENCY/(pointer->currentPidSpeed)) + 0.5);
849  }
850  else if(pointer->currentPidSpeed < -5.0)
851  {
852  pointer->stepDelay = (uint32_t)((STEPGENERATORFREQUENCY/(-pointer->currentPidSpeed)) + 0.5);
853  }
854  else
855  {
856  pointer->stepDelay = 20000;
857  }
858  this->state = tempState;
859  Serial.print("\r\nstate: ");
860  Serial.println(this->state);
861  sei();
862  Serial.print("\r\nstate: ");
863  Serial.println(this->state);
864  Serial.print(" StepsSinceReset: ");
865  Serial.println(this->stepsSinceReset);
866  Serial.print(" CurrentSpeed: ");
867  Serial.println(this->currentPidSpeed);
868  Serial.print(" currentPidAcceleration: ");
869  Serial.println(this->currentPidAcceleration);
870  Serial.print(" Dir: ");
871  Serial.println(this->direction);
872  Serial.print(" Dir: ");
873  Serial.println(this->stepGeneratorDirection);
874  Serial.print(" this->decelToAccelThreshold: ");
875  Serial.println(this->decelToAccelThreshold);
876  Serial.print(" this->accelToCruiseThreshold: ");
877  Serial.println(this->accelToCruiseThreshold);
878  Serial.println("");
879 
880  PORTD &= ~(1 << 4);
881  TCCR3B |= (1 << CS30);
882 }
883 
884 void uStepperSLite::moveSteps(int32_t steps, bool dir, bool holdMode)
885 {
886  float curVel, startVelocity = 0;
887  uint8_t state;
888  uint32_t totalSteps;
889  uint32_t accelSteps;
890  uint32_t decelSteps;
891  uint32_t initialDecelSteps;
892  uint32_t cruiseSteps = 0;
893 
894  if(this->mode == DROPIN)
895  {
896  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
897  }
898 
899  if(steps < 1)
900  {
901  return;
902  }
903  //steps--;
904  totalSteps = steps;
905  cli();
906  curVel = this->currentPidSpeed;
907  sei();
908 
909  initialDecelSteps = 0;
910 
911  if(this->state == STOP) //If motor is currently at full stop (state = STOP)
912  {
913  state = ACCEL;
914  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)))
915  //No initial deceleration phase needed
916 
917  if(accelSteps > (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
918  {
919  cruiseSteps = 0; //No cruise phase needed
920  accelSteps = decelSteps = (totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
921  accelSteps += totalSteps - accelSteps - decelSteps; //if there are still a step left to perform, due to rounding errors, do this step as an acceleration step
922  }
923 
924  else
925  {
926  decelSteps = accelSteps; //If top speed is reached before half the total steps are performed, deceleration period should be same length as acceleration period
927  cruiseSteps = totalSteps - accelSteps - decelSteps; //Perform remaining steps as cruise steps
928  }
929  startVelocity = 0.0;//sqrt(2.0*this->acceleration); //number of interrupts before the first step should be performed.
930  }
931  else if((dir == CW && curVel < 0) || (dir == CCW && curVel > 0)) //If motor turns CCW and should turn CW, or if motor turns CW and shoúld turn CCW
932  {
933  state = INITDECEL; //We should decelerate the motor to full stop
934  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)))
935  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)))
936  totalSteps += 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
937 
938  if(accelSteps > (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
939  {
940  accelSteps = decelSteps = (totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
941  accelSteps += totalSteps - accelSteps - decelSteps; //If there are still a step left to perform, due to rounding errors, do this step as an acceleration step
942  cruiseSteps = 0;
943  }
944  else
945  {
946  decelSteps = accelSteps; //If top speed is reached before half the total steps are performed, deceleration period should be same length as acceleration period
947  cruiseSteps = totalSteps - accelSteps - decelSteps; //Perform remaining steps, as cruise steps
948  }
949  startVelocity = curVel; //sqrt((curVel*curVel) + 2.0*this->acceleration); //number of interrupts before the first step should be performed.
950  }
951  else if((dir == CW && curVel > 0) || (dir == CCW && curVel < 0)) //If the motor is currently rotating the same direction as desired, we dont necessarily need to decelerate
952  {
953  startVelocity = curVel;
954  if(abs(curVel) > this->velocity) //If current velocity is greater than desired velocity
955  {
956  state = INITDECEL; //We need to decelerate the motor to desired velocity
957  initialDecelSteps = (uint32_t)(((abs(curVel)*abs(curVel)) - (this->velocity*this->velocity))/(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)))
958  accelSteps = 0; //No acceleration phase is needed
959  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
960  if(totalSteps <= (initialDecelSteps + decelSteps))
961  {
962  cruiseSteps = 0;
963  }
964  else
965  {
966  cruiseSteps = steps - initialDecelSteps - decelSteps; //Perform remaining steps as cruise steps
967  }
968  }
969 
970  else if(abs(curVel) < this->velocity) //If current velocity is less than desired velocity
971  {
972  state = ACCEL; //Start accelerating
973  accelSteps = (int32_t)((((this->velocity*this->velocity) - abs(curVel)*abs(curVel)))/(2.0*this->acceleration)); //Number of Steps needed to accelerate from current velocity to full speed
974 
975  if(accelSteps > (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
976  {
977  accelSteps = decelSteps = (totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
978  accelSteps += totalSteps - accelSteps - decelSteps; //If there are still a step left to perform, due to rounding errors, do this step as an acceleration step
979  cruiseSteps = 0;
980  }
981  else
982  {
983  decelSteps = accelSteps; //If top speed is reached before half the total steps are performed, deceleration period should be same length as acceleration period
984  cruiseSteps = totalSteps - accelSteps - decelSteps; //Perform remaining steps, as cruise steps
985  }
986 
987  cruiseSteps = totalSteps - accelSteps - decelSteps; //Perform remaining steps as cruise steps
988  initialDecelSteps = 0; //No initial deceleration phase needed
989  }
990 
991  else //If current velocity is equal to desired velocity
992  {
993  state = CRUISE; //We are already at desired speed, therefore we start at cruise phase
994  decelSteps = (int32_t)((this->velocity*this->velocity)/(2.0*this->acceleration)); //Number of steps needed to decelerate the motor from top speed to full stop
995  accelSteps = 0; //No acceleration phase needed
996  initialDecelSteps = 0; //No initial deceleration phase needed
997 
998  if(decelSteps >= totalSteps)
999  {
1000  cruiseSteps = 0;
1001  decelSteps = totalSteps;
1002  }
1003  else
1004  {
1005  cruiseSteps = totalSteps - decelSteps; //Perform remaining steps as cruise steps
1006  }
1007  }
1008  }
1009  else
1010  {
1011  state = ACCEL;
1012  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)))
1013  //No initial deceleration phase needed
1014 
1015  if(accelSteps > (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
1016  {
1017  cruiseSteps = 0; //No cruise phase needed
1018  accelSteps = decelSteps = (totalSteps >> 1); //Accelerate and decelerate for the same amount of steps (half the total steps)
1019  accelSteps += totalSteps - accelSteps - decelSteps; //if there are still a step left to perform, due to rounding errors, do this step as an acceleration step
1020  }
1021 
1022  else
1023  {
1024  decelSteps = accelSteps; //If top speed is reached before half the total steps are performed, deceleration period should be same length as acceleration period
1025  cruiseSteps = totalSteps - accelSteps - decelSteps; //Perform remaining steps as cruise steps
1026  }
1027  startVelocity = 0.0;
1028  }
1029  cli();
1030 
1031  this->direction = dir;
1032 
1033  this->continous = 0;
1034 
1035  if(dir == CW)
1036  {
1037  if(this->direction != this->stepGeneratorDirection)
1038  {
1039  this->decelToAccelThreshold = this->stepsSinceReset - initialDecelSteps;
1040  }
1041  else
1042  {
1043  this->decelToAccelThreshold = this->stepsSinceReset + initialDecelSteps;
1044  }
1045  this->accelToCruiseThreshold = this->decelToAccelThreshold + accelSteps;
1046  this->cruiseToDecelThreshold = this->accelToCruiseThreshold + cruiseSteps;
1047  this->decelToStopThreshold = this->cruiseToDecelThreshold + decelSteps;
1048  if(state != INITDECEL)
1049  {
1050  PORTB |= (1 << 2);
1051  }
1052  else
1053  {
1054  PORTB &= ~(1 << 2);
1055  }
1056  }
1057  else
1058  {
1059  if(this->direction != this->stepGeneratorDirection)
1060  {
1061  this->decelToAccelThreshold = this->stepsSinceReset + initialDecelSteps;
1062  }
1063  else
1064  {
1065  this->decelToAccelThreshold = this->stepsSinceReset - initialDecelSteps;
1066  }
1067  this->accelToCruiseThreshold = this->decelToAccelThreshold - accelSteps;
1068  this->cruiseToDecelThreshold = this->accelToCruiseThreshold - cruiseSteps;
1069  this->decelToStopThreshold = this->cruiseToDecelThreshold - decelSteps;
1070  if(state != INITDECEL)
1071  {
1072  PORTB &= ~(1 << 2);
1073  }
1074  else
1075  {
1076  PORTB |= (1 << 2);
1077  }
1078 
1079  }
1080  this->currentPidSpeed = startVelocity;
1081 
1082  if(pointer->currentPidSpeed > 5.0)
1083  {
1084  pointer->stepDelay = (uint32_t)((STEPGENERATORFREQUENCY/(pointer->currentPidSpeed)) + 0.5);
1085  }
1086  else if(pointer->currentPidSpeed < -5.0)
1087  {
1088  pointer->stepDelay = (uint32_t)((STEPGENERATORFREQUENCY/(-pointer->currentPidSpeed)) + 0.5);
1089  }
1090  else
1091  {
1092  pointer->stepDelay = 20000;
1093  }
1094  this->state = state;
1095  this->targetPosition = this->decelToStopThreshold;
1096  this->brake = holdMode;
1097 
1098  Serial.print("\r\nstate: ");
1099  Serial.println(this->state);
1100  sei();
1101  Serial.print("\r\nstate: ");
1102  Serial.println(this->state);
1103  Serial.print(" StepsSinceReset: ");
1104  Serial.println(this->stepsSinceReset);
1105  Serial.print(" CurrentSpeed: ");
1106  Serial.println(this->currentPidSpeed);
1107  Serial.print(" currentPidAcceleration: ");
1108  Serial.println(this->currentPidAcceleration);
1109  Serial.print(" Dir: ");
1110  Serial.println(this->direction);
1111  Serial.print(" Dir: ");
1112  Serial.println(this->stepGeneratorDirection);
1113  Serial.print(" this->decelToAccelThreshold: ");
1114  Serial.println(this->decelToAccelThreshold);
1115  Serial.print(" this->accelToCruiseThreshold: ");
1116  Serial.println(this->accelToCruiseThreshold);
1117  Serial.println("");
1118 
1119  PORTD &= ~(1 << 4);
1120  TCCR3B |= (1 << CS30);
1121 }
1122 
1123 void uStepperSLite::hardStop(bool holdMode)
1124 {
1125  if(this->mode == DROPIN)
1126  {
1127  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1128  }
1129 
1130  cli();
1131 
1132  this->stepsSinceReset = (int32_t)((float)this->encoder.angleMoved * this->stepConversion);
1133  this->targetPosition = this->stepsSinceReset;
1134  this->pidTargetPosition = this->targetPosition;
1135  this->decelToStopThreshold = this->targetPosition;
1136  pointer->decelToAccelThreshold = pointer->decelToStopThreshold;
1137  pointer->accelToCruiseThreshold = pointer->decelToStopThreshold;
1138  pointer->cruiseToDecelThreshold = pointer->decelToStopThreshold;
1139 
1140  this->state = DECEL;
1141  this->brake = holdMode;
1142  this->continous = 0;
1143  this->stepDelay = 2;
1144  Serial.print("\r\nstate: ");
1145  Serial.println(this->state);
1146  sei();
1147  Serial.print("\r\nstate: ");
1148  Serial.println(this->state);
1149  Serial.print(" StepsSinceReset: ");
1150  Serial.println(this->stepsSinceReset);
1151  Serial.print(" CurrentSpeed: ");
1152  Serial.println(this->currentPidSpeed);
1153  Serial.print(" currentPidAcceleration: ");
1154  Serial.println(this->currentPidAcceleration);
1155  Serial.print(" Dir: ");
1156  Serial.println(this->direction);
1157  Serial.print(" Dir: ");
1158  Serial.println(this->stepGeneratorDirection);
1159  Serial.print(" this->decelToAccelThreshold: ");
1160  Serial.println(this->decelToAccelThreshold);
1161  Serial.print(" this->accelToCruiseThreshold: ");
1162  Serial.println(this->accelToCruiseThreshold);
1163  Serial.println("");
1164  PORTD &= ~(1 << 4);
1165  TCCR3B |= (1 << CS30);
1166 }
1167 
1168 void uStepperSLite::stop(bool brake)
1169 {
1170  this->softStop(brake);
1171 }
1172 
1173 void uStepperSLite::softStop(bool holdMode)
1174 {
1175  uint32_t decelSteps = 0;
1176  float curVel = 0.0;
1177 
1178  if(this->mode == DROPIN)
1179  {
1180  return; //Drop in feature is activated. just return since this function makes no sense with drop in activated!
1181  }
1182 
1183  cli();
1184  curVel = this->currentPidSpeed;
1185  decelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
1186  if(this->direction == CW)
1187  {
1188  this->targetPosition = this->stepsSinceReset + decelSteps;
1189  pointer->decelToStopThreshold = this->targetPosition;
1190  }
1191  else
1192  {
1193  this->targetPosition = this->stepsSinceReset - decelSteps;
1194  pointer->decelToStopThreshold = this->targetPosition;
1195  }
1196 
1197  pointer->decelToAccelThreshold = pointer->decelToStopThreshold;
1198  pointer->accelToCruiseThreshold = pointer->decelToStopThreshold;
1199  pointer->cruiseToDecelThreshold = pointer->decelToStopThreshold;
1200 
1201  this->state = DECEL;
1202  this->brake = holdMode;
1203  this->continous = 0;
1204  this->currentPidSpeed = curVel;
1205  if(pointer->currentPidSpeed > 5.0)
1206  {
1207  pointer->stepDelay = (uint32_t)((STEPGENERATORFREQUENCY/(pointer->currentPidSpeed)) + 0.5);
1208  }
1209  else if(pointer->currentPidSpeed < -5.0)
1210  {
1211  pointer->stepDelay = (uint32_t)((STEPGENERATORFREQUENCY/(-pointer->currentPidSpeed)) + 0.5);
1212  }
1213  else
1214  {
1215  pointer->stepDelay = 20000;
1216  }
1217  Serial.print("\r\nstate: ");
1218  Serial.println(this->state);
1219  sei();
1220  Serial.print("\r\nstate: ");
1221  Serial.println(this->state);
1222  Serial.print(" StepsSinceReset: ");
1223  Serial.println(this->stepsSinceReset);
1224  Serial.print(" CurrentSpeed: ");
1225  Serial.println(this->currentPidSpeed);
1226  Serial.print(" currentPidAcceleration: ");
1227  Serial.println(this->currentPidAcceleration);
1228  Serial.print(" Dir: ");
1229  Serial.println(this->direction);
1230  Serial.print(" Dir: ");
1231  Serial.println(this->stepGeneratorDirection);
1232  Serial.print(" this->decelToAccelThreshold: ");
1233  Serial.println(this->decelToAccelThreshold);
1234  Serial.print(" this->accelToCruiseThreshold: ");
1235  Serial.println(this->accelToCruiseThreshold);
1236  Serial.println("");
1237 
1238  PORTD &= ~(1 << 4);
1239  TCCR3B |= (1 << CS30);
1240 }
1241 
1243 {
1244  uint8_t data[2], i;
1245  uint16_t angle;
1246  int16_t angleDiff[3];
1247 
1248  I2C.read(ENCODERADDR, ANGLE, 2, data);
1249  angle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1250 
1251  PORTB &= ~(1 << 2);
1252 
1253  for(i = 0; i < 50; i++)
1254  {
1255  PORTD |= (1 << 7);
1256  delayMicroseconds(1);
1257  PORTD &= ~(1 << 7);
1258  _delay_ms(10);
1259  }
1260 
1261  angleDiff[0] = (int16_t)angle;
1262 
1263  I2C.read(ENCODERADDR, ANGLE, 2, data);
1264  angle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1265 
1266  angleDiff[0] -= (int16_t)angle;
1267 
1268  PORTB |= (1 << 2);
1269 
1270  for(i = 0; i < 50; i++)
1271  {
1272  PORTD |= (1 << 7);
1273  delayMicroseconds(1);
1274  PORTD &= ~(1 << 7);
1275  _delay_ms(10);
1276  }
1277 
1278  angleDiff[1] = (int16_t)angle;
1279 
1280  I2C.read(ENCODERADDR, ANGLE, 2, data);
1281  angle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1282 
1283  angleDiff[1] -= (int16_t)angle;
1284 
1285  for(i = 0; i < 2; i++)
1286  {
1287  if(angleDiff[i] > 2048)
1288  {
1289  angleDiff[i] -= 4096;
1290  }
1291  else if(angleDiff[i] < -2048)
1292  {
1293  angleDiff[i] += 4096;
1294  }
1295  }
1296 
1297  if((angleDiff[0] > 2 && angleDiff[1] < -2))
1298  {
1299  if(mode == DROPIN)
1300  {
1301  this->driver.invertDirection();
1302  }
1303  }
1304  else
1305  {
1306  if(mode != DROPIN)
1307  {
1308  this->driver.invertDirection();
1309  }
1310  }
1311 }
1312 
1313 void uStepperSLite::setup( uint8_t mode = NORMAL,
1314  float stepsPerRevolution = 3200.0,
1315  float pTerm = 0.75,
1316  float iTerm = 3.0,
1317  float dTerm = 0.0,
1318  bool setHome = true,
1319  uint8_t invert,
1320  uint8_t runCurrent,
1321  uint8_t holdCurrent)
1322 {
1323  dropinCliSettings_t tempSettings;
1324 
1325  p = &(this->stepsSinceReset);
1326  this->pidDisabled = 1;
1327  this->mode = mode;
1328  this->encoder.setup();
1329 
1330  this->state = STOP;
1331  this->driver.setup();
1332  this->driver.enableDriver();
1333  _delay_ms(200);
1334 
1335  if((uint16_t)stepsPerRevolution == FULL)
1336  {
1337  stepsPerRevolution = 200.0;
1338  }
1339  else if((uint16_t)stepsPerRevolution == HALF)
1340  {
1341  stepsPerRevolution = 400.0;
1342  }
1343  else if((uint16_t)stepsPerRevolution == QUARTER)
1344  {
1345  stepsPerRevolution = 800.0;
1346  }
1347  else if((uint16_t)stepsPerRevolution == EIGHT)
1348  {
1349  stepsPerRevolution = 1600.0;
1350  }
1351  else if((uint16_t)stepsPerRevolution == SIXTEEN)
1352  {
1353  stepsPerRevolution = 3200.0;
1354  }
1355 
1356  this->stepConversion = (float)(stepsPerRevolution)/4096.0; //Calculate conversion coefficient from raw encoder data, to actual moved steps
1357  this->angleToStep = ((float)(stepsPerRevolution))/360.0; //Calculate conversion coefficient from angle to corresponding number of steps
1358  this->stepToAngle = 360.0/((float)(stepsPerRevolution)); //Calculate conversion coefficient from steps to corresponding angle
1359  this->stepsPerSecondToRPM = 60.0/stepsPerRevolution;
1360  this->RPMToStepsPerSecond = stepsPerRevolution/60.0;
1361  this->RPMToStepDelay = STEPGENERATORFREQUENCY/this->RPMToStepsPerSecond;
1362  this->encoder.setHome();
1363 
1364  if(this->mode)
1365  {
1366  if(this->mode == DROPIN)
1367  {
1368  //Set Enable, Step and Dir signal pins from 3dPrinter controller as inputs
1369  pinMode(2,INPUT);
1370  pinMode(3,INPUT);
1371  pinMode(4,INPUT);
1372  //Enable internal pull-up resistors on the above pins
1373  digitalWrite(2,HIGH);
1374  digitalWrite(3,HIGH);
1375  digitalWrite(4,HIGH);
1376  EICRA = 0x06;
1377  EIMSK = 0x03;
1378 
1379  Serial.begin(9600);
1380 
1381  tempSettings.P.f = pTerm;
1382  tempSettings.I.f = iTerm;
1383  tempSettings.D.f = dTerm;
1384  tempSettings.invert = invert;
1385  tempSettings.runCurrent = runCurrent;
1386  tempSettings.holdCurrent = holdCurrent;
1387  tempSettings.checksum = this->dropinSettingsCalcChecksum(&tempSettings);
1388 
1389  if(tempSettings.checksum != EEPROM.read(sizeof(dropinCliSettings_t)))
1390  {
1391  this->dropinSettings = tempSettings;
1392  this->saveDropinSettings();
1393  EEPROM.put(sizeof(dropinCliSettings_t),this->dropinSettings.checksum);
1394  this->loadDropinSettings();
1395  }
1396  else
1397  {
1398  if(!this->loadDropinSettings())
1399  {
1400  this->dropinSettings = tempSettings;
1401  this->saveDropinSettings();
1402  EEPROM.put(sizeof(dropinCliSettings_t),this->dropinSettings.checksum);
1403  this->loadDropinSettings();
1404  }
1405  }
1406  delay(10000);
1407  this->dropinPrintHelp();
1408  }
1409  else
1410  {
1411  //Scale supplied controller coefficents. This is done to enable the user to use easier to manage numbers for these coefficients.
1412  this->pTerm = pTerm;
1413  this->iTerm = iTerm * ENCODERINTSAMPLETIME;
1414  this->dTerm = dTerm * ENCODERINTFREQ;
1415  }
1416  }
1417 
1419  this->encoder.setHome();
1420 
1421  this->pidDisabled = 0;
1422  TCNT3 = 0;
1423  ICR3 = 159;
1424  TIFR3 = 0;
1425  TIMSK3 = (1 << OCIE3A);
1426  TCCR3A = (1 << WGM31);
1427  TCCR3B = (1 << WGM32) | (1 << WGM33);
1428  sei();
1429 }
1430 
1432 {
1433  this->driver.enableDriver(); //Enable motor driver
1434 }
1435 
1437 {
1438  this->driver.disableDriver(); //Disable motor driver
1439 }
1440 
1442 {
1443  return this->direction;
1444 }
1445 
1447 {
1448  if(this->state != STOP)
1449  {
1450  return this->state; //Motor running
1451  }
1452 
1453  return 0; //Motor not running
1454 }
1455 
1457 {
1458  return this->stepsSinceReset;
1459 }
1460 
1461 void uStepperSLite::setCurrent(uint8_t runCurrent, uint8_t holdCurrent)
1462 {
1463  this->driver.setCurrent(runCurrent, holdCurrent);
1464 }
1465 
1466 void uStepperSLite::setHoldCurrent(uint8_t holdCurrent)
1467 {
1468  this->driver.setHoldCurrent(holdCurrent);
1469 }
1470 
1471 void uStepperSLite::setRunCurrent(uint8_t runCurrent)
1472 {
1473  this->driver.setRunCurrent(runCurrent);
1474 }
1475 
1476 //Skal GennemgÃ¥s !
1477 float uStepperSLite::moveToEnd(bool dir, float stallSensitivity)
1478 {
1479  uint8_t checks = 0;
1480  float pos = 0.0;
1481  float lengthMoved;
1482 
1483  if(this->mode == DROPIN)
1484  {
1485  return 0.0; //Doesn't make sense in dropin mode
1486  }
1487 
1488  lengthMoved = this->encoder.getAngleMoved();
1489 
1490  this->stop(HARD);
1491  _delay_ms(50);
1492  this->runContinous(dir);
1493  _delay_ms(200);
1494  while(!this->isStalled(stallSensitivity));
1495  this->stop(SOFT);//stop motor without brake
1496 
1497  this->moveSteps(20, !dir, SOFT);
1498  while(this->getMotorState())
1499  {
1500  _delay_ms(1);
1501  }
1502  _delay_ms(100);
1503  if(dir == CW)
1504  {
1505  lengthMoved = this->encoder.getAngleMoved() - lengthMoved;
1506  }
1507  else
1508  {
1509  lengthMoved -= this->encoder.getAngleMoved();
1510  }
1511  this->encoder.setHome();//set new home position
1512 
1513  return lengthMoved;
1514 }
1515 
1516 void uStepperSLite::moveToAngle(float angle, bool holdMode)
1517 {
1518  float diff;
1519  uint32_t steps;
1520 
1521  diff = angle - this->encoder.getAngleMoved();
1522  steps = (uint32_t)((abs(diff)*angleToStep) + 0.5);
1523 
1524  if(diff < 0.0)
1525  {
1526  this->moveSteps(steps, CCW, holdMode);
1527  }
1528  else
1529  {
1530  this->moveSteps(steps, CW, holdMode);
1531  }
1532 }
1533 
1534 void uStepperSLite::moveAngle(float angle, bool holdMode)
1535 {
1536  int32_t steps;
1537 
1538  if(angle < 0.0)
1539  {
1540  steps = -(int32_t)((angle*angleToStep) - 0.5);
1541  this->moveSteps(steps, CCW, holdMode);
1542  }
1543  else
1544  {
1545  steps = (int32_t)((angle*angleToStep) + 0.5);
1546  this->moveSteps(steps, CW, holdMode);
1547  }
1548 }
1549 
1550 void uStepperSLite::pid(float error)
1551 {
1552  float u, uSat, temp;
1553  float limit = abs(this->currentPidSpeed) + 6000.0;
1554  static float integral;
1555  static bool integralReset = 0;
1556 
1557  if(this->pidDisabled)
1558  {
1559  integral = 0.0;
1560  integralReset = 0;
1561  this->currentPidError = 0.0;
1562  if(this->state == STOP)
1563  {
1564  if(this->brake == BRAKEON)
1565  {
1566  PORTD &= ~(1 << 4);
1567  }
1568  else
1569  {
1570  PORTD |= (1 << 4);
1571  }
1572  }
1573 
1574  return;
1575  }
1576 
1577  PORTD &= ~(1 << 4);
1578 
1579  this->currentPidError = error;
1580 
1581  u = error*this->pTerm;
1582 
1583  if(u > 0.0)
1584  {
1585  if(u > limit)
1586  {
1587  u = limit;
1588  }
1589  }
1590  else if(u < 0.0)
1591  {
1592  if(u < -limit)
1593  {
1594  u = -limit;
1595  }
1596  }
1597 
1598  integral += error*this->iTerm;
1599 
1600  if(integral > 10000.0)
1601  {
1602  integral = 10000.0;
1603  }
1604  else if(integral < -10000.0)
1605  {
1606  integral = -10000.0;
1607  }
1608 
1609  u += integral;
1610 
1611  if(u > 100000 || u < -100000)
1612  {
1613  uSat = 100000;
1614  uSat = ((u > 0) - (u < 0)) * uSat;
1615  }
1616  else
1617  {
1618  uSat = u;
1619  }
1620 
1621  if(error < -2.0 || error > 2.0)
1622  {
1623  if(error < 0.0)
1624  {
1625  if(error < -255.0)
1626  {
1627  this->pidError = 255;
1628  }
1629  else
1630  {
1631  this->pidError = (uint8_t)-error;
1632  }
1633  }
1634  else
1635  {
1636  if(error > 255.0)
1637  {
1638  this->pidError = 255;
1639  }
1640  else
1641  {
1642  this->pidError = (uint8_t)error;
1643  }
1644  }
1645  TCCR3B |= (1 << CS30);
1646  }
1647  else
1648  {
1649 
1650  if(this->state == STOP)
1651  {
1652  TCCR3B &= ~(1 << CS30);
1653  }
1654  this->pidError = 0;
1655  }
1656 
1657  if(uSat > 5.0)
1658  {
1659  temp = (uint32_t)((STEPGENERATORFREQUENCY/uSat) + 0.5);
1660  this->stepGeneratorDirectionPid = CW;
1661 
1662  cli();
1663  pointer->stepDelay = temp;
1664  sei();
1665  }
1666  else if(uSat < -5.0)
1667  {
1668  this->stepGeneratorDirectionPid = CCW;
1669 
1670  temp = (uint32_t)((STEPGENERATORFREQUENCY/-uSat) + 0.5);
1671  cli();
1672  pointer->stepDelay = temp;
1673  sei();
1674  }
1675  else if(uSat > 0.0)
1676  {
1677  this->stepGeneratorDirectionPid = CW;
1678  cli();
1679  pointer->stepDelay = 20000;
1680  sei();
1681  }
1682  else if(uSat < 0.0)
1683  {
1684  this->stepGeneratorDirectionPid = CCW;
1685 
1686  cli();
1687  pointer->stepDelay = 20000;
1688  sei();
1689  }
1690  else
1691  {
1692  this->stepGeneratorDirectionPid = this->stepGeneratorDirection;
1693  cli();
1694  pointer->stepDelay = 20000;
1695  sei();
1696  }
1697 }
1698 
1700 {
1701  this->pidDisabled = 1;
1702 }
1703 
1705 {
1706  cli();
1707  this->pidDisabled = 0;
1708  this->pidTargetPosition = this->encoder.angleMoved * this->stepConversion;
1709  this->targetPosition = this->pidTargetPosition;
1710  this->state = STOP;
1711  this->pidError = 0;
1712  sei();
1713 }
1714 
1716 {
1717  return this->currentPidError;
1718 }
1719 
1720 void uStepperSLite::pidDropin(float error)
1721 {
1722  float u;
1723  float limit = abs(this->currentPidSpeed) + 6000.0;
1724  static float integral;
1725  static bool integralReset = 0;
1726 
1727  PORTD &= ~(1 << 4);
1728 
1729  this->currentPidError = error;
1730 
1731  u = error*this->pTerm;
1732 
1733  if(u > 0.0)
1734  {
1735  if(u > limit)
1736  {
1737  u = limit;
1738  }
1739  }
1740  else if(u < 0.0)
1741  {
1742  if(u < -limit)
1743  {
1744  u = -limit;
1745  }
1746  }
1747 
1748  integral += error*this->iTerm;
1749 
1750  if(integral > 10000.0)
1751  {
1752  integral = 10000.0;
1753  }
1754  else if(integral < -10000.0)
1755  {
1756  integral = -10000.0;
1757  }
1758 
1759  if(error > -10 && error < 10)
1760  {
1761  if(!integralReset)
1762  {
1763  integralReset = 1;
1764  integral = 0;
1765  }
1766  }
1767  else
1768  {
1769  integralReset = 0;
1770  }
1771 
1772  u += integral;
1773  u *= this->stepsPerSecondToRPM;
1774 
1775  this->driver.setVelocity(u);
1776 }
1777 
1779 {
1780  static float oldTargetPosition;
1781  static float oldEncoderPosition;
1782  static float encoderPositionChange;
1783  static float targetPositionChange;
1784  float encoderPosition = ((float)this->encoder.angleMoved*this->stepConversion);
1785  static float internalStall = 0.0;
1786 
1787  encoderPositionChange *= 0.99;
1788  encoderPositionChange += 0.01*(oldEncoderPosition - encoderPosition);
1789  oldEncoderPosition = encoderPosition;
1790 
1791  targetPositionChange *= 0.99;
1792  targetPositionChange += 0.01*(oldTargetPosition - this->pidTargetPosition);
1793  oldTargetPosition = this->pidTargetPosition;
1794 
1795  if(abs(encoderPositionChange) < abs(targetPositionChange)*0.5)
1796  {
1797  internalStall *= this->stallSensitivity;
1798  internalStall += 1.0-this->stallSensitivity;
1799  }
1800  else
1801  {
1802  internalStall *= this->stallSensitivity;
1803  }
1804 
1805  if(internalStall >= 0.95) //3 timeconstants
1806  {
1807  this->stall = 1;
1808  }
1809  else
1810  {
1811  this->stall = 0;
1812  }
1813 }
1814 
1815 bool uStepperSLite::isStalled(float stallSensitivity)
1816 {
1817  if(this->stallSensitivity > 1.0)
1818  {
1819  this->stallSensitivity = 1.0;
1820  }
1821  else if(this->stallSensitivity < 0.0)
1822  {
1823  this->stallSensitivity = 0.0;
1824  }
1825  else{
1826  this->stallSensitivity = stallSensitivity;
1827  }
1828 
1829  return this->stall;
1830 }
1831 
1833 {
1834  this->pTerm = P;
1835 }
1836 
1838 {
1839  this->iTerm = I * ENCODERINTSAMPLETIME;
1840 }
1841 
1843 {
1844  this->dTerm = D * ENCODERINTFREQ;
1845 }
1846 
1848 {
1849  this->invertPidDropinDirection = invert;
1850 }
1851 
1853 {
1854  uint8_t i = 0;
1855  String value;
1856 
1857  if(cmd->charAt(2) == ';')
1858  {
1859  Serial.println("COMMAND NOT ACCEPTED");
1860  return;
1861  }
1862 
1863  value.remove(0);
1864  /****************** SET P Parameter ***************************
1865  * *
1866  * *
1867  **************************************************************/
1868  if(cmd->substring(0,2) == String("P="))
1869  {
1870  for(i = 2;;i++)
1871  {
1872  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1873  {
1874  value.concat(cmd->charAt(i));
1875  }
1876  else if(cmd->charAt(i) == '.')
1877  {
1878  value.concat(cmd->charAt(i));
1879  i++;
1880  break;
1881  }
1882  else if(cmd->charAt(i) == ';')
1883  {
1884  break;
1885  }
1886  else
1887  {
1888  Serial.println("COMMAND NOT ACCEPTED");
1889  return;
1890  }
1891  }
1892 
1893  for(;;i++)
1894  {
1895  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1896  {
1897  value.concat(cmd->charAt(i));
1898  }
1899  else if(cmd->charAt(i) == ';')
1900  {
1901  Serial.print("COMMAND ACCEPTED. P = ");
1902  Serial.println(value.toFloat(),4);
1903  this->dropinSettings.P.f = value.toFloat();
1904  this->saveDropinSettings();
1905  this->setProportional(value.toFloat());
1906  return;
1907  }
1908  else
1909  {
1910  Serial.println("COMMAND NOT ACCEPTED");
1911  return;
1912  }
1913  }
1914  }
1915 
1916 /****************** SET I Parameter ***************************
1917  * *
1918  * *
1919  **************************************************************/
1920  else if(cmd->substring(0,2) == String("I="))
1921  {
1922  for(i = 2;;i++)
1923  {
1924  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1925  {
1926  value.concat(cmd->charAt(i));
1927  }
1928  else if(cmd->charAt(i) == '.')
1929  {
1930  value.concat(cmd->charAt(i));
1931  i++;
1932  break;
1933  }
1934  else if(cmd->charAt(i) == ';')
1935  {
1936  break;
1937  }
1938  else
1939  {
1940  Serial.println("COMMAND NOT ACCEPTED");
1941  return;
1942  }
1943  }
1944 
1945  for(;;i++)
1946  {
1947  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1948  {
1949  value.concat(cmd->charAt(i));
1950  }
1951  else if(cmd->charAt(i) == ';')
1952  {
1953  Serial.print("COMMAND ACCEPTED. I = ");
1954  Serial.println(value.toFloat(),4);
1955 
1956  this->dropinSettings.I.f = value.toFloat();
1957  this->saveDropinSettings();
1958  this->setIntegral(value.toFloat());
1959  return;
1960  }
1961  else
1962  {
1963  Serial.println("COMMAND NOT ACCEPTED");
1964  return;
1965  }
1966  }
1967  }
1968 
1969 /****************** SET D Parameter ***************************
1970  * *
1971  * *
1972  **************************************************************/
1973  else if(cmd->substring(0,2) == String("D="))
1974  {
1975  for(i = 2;;i++)
1976  {
1977  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1978  {
1979  value.concat(cmd->charAt(i));
1980  }
1981  else if(cmd->charAt(i) == '.')
1982  {
1983  value.concat(cmd->charAt(i));
1984  i++;
1985  break;
1986  }
1987  else if(cmd->charAt(i) == ';')
1988  {
1989  break;
1990  }
1991  else
1992  {
1993  Serial.println("COMMAND NOT ACCEPTED");
1994  return;
1995  }
1996  }
1997 
1998  for(;;i++)
1999  {
2000  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
2001  {
2002  value.concat(cmd->charAt(i));
2003  }
2004  else if(cmd->charAt(i) == ';')
2005  {
2006  Serial.print("COMMAND ACCEPTED. D = ");
2007  Serial.println(value.toFloat(),4);
2008  this->dropinSettings.D.f = value.toFloat();
2009  this->saveDropinSettings();
2010  this->setDifferential(value.toFloat());
2011  return;
2012  }
2013  else
2014  {
2015  Serial.println("COMMAND NOT ACCEPTED");
2016  return;
2017  }
2018  }
2019  }
2020 
2021 /****************** invert Direction ***************************
2022  * *
2023  * *
2024  **************************************************************/
2025  else if(cmd->substring(0,6) == String("invert"))
2026  {
2027  if(cmd->charAt(6) != ';')
2028  {
2029  Serial.println("COMMAND NOT ACCEPTED");
2030  return;
2031  }
2032  if(this->invertPidDropinDirection)
2033  {
2034  Serial.println(F("Direction normal!"));
2035  this->dropinSettings.invert = 0;
2036  this->saveDropinSettings();
2037  this->invertDropinDir(0);
2038  return;
2039  }
2040  else
2041  {
2042  Serial.println(F("Direction inverted!"));
2043  this->dropinSettings.invert = 1;
2044  this->saveDropinSettings();
2045  this->invertDropinDir(1);
2046  return;
2047  }
2048  }
2049 
2050  /****************** get Current Pid Error ********************
2051  * *
2052  * *
2053  **************************************************************/
2054  else if(cmd->substring(0,5) == String("error"))
2055  {
2056  if(cmd->charAt(5) != ';')
2057  {
2058  Serial.println("COMMAND NOT ACCEPTED");
2059  return;
2060  }
2061  Serial.print(F("Current Error: "));
2062  Serial.print(this->getPidError());
2063  Serial.println(F(" Steps"));
2064  }
2065 
2066  /****************** Get run/hold current settings ************
2067  * *
2068  * *
2069  **************************************************************/
2070  else if(cmd->substring(0,7) == String("current"))
2071  {
2072  if(cmd->charAt(7) != ';')
2073  {
2074  Serial.println("COMMAND NOT ACCEPTED");
2075  return;
2076  }
2077  Serial.print(F("Run Current: "));
2078  Serial.print(this->driver.getRunCurrent());
2079  Serial.println(F(" %"));
2080  Serial.print(F("Hold Current: "));
2081  Serial.print(this->driver.getHoldCurrent());
2082  Serial.println(F(" %"));
2083  }
2084 
2085  /****************** Get PID Parameters ***********************
2086  * *
2087  * *
2088  **************************************************************/
2089  else if(cmd->substring(0,10) == String("parameters"))
2090  {
2091  if(cmd->charAt(10) != ';')
2092  {
2093  Serial.println("COMMAND NOT ACCEPTED");
2094  return;
2095  }
2096  Serial.print(F("P: "));
2097  Serial.print(this->dropinSettings.P.f,4);
2098  Serial.print(F(", "));
2099  Serial.print(F("I: "));
2100  Serial.print(this->dropinSettings.I.f,4);
2101  Serial.print(F(", "));
2102  Serial.print(F("D: "));
2103  Serial.println(this->dropinSettings.D.f,4);
2104  }
2105 
2106  /****************** Help menu ********************************
2107  * *
2108  * *
2109  **************************************************************/
2110  else if(cmd->substring(0,4) == String("help"))
2111  {
2112  if(cmd->charAt(4) != ';')
2113  {
2114  Serial.println("COMMAND NOT ACCEPTED");
2115  return;
2116  }
2117  this->dropinPrintHelp();
2118  }
2119 
2120 /****************** SET run current ***************************
2121  * *
2122  * *
2123  **************************************************************/
2124  else if(cmd->substring(0,11) == String("runCurrent="))
2125  {
2126  for(i = 11;;i++)
2127  {
2128  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
2129  {
2130  value.concat(cmd->charAt(i));
2131  }
2132  else if(cmd->charAt(i) == '.')
2133  {
2134  value.concat(cmd->charAt(i));
2135  i++;
2136  break;
2137  }
2138  else if(cmd->charAt(i) == ';')
2139  {
2140  break;
2141  }
2142  else
2143  {
2144  Serial.println("COMMAND NOT ACCEPTED");
2145  return;
2146  }
2147  }
2148 
2149  for(;;i++)
2150  {
2151  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
2152  {
2153  value.concat(cmd->charAt(i));
2154  }
2155  else if(cmd->charAt(i) == ';')
2156  {
2157  if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
2158  {
2159  i = (uint8_t)value.toFloat();
2160  break;
2161  }
2162  else
2163  {
2164  Serial.println("COMMAND NOT ACCEPTED");
2165  return;
2166  }
2167  }
2168  else
2169  {
2170  Serial.println("COMMAND NOT ACCEPTED");
2171  return;
2172  }
2173  }
2174 
2175  Serial.print("COMMAND ACCEPTED. runCurrent = ");
2176  Serial.print(i);
2177  Serial.println(F(" %"));
2178  this->dropinSettings.runCurrent = i;
2179  this->saveDropinSettings();
2180  this->driver.setRunCurrent(i);
2181  }
2182 
2183  /****************** SET run current ***************************
2184  * *
2185  * *
2186  **************************************************************/
2187  else if(cmd->substring(0,12) == String("holdCurrent="))
2188  {
2189  for(i = 12;;i++)
2190  {
2191  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
2192  {
2193  value.concat(cmd->charAt(i));
2194  }
2195  else if(cmd->charAt(i) == '.')
2196  {
2197  value.concat(cmd->charAt(i));
2198  i++;
2199  break;
2200  }
2201  else if(cmd->charAt(i) == ';')
2202  {
2203  break;
2204  }
2205  else
2206  {
2207  Serial.println(F("COMMAND NOT ACCEPTED"));
2208  return;
2209  }
2210  }
2211 
2212  for(;;i++)
2213  {
2214  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
2215  {
2216  value.concat(cmd->charAt(i));
2217  }
2218  else if(cmd->charAt(i) == ';')
2219  {
2220  if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
2221  {
2222  i = (uint8_t)value.toFloat();
2223  break;
2224  }
2225  else
2226  {
2227  Serial.println(F("COMMAND NOT ACCEPTED"));
2228  return;
2229  }
2230  }
2231  else
2232  {
2233  Serial.println(F("COMMAND NOT ACCEPTED"));
2234  return;
2235  }
2236  }
2237 
2238  Serial.print(F("COMMAND ACCEPTED. holdCurrent = "));
2239  Serial.print(i);
2240  Serial.println(F(" %"));
2241  this->dropinSettings.holdCurrent = i;
2242  this->saveDropinSettings();
2243  this->driver.setHoldCurrent(i);
2244  }
2245 
2246  /****************** DEFAULT (Reject!) ************************
2247  * *
2248  * *
2249  **************************************************************/
2250  else
2251  {
2252  Serial.println(F("COMMAND NOT ACCEPTED"));
2253  return;
2254  }
2255 
2256 }
2257 
2259 {
2260  static String stringInput;
2261  static uint32_t t = millis();
2262 
2263  while(1)
2264  {
2265  while(!Serial.available())
2266  {
2267  delay(1);
2268  if((millis() - t) >= 500)
2269  {
2270  stringInput.remove(0);
2271  t = millis();
2272  }
2273  }
2274  t = millis();
2275  stringInput += (char)Serial.read();
2276  if(stringInput.lastIndexOf(';') > -1)
2277  {
2278  this->parseCommand(&stringInput);
2279  stringInput.remove(0);
2280  }
2281  }
2282 }
2283 
2285 {
2286  Serial.println(F("uStepper S-lite Dropin !"));
2287  Serial.println(F(""));
2288  Serial.println(F("Usage:"));
2289  Serial.println(F("Show this command list: 'help;'"));
2290  Serial.println(F("Get PID Parameters: 'parameters;'"));
2291  Serial.println(F("Set Proportional constant: 'P=10.002;'"));
2292  Serial.println(F("Set Integral constant: 'I=10.002;'"));
2293  Serial.println(F("Set Differential constant: 'D=10.002;'"));
2294  Serial.println(F("Invert Direction: 'invert;'"));
2295  Serial.println(F("Get Current PID Error: 'error;'"));
2296  Serial.println(F("Get Run/Hold Current Settings: 'current;'"));
2297  Serial.println(F("Set Run Current (percent): 'runCurrent=50.0;'"));
2298  Serial.println(F("Set Hold Current (percent): 'holdCurrent=50.0;'"));
2299  Serial.println(F(""));
2300  Serial.println(F(""));
2301 }
2302 
2304 {
2305  dropinCliSettings_t tempSettings;
2306 
2307  EEPROM.get(0,tempSettings);
2308 
2309  if(this->dropinSettingsCalcChecksum(&tempSettings) != tempSettings.checksum)
2310  {
2311  return 0;
2312  }
2313 
2314  this->dropinSettings = tempSettings;
2315  this->setProportional(this->dropinSettings.P.f);
2316  this->setIntegral(this->dropinSettings.I.f);
2317  this->setDifferential(this->dropinSettings.D.f);
2318  this->invertDropinDir((bool)this->dropinSettings.invert);
2319  this->setCurrent(this->dropinSettings.runCurrent,this->dropinSettings.holdCurrent);
2320  return 1;
2321 }
2322 
2324 {
2326 
2327  EEPROM.put(0,this->dropinSettings);
2328 }
2329 
2331 {
2332  uint8_t i;
2333  uint8_t checksum = 0xAA;
2334  uint8_t *p = (uint8_t*)settings;
2335 
2336  for(i=0; i < 15; i++)
2337  {
2338  checksum ^= *p++;
2339  }
2340 
2341  return checksum;
2342 }
uStepperSLite::currentPidAcceleration
volatile float currentPidAcceleration
Definition: uStepperSLite.h:554
I2CFREE
#define I2CFREE
Definition: i2cMaster.h:43
uStepperSLite::velocity
float velocity
Definition: uStepperSLite.h:491
dropinCliSettings_t::P
floatBytes_t P
Definition: uStepperSLite.h:156
uStepperSLite::stepsPerSecondToRPM
float stepsPerSecondToRPM
Definition: uStepperSLite.h:510
uStepperEncoder::angle
volatile uint16_t angle
Definition: uStepperSLite.h:320
uStepperSLite
Prototype of class for accessing all features of the uStepper S-lite in a single object.
Definition: uStepperSLite.h:438
Tmc2208::invertDirection
void invertDirection(bool normal=INVERSEDIRECTION)
Invert motor direction.
Definition: TMC2208.cpp:133
uStepperSLite::encoder
uStepperEncoder encoder
Definition: uStepperSLite.h:630
uStepperSLite::acceleration
float acceleration
Definition: uStepperSLite.h:499
Tmc2208::setCurrent
void setCurrent(uint8_t runPercent, uint8_t holdPercent)
Change run and hold current settings of the stepper motor driver - TMC2208.
Definition: TMC2208.cpp:196
uStepperSLite::cruiseToDecelThreshold
volatile int32_t cruiseToDecelThreshold
Definition: uStepperSLite.h:548
uStepperSLite::RPMToStepsPerSecond
float RPMToStepsPerSecond
Definition: uStepperSLite.h:513
uStepperSLite::isStalled
bool isStalled(float stallSensitivity=0.992)
This method returns a bool variable indicating wether the motor is stalled or not.
Definition: uStepperSLite.cpp:1815
uStepperSLite::runContinous
void runContinous(bool dir)
Make the motor rotate continuously.
Definition: uStepperSLite.cpp:730
uStepperSLite::invertDropinDir
void invertDropinDir(bool invert)
This method is used to invert the drop-in direction pin interpretation.
Definition: uStepperSLite.cpp:1847
uStepperSLite::accelToCruiseThreshold
volatile int32_t accelToCruiseThreshold
Definition: uStepperSLite.h:545
uStepperEncoder::angleMoved
volatile int32_t angleMoved
Definition: uStepperSLite.h:308
Tmc2208::setVelocity
void setVelocity(float RPM)
Set motor velocity in RPM.
Definition: TMC2208.cpp:228
i2cMaster::read
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: i2cMaster.cpp:61
dropinCliSettings_t
Struct to store dropin settings.
Definition: uStepperSLite.h:155
uStepperEncoder::oldAngle
volatile uint16_t oldAngle
Definition: uStepperSLite.h:316
uStepperSLite::invertPidDropinDirection
bool invertPidDropinDirection
Definition: uStepperSLite.h:638
uStepperSLite::stepConversion
volatile float stepConversion
Definition: uStepperSLite.h:503
Tmc2208::setup
void setup(void)
Initializes the different parts of the TMC2208 object.
Definition: TMC2208.cpp:114
uStepperSLite::dropinPrintHelp
void dropinPrintHelp()
This method is used to print the dropinCli menu explainer:
Definition: uStepperSLite.cpp:2284
uStepperSLite::enableMotor
void enableMotor(void)
Enables the stepper driver output stage.
Definition: uStepperSLite.cpp:1431
uStepperSLite::stallSensitivity
float stallSensitivity
Definition: uStepperSLite.h:522
uStepperSLite::pTerm
float pTerm
Definition: uStepperSLite.h:507
dropinCliSettings_t::D
floatBytes_t D
Definition: uStepperSLite.h:158
uStepperSLite::targetPosition
volatile int32_t targetPosition
Definition: uStepperSLite.h:560
i2cMaster::begin
void begin(void)
Setup TWI (I2C) interface.
Definition: i2cMaster.cpp:221
uStepperSLite::pidError
volatile uint8_t pidError
Definition: uStepperSLite.h:468
uStepperSLite::stepDelay
volatile uint32_t stepDelay
Definition: uStepperSLite.h:453
uStepperSLite::stepGeneratorDirection
volatile uint8_t stepGeneratorDirection
Definition: uStepperSLite.h:456
uStepperSLite::continous
bool continous
Definition: uStepperSLite.h:465
Tmc2208::setHoldCurrent
void setHoldCurrent(uint8_t holdPercent)
Change hold current setting of the stepper motor driver - TMC2208.
Definition: TMC2208.cpp:215
uStepperEncoder::getAngle
float getAngle(void)
Measure the current shaft angle.
Definition: uStepperSLite.cpp:615
uStepperSLite::dropinSettingsCalcChecksum
uint8_t dropinSettingsCalcChecksum(dropinCliSettings_t *settings)
This method is used internally for stall detection.
Definition: uStepperSLite.cpp:2330
uStepperSLite::RPMToStepDelay
volatile float RPMToStepDelay
Definition: uStepperSLite.h:538
i2cMaster::getStatus
uint8_t getStatus(void)
Get current I2C status.
Definition: i2cMaster.cpp:216
uStepperSLite::setDifferential
void setDifferential(float D)
This method is used to change the PID differential parameter D.
Definition: uStepperSLite.cpp:1842
uStepperSLite::driver
Tmc2208 driver
Definition: uStepperSLite.h:633
uStepperSLite::brake
bool brake
Definition: uStepperSLite.h:566
dropinCliSettings_t::checksum
uint8_t checksum
Definition: uStepperSLite.h:162
uStepperSLite::enablePid
void enablePid(void)
This method enables the PID after being disabled (disablePid).
Definition: uStepperSLite.cpp:1704
uStepperSLite::setMaxAcceleration
void setMaxAcceleration(float accel)
Set the maximum acceleration of the stepper motor.
Definition: uStepperSLite.cpp:680
uStepperSLite::moveAngle
void moveAngle(float angle, bool holdMode=BRAKEON)
Moves the motor to a relative angle.
Definition: uStepperSLite.cpp:1534
uStepperSLite::currentPidSpeed
volatile float currentPidSpeed
Definition: uStepperSLite.h:551
uStepperSLite::getPidError
float getPidError(void)
This method returns the current PID error.
Definition: uStepperSLite.cpp:1715
Tmc2208::disableDriver
void disableDriver(void)
Disable the stepper motor driver - TMC2208.
Definition: TMC2208.cpp:155
uStepperEncoder::getAngleMoved
float getAngleMoved(void)
Measure the angle moved from reference position.
Definition: uStepperSLite.cpp:570
uStepperEncoder::getSpeed
float getSpeed(bool unit=SPS)
Measure the current speed of the motor.
Definition: uStepperSLite.cpp:575
floatBytes_t::f
float f
Definition: uStepperSLite.h:142
uStepperSLite::decelToAccelThreshold
volatile int32_t decelToAccelThreshold
Definition: uStepperSLite.h:542
dropinCliSettings_t::invert
uint8_t invert
Definition: uStepperSLite.h:159
Tmc2208::enableDriver
void enableDriver(void)
Enable the stepper motor driver - TMC2208.
Definition: TMC2208.cpp:150
uStepperSLite::detectStall
bool detectStall(void)
This method is used internally for stall detection.
Definition: uStepperSLite.cpp:1778
uStepperSLite::stop
void stop(bool brake=BRAKEON)
Stop the motor with deceleration.
Definition: uStepperSLite.cpp:1168
uStepperSLite::getMotorState
uint8_t getMotorState(void)
Get the current state of the motor.
Definition: uStepperSLite.cpp:1446
uStepperSLite::getCurrentDirection
bool getCurrentDirection(void)
Returns the direction the motor is currently configured to rotate.
Definition: uStepperSLite.cpp:1441
uStepperSLite::state
volatile uint8_t state
Definition: uStepperSLite.h:473
uStepperSLite::hardStop
void hardStop(bool holdMode=BRAKEON)
Stop the motor without deceleration.
Definition: uStepperSLite.cpp:1123
dropinCliSettings_t::I
floatBytes_t I
Definition: uStepperSLite.h:157
uStepperSLite::angleToStep
float angleToStep
Definition: uStepperSLite.h:526
uStepperEncoder::curSpeed
volatile float curSpeed
Definition: uStepperSLite.h:324
uStepperSLite::moveToEnd
float moveToEnd(bool dir, float stallSensitivity=0.992)
Moves the motor to its physical limit, without limit switch.
Definition: uStepperSLite.cpp:1477
uStepperEncoder::detectMagnet
uint8_t detectMagnet(void)
Detect if magnet is present and within range.
Definition: uStepperSLite.cpp:640
uStepperSLite::mode
uint8_t mode
Definition: uStepperSLite.h:477
dropinCliSettings_t::holdCurrent
uint8_t holdCurrent
Definition: uStepperSLite.h:160
Tmc2208::setRunCurrent
void setRunCurrent(uint8_t runPercent)
Change run current setting of the stepper motor driver - TMC2208.
Definition: TMC2208.cpp:202
uStepperEncoder::uStepperEncoder
uStepperEncoder(void)
Constructor.
Definition: uStepperSLite.cpp:565
uStepperSLite::iTerm
float iTerm
Definition: uStepperSLite.h:516
uStepperSLite::setIntegral
void setIntegral(float I)
This method is used to change the PID integral parameter I.
Definition: uStepperSLite.cpp:1837
uStepperSLite::checkConnectorOrientation
void checkConnectorOrientation(uint8_t mode)
This method handles the connector orientation check in order to automatically compensate the PID for ...
Definition: uStepperSLite.cpp:1242
uStepperSLite::direction
volatile uint8_t direction
Definition: uStepperSLite.h:486
dropinCliSettings_t::runCurrent
uint8_t runCurrent
Definition: uStepperSLite.h:161
uStepperSLite::stepCnt
volatile int32_t stepCnt
Definition: uStepperSLite.h:483
uStepperSLite::currentPidError
volatile float currentPidError
Definition: uStepperSLite.h:622
uStepperSLite::setProportional
void setProportional(float P)
This method is used to change the PID proportional parameter P.
Definition: uStepperSLite.cpp:1832
uStepperEncoder::setup
void setup(void)
Setup the encoder.
Definition: uStepperSLite.cpp:587
i2cMaster
Prototype of class for accessing the TWI (I2C) interface of the AVR (master mode only).
Definition: i2cMaster.h:94
uStepperSLite::loadDropinSettings
bool loadDropinSettings(void)
This method loads the dropin settings stored in EEPROM.
Definition: uStepperSLite.cpp:2303
uStepperSLite::getStepsSinceReset
int32_t getStepsSinceReset(void)
Get the number of steps applied since reset.
Definition: uStepperSLite.cpp:1456
uStepperSLite::pidDropin
void pidDropin(float error)
This method handles the actual PID Drop-in controller calculations, if enabled.
Definition: uStepperSLite.cpp:1720
uStepperSLite::setHoldCurrent
void setHoldCurrent(uint8_t holdCurrent)
Set motor hold current.
Definition: uStepperSLite.cpp:1466
uStepperEncoder::setHome
void setHome(void)
Define new reference(home) position.
Definition: uStepperSLite.cpp:597
uStepperSLite::pid
void pid(float error)
This method handles the actual PID controller calculations, if enabled.
Definition: uStepperSLite.cpp:1550
uStepperSLite::moveSteps
void moveSteps(int32_t steps, bool dir, bool holdMode=BRAKEON)
Make the motor perform a predefined number of steps.
Definition: uStepperSLite.cpp:884
uStepperSLite::decelToStopThreshold
volatile int32_t decelToStopThreshold
Definition: uStepperSLite.h:460
uStepperSLite::setRunCurrent
void setRunCurrent(uint8_t runCurrent)
Set motor run current.
Definition: uStepperSLite.cpp:1471
uStepperSLite::dropinSettings
dropinCliSettings_t dropinSettings
Definition: uStepperSLite.h:1039
uStepperSLite::dTerm
float dTerm
Definition: uStepperSLite.h:519
uStepperEncoder::getAgc
uint8_t getAgc(void)
Read the current AGC value of the encoder chip.
Definition: uStepperSLite.cpp:631
uStepperEncoder::getStrength
uint16_t getStrength(void)
Measure the strength of the magnet.
Definition: uStepperSLite.cpp:620
uStepperSLite::stepsSinceReset
volatile int32_t stepsSinceReset
Definition: uStepperSLite.h:446
uStepperEncoder::encoderOffset
uint16_t encoderOffset
Definition: uStepperSLite.h:311
uStepperServo::angle
uint8_t angle
Definition: uStepperServo.h:50
uStepperSLite::setMaxVelocity
void setMaxVelocity(float vel)
Sets the maximum rotational velocity of the motor.
Definition: uStepperSLite.cpp:697
uStepperSLite::setup
void setup(uint8_t mode=NORMAL, float stepsPerRevolution=3200.0, float pTerm=0.75, float iTerm=3.0, float dTerm=0.0, bool setHome=true, uint8_t invert=0, uint8_t runCurrent=50, uint8_t holdCurrent=30)
Initializes the different parts of the uStepper S-lite object.
Definition: uStepperSLite.cpp:1313
uStepperSLite::uStepperSLite
uStepperSLite(float accel=1000.0, float vel=1000.0)
Constructor of uStepper S-lite class.
Definition: uStepperSLite.cpp:666
uStepperSLite::parseCommand
void parseCommand(String *cmd)
This method is used for the dropinCli to take in user commands.
Definition: uStepperSLite.cpp:1852
uStepperSLite::softStop
void softStop(bool holdMode=BRAKEON)
Stop the motor with deceleration.
Definition: uStepperSLite.cpp:1173
uStepperSLite::stall
volatile bool stall
Definition: uStepperSLite.h:534
uStepperSLite::disablePid
void disablePid(void)
This method disables the PID until calling enablePid.
Definition: uStepperSLite.cpp:1699
uStepperSLite::moveToAngle
void moveToAngle(float angle, bool holdMode=BRAKEON)
Moves the motor to an absolute angle.
Definition: uStepperSLite.cpp:1516
uStepperSLite::saveDropinSettings
void saveDropinSettings(void)
This method stores the current dropin settings in EEPROM.
Definition: uStepperSLite.cpp:2323
uStepperSLite::dropinCli
void dropinCli()
This method is used to tune Drop-in parameters. After tuning uStepper S-lite the parameters are saved...
Definition: uStepperSLite.cpp:2258
uStepperSLite::stepToAngle
float stepToAngle
Definition: uStepperSLite.h:530
uStepperSLite::setCurrent
void setCurrent(uint8_t runCurrent, uint8_t holdCurrent=25)
Set motor run and hold current.
Definition: uStepperSLite.cpp:1461
uStepperSLite::disableMotor
void disableMotor(void)
Disables the stepper driver output stage.
Definition: uStepperSLite.cpp:1436
uStepperSLite::pidTargetPosition
volatile float pidTargetPosition
Definition: uStepperSLite.h:627
I2C
i2cMaster I2C(1)
uStepperSLite::pidDisabled
volatile bool pidDisabled
Definition: uStepperSLite.h:563