uStepper S
uStepperS.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepperS.cpp *
3 * Version: 2.2.0 *
4 * Date: September 22nd, 2020 *
5 * Authors: Thomas Hørring Olsen *
6 * Emil Jacobsen *
7 * *
8 *********************************************************************************************
9 * (C) 2020 *
10 * *
11 * uStepper ApS *
12 * www.ustepper.com *
13 * administration@ustepper.com *
14 * *
15 * The code contained in this file is released under the following open source license: *
16 * *
17 * Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International *
18 * *
19 * The code in this file is provided without warranty of any kind - use at own risk! *
20 * neither uStepper ApS nor the author, can be held responsible for any damage *
21 * caused by the use of the code contained in this file ! *
22 * *
23 ********************************************************************************************/
33 #include <uStepperS.h>
35 
37 {
38  pointer = this;
39 
40  this->microSteps = 256;
41  this->init();
42 
43  this->setMaxAcceleration(2000.0);
44  this->setMaxDeceleration(2000.0);
45  this->setMaxVelocity(100.0);
46 }
47 
48 uStepperS::uStepperS(float acceleration, float velocity)
49 {
50  pointer = this;
51  this->microSteps = 256;
52  this->init();
53 
54  this->setMaxAcceleration(acceleration);
55  this->setMaxVelocity(velocity);
56 }
57 
58 
59 void uStepperS::init( void ){
60 
61 
62  this->pidDisabled = 1;
63  /* Set CS, MOSI, SCK and DRV_ENN as Output */
64  DDRC = (1<<SCK1)|(1<<MOSI_ENC);
65  DDRD = (1<<DRV_ENN)|(1<<SD_MODE)|(1<<CS_ENCODER);
66  DDRE = (1<<MOSI1)|(1<<CS_DRIVER);
67 
68  PORTD |= (1 << DRV_ENN); // Set DRV_ENN HIGH, while configuring
69  PORTD &= ~(1 << SD_MODE); // Set SD_MODE LOW
70 
71  /*
72  * ---- Global SPI1 configuration ----
73  * SPE = 1: SPI enabled
74  * MSTR = 1: Master
75  * SPR0 = 0 & SPR1 = 0: fOSC/4 = 4Mhz
76  */
77  SPCR1 = (1<<SPE1)|(1<<MSTR1);
78 
79  driver.init( this );
80  encoder.init( this );
81  PORTD &= ~(1 << DRV_ENN); // Set DRV_ENN LOW
82 }
83 
84 bool uStepperS::getMotorState(uint8_t statusType)
85 {
86  this->driver.readMotorStatus();
87  if(this->driver.status & statusType)
88  {
89  return 0;
90  }
91  return 1;
92 }
93 
95 {
96  int32_t velocity = this->driver.getVelocity();
97 
98  return (float)velocity * this->velToRpm;
99 }
100 
101 void uStepperS::checkOrientation(float distance)
102 {
103  float startAngle;
104  uint8_t inverted = 0;
105  uint8_t noninverted = 0;
106  this->disablePid();
107  this->shaftDir = 0;
108  this->driver.setShaftDirection(this->shaftDir);
109 
110  startAngle = this->encoder.getAngleMoved();
111  this->moveAngle(distance);
112 
113  while(this->getMotorState());
114 
115  startAngle -= distance/2.0;
116  if(this->encoder.getAngleMoved() < startAngle)
117  {
118  inverted++;
119  }
120  else
121  {
122  noninverted++;
123  }
124 
125  startAngle = this->encoder.getAngleMoved();
126  this->moveAngle(-distance);
127 
128  while(this->getMotorState());
129 
130  startAngle += distance/2.0;
131  if(this->encoder.getAngleMoved() > startAngle)
132  {
133  inverted++;
134  }
135  else
136  {
137  noninverted++;
138  }
139 
140  startAngle = this->encoder.getAngleMoved();
141  this->moveAngle(distance);
142 
143  while(this->getMotorState());
144 
145  startAngle -= distance/2.0;
146  if(this->encoder.getAngleMoved() < startAngle)
147  {
148  inverted++;
149  }
150  else
151  {
152  noninverted++;
153  }
154 
155  this->moveAngle(-distance);
156 
157  while(this->getMotorState());
158 
159  if(inverted > noninverted)
160  {
161  this->shaftDir = 1;
162  this->driver.setShaftDirection(this->shaftDir);
163  }
164  this->enablePid();
165 }
166 
167 void uStepperS::setup( uint8_t mode,
168  uint16_t stepsPerRevolution,
169  float pTerm,
170  float iTerm,
171  float dTerm,
172  uint16_t dropinStepSize,
173  bool setHome,
174  uint8_t invert,
175  uint8_t runCurrent,
176  uint8_t holdCurrent)
177 {
178  dropinCliSettings_t tempSettings;
179  this->pidDisabled = 1;
180  // Should setup mode etc. later
181  this->mode = mode;
182  this->fullSteps = stepsPerRevolution;
183  this->dropinStepSize = 256/dropinStepSize;
184  this->angleToStep = (float)this->fullSteps * (float)this->microSteps / 360.0;
185  this->rpmToVelocity = (float)(279620.267 * fullSteps * microSteps)/(CLOCKFREQ);
186  this->stepsPerSecondToRPM = 60.0/(this->microSteps*this->fullSteps);
187  this->RPMToStepsPerSecond = (this->microSteps*this->fullSteps)/60.0;
188 
189 
190  this->stepTime = 16777216.0/CLOCKFREQ; // 2^24/CLOCKFREQ
191  this->rpmToVel = (this->fullSteps*this->microSteps)/(60.0/this->stepTime);
192  this->velToRpm = 1.0/this->rpmToVel;
193 
194  this->init();
195 
196  this->driver.setDeceleration( (uint32_t)( this->maxDeceleration ) );
197  this->driver.setAcceleration( (uint32_t)(this->maxAcceleration ) );
198 
199  this->setCurrent(0.0);
200  this->setHoldCurrent(0.0);
201 
202  this->setCurrent(40.0);
203  this->setHoldCurrent(0.0);
204 
205  this->encoder.Beta=5;
206  if(this->mode)
207  {
208  if(this->mode == DROPIN)
209  {
210  //Set Enable, Step and Dir signal pins from 3dPrinter controller as inputs
211  this->encoder.Beta = 2;
212  pinMode(2,INPUT);
213  pinMode(3,INPUT);
214  pinMode(4,INPUT);
215  //Enable internal pull-up resistors on the above pins
216  digitalWrite(2,HIGH);
217  digitalWrite(3,HIGH);
218  digitalWrite(4,HIGH);
219  delay(10000);
220  attachInterrupt(0, interrupt0, FALLING);
221  attachInterrupt(1, interrupt1, CHANGE);
222  this->driver.setDeceleration( 0xFFFE );
223  this->driver.setAcceleration( 0xFFFE );
224  Serial.begin(9600);
225 
226  tempSettings.P.f = pTerm;
227  tempSettings.I.f = iTerm;
228  tempSettings.D.f = dTerm;
229  tempSettings.invert = invert;
230  tempSettings.runCurrent = runCurrent;
231  tempSettings.holdCurrent = holdCurrent;
232  tempSettings.checksum = this->dropinSettingsCalcChecksum(&tempSettings);
233 
234  if(tempSettings.checksum != EEPROM.read(sizeof(dropinCliSettings_t)))
235  {
236  this->dropinSettings = tempSettings;
237  this->saveDropinSettings();
238  EEPROM.put(sizeof(dropinCliSettings_t),this->dropinSettings.checksum);
239  this->loadDropinSettings();
240  }
241  else
242  {
243  if(!this->loadDropinSettings())
244  {
245  this->dropinSettings = tempSettings;
246  this->saveDropinSettings();
247  EEPROM.put(sizeof(dropinCliSettings_t),this->dropinSettings.checksum);
248  this->loadDropinSettings();
249  }
250  }
251 
252 
253  this->dropinPrintHelp();
254  }
255  else
256  {
257  this->encoder.Beta = 4;
258  }
259  }
260 
261  if(setHome == true){
262  encoder.setHome();
263  }
264 
265  this->pidDisabled = 0;
266 
267  DDRB |= (1 << 4);
268 }
269 
270 void uStepperS::moveSteps( int32_t steps )
271 {
272  this->driver.setDeceleration( (uint16_t)( this->maxDeceleration ) );
273  this->driver.setAcceleration( (uint16_t)(this->maxAcceleration ) );
274  this->driver.setVelocity( (uint32_t)( this->maxVelocity ) );
275 
276  // Get current position
277  int32_t current = this->driver.getPosition();
278 
279  // Set new position
280  this->driver.setPosition( current + steps);
281 }
282 
283 
284 
285 void uStepperS::moveAngle( float angle )
286 {
287  int32_t steps;
288 
289  if(angle < 0.0)
290  {
291  steps = (int32_t)((angle * angleToStep) - 0.5);
292  this->moveSteps( steps );
293  }
294  else
295  {
296  steps = (int32_t)((angle * angleToStep) + 0.5);
297  this->moveSteps( steps );
298  }
299 }
300 
301 
302 void uStepperS::moveToAngle( float angle )
303 {
304  float diff;
305  int32_t steps;
306 
307  diff = angle - this->angleMoved();
308  steps = (int32_t)( (abs(diff) * angleToStep) + 0.5);
309 
310  if(diff < 0.0)
311  {
312  this->moveSteps( -steps );
313  }
314  else
315  {
316  this->moveSteps( steps );
317  }
318 }
319 
320 void uStepperS::enableStallguard( int8_t threshold, bool stopOnStall, float rpm )
321 {
322  this->clearStall();
323  this->stallThreshold = threshold;
324  this->stallStop = stopOnStall;
325 
326  pointer->driver.enableStallguard( threshold, stopOnStall, rpm);
327 
328  this->stallEnabled = true;
329 }
330 
332 {
333  pointer->driver.disableStallguard();
334 
335  this->stallEnabled = false;
336 }
337 
339 {
340  pointer->driver.clearStall();
341 }
342 
344 {
345  return this->isStalled( this->stallThreshold );
346 }
347 
348 bool uStepperS::isStalled( int8_t threshold )
349 {
350  // If the threshold is different from what is configured..
351  if( threshold != this->stallThreshold || this->stallEnabled == false ){
352  // Reconfigure stallguard
353  this->enableStallguard( threshold, this->stallStop, 10 );
354  }
355 
356  int32_t stats = pointer->driver.readRegister(RAMP_STAT);
357 
358  // Only interested in 'status_sg', with bit position 13 (last bit in RAMP_STAT).
359  return ( stats >> 13 );
360 }
361 
362 void uStepperS::setBrakeMode( uint8_t mode, float brakeCurrent )
363 {
364  int32_t registerContent = this->driver.readRegister(PWMCONF);
365  registerContent &= ~(3UL << 20);
366  if(mode == FREEWHEELBRAKE)
367  {
368  this->setHoldCurrent(0.0);
369  this->driver.writeRegister( PWMCONF, PWM_AUTOSCALE(1) | PWM_GRAD(1) | PWM_AMPL(128) | PWM_FREQ(0) | FREEWHEEL(1) );
370  }
371  else if(mode == COOLBRAKE)
372  {
373  this->setHoldCurrent(0.0);
374  this->driver.writeRegister( PWMCONF, PWM_AUTOSCALE(1) | PWM_GRAD(1) | PWM_AMPL(128) | PWM_FREQ(0) | FREEWHEEL(2) );
375  }
376  else
377  {
378  this->setHoldCurrent(brakeCurrent);
379  this->driver.writeRegister( PWMCONF, PWM_AUTOSCALE(1) | PWM_GRAD(1) | PWM_AMPL(128) | PWM_FREQ(0) | FREEWHEEL(0) );
380  }
381 }
382 
383 void uStepperS::setRPM( float rpm)
384 {
385  int32_t velocityDir = rpmToVelocity * rpm;
386 
387  if(velocityDir > 0){
388  driver.setDirection(1);
389  }else{
390  driver.setDirection(0);
391  }
392 
393  // The velocity cannot be signed
394  uint32_t velocity = abs(velocityDir);
395 
396  driver.setVelocity( (uint32_t)velocity );
397 }
398 
399 
400 void uStepperS::setSPIMode( uint8_t mode ){
401 
402  switch(mode){
403  case 2:
404  SPCR1 |= (1<<CPOL1); // Set CPOL HIGH = 1
405  SPCR1 &= ~(1<<CPHA1); // Set CPHA LOW = 0
406  break;
407 
408  case 3:
409  SPCR1 |= (1<<CPOL1); // Set CPOL HIGH = 1
410  SPCR1 |= (1<<CPHA1); // Set CPHA HIGH = 1
411  break;
412  }
413 }
414 
415 uint8_t uStepperS::SPI(uint8_t data){
416 
417  SPDR1 = data;
418 
419  // Wait for transmission complete
420  while(!( SPSR1 & (1 << SPIF1) ));
421 
422  return SPDR1;
423 
424 }
425 
426 void uStepperS::setMaxVelocity( float velocity )
427 {
428  velocity *= (float)this->microSteps;
429  velocity = abs(velocity)*VELOCITYCONVERSION;
430 
431  this->maxVelocity = velocity;
432 
433  // Steps per second, has to be converted to microsteps
434  this->driver.setVelocity( (uint32_t)( this->maxVelocity ) );
435 }
436 
437 void uStepperS::setMaxAcceleration( float acceleration )
438 {
439  acceleration *= (float)this->microSteps;
440  acceleration = abs(acceleration) * ACCELERATIONCONVERSION;
441 
442  this->maxAcceleration = acceleration;
443 
444 
445  // Steps per second, has to be converted to microsteps
446  this->driver.setAcceleration( (uint32_t)(this->maxAcceleration ) );
447 }
448 
449 void uStepperS::setMaxDeceleration( float deceleration )
450 {
451  deceleration *= (float)this->microSteps;
452  deceleration = abs(deceleration) * ACCELERATIONCONVERSION;
453 
454  this->maxDeceleration = deceleration;
455 
456  // Steps per second, has to be converted to microsteps
457  this->driver.setDeceleration( (uint32_t)(this->maxDeceleration ) );
458 }
459 
460 void uStepperS::setCurrent( double current )
461 {
462  if( current <= 100.0 && current >= 0.0){
463  // The current needs to be in the range of 0-31
464  this->driver.current = ceil(0.31 * current);
465  }else{
466  // If value is out of range, set default
467  this->driver.current = 16;
468  }
469 
471 }
472 
473 void uStepperS::setHoldCurrent( double current )
474 {
475  // The current needs to be in the range of 0-31
476  if( current <= 100.0 && current >= 0.0){
477  // The current needs to be in the range of 0-31
478  this->driver.holdCurrent = ceil(0.31 * current);
479  }else{
480  // If value is out of range, set default
481  this->driver.holdCurrent = 16;
482  }
483 
485 }
486 
487 void uStepperS::runContinous( bool direction )
488 {
489  this->driver.setDeceleration( (uint32_t)( this->maxDeceleration ) );
490  this->driver.setAcceleration( (uint32_t)(this->maxAcceleration ) );
491  this->driver.setVelocity( (uint32_t)( this->maxVelocity ) );
492 
493  // Make sure we use velocity mode
495 
496  // Set the direction
497  this->driver.setDirection( direction );
498 }
499 
500 float uStepperS::angleMoved ( void )
501 {
502  return this->encoder.getAngleMoved();
503 }
504 
505 void uStepperS::stop( bool mode){
506 
507  if(mode == HARD)
508  {
509  this->driver.setDeceleration( 0xFFFE );
510  this->driver.setAcceleration( 0xFFFE );
511  this->setRPM(0);
512  while(this->driver.readRegister(VACTUAL) != 0);
513  this->driver.setDeceleration( (uint32_t)( this->maxDeceleration ) );
514  this->driver.setAcceleration( (uint32_t)(this->maxAcceleration ) );
515  }
516  else
517  {
518  this->setRPM(0);
519  }
520 }
521 
522 void uStepperS::filterSpeedPos(posFilter_t *filter, int32_t steps)
523 {
524  if(this->mode != DROPIN)
525  {
526  filter->posEst += filter->velEst * ENCODERINTPERIOD * 0.5f;
527  }
528  else
529  {
530  filter->posEst += filter->velEst * ENCODERINTPERIOD;
531  }
532 
533 
534  filter->posError = (float)steps - filter->posEst;
535  if(this->mode != DROPIN)
536  {
537  filter->velIntegrator += filter->posError * PULSEFILTERKI * 0.5f;
538  }
539  else
540  {
541  filter->velIntegrator += filter->posError * PULSEFILTERKI;
542  }
543  filter->velEst = (filter->posError * PULSEFILTERKP) + filter->velIntegrator;
544 }
545 
546 void interrupt1(void)
547 {
548  if(PIND & 0x04)
549  {
550  PORTD |= (1 << 4);
551  }
552  else
553  {
554  PORTD &= ~(1 << 4);
555  }
556 }
557 
558 void interrupt0(void)
559 {
560  if(PIND & 0x04)
561  {
562  PORTD |= (1 << 4);
563  }
564  else
565  {
566  PORTD &= ~(1 << 4);
567  }
568  if((PINB & (0x08))) //CCW
569  {
571  {
572  pointer->stepCnt-=pointer->dropinStepSize; //DIR is set to CCW, therefore we subtract 1 step from step count (negative values = number of steps in CCW direction from initial postion)
573  }
574  else
575  {
576  pointer->stepCnt+=pointer->dropinStepSize; //DIR is set to CW, therefore we add 1 step to step count (positive values = number of steps in CW direction from initial postion)
577  }
578 
579  }
580  else //CW
581  {
583  {
584  pointer->stepCnt+=pointer->dropinStepSize; //DIR is set to CW, therefore we add 1 step to step count (positive values = number of steps in CW direction from initial postion)
585  }
586  else
587  {
588  pointer->stepCnt-=pointer->dropinStepSize; //DIR is set to CCW, therefore we subtract 1 step from step count (negative values = number of steps in CCW direction from initial postion)
589  }
590  }
591 }
592 
594 {
595 
596  int32_t stepsMoved;
597  int32_t stepCntTemp;
598  float error;
599  float output;
600  sei();
601 
603  stepsMoved = pointer->driver.getPosition();
604  if(pointer->mode == DROPIN)
605  {
606  cli();
607  stepCntTemp = pointer->stepCnt;
608  sei();
609 
611 
612  if(!pointer->pidDisabled)
613  {
614  error = (stepCntTemp - (int32_t)(pointer->encoder.angleMoved * ENCODERDATATOSTEP))/16;
616  pointer->pid(error);
617  }
618  return;
619  }
620  else if(pointer->mode == CLOSEDLOOP)
621  {
622  if(!pointer->pidDisabled)
623  {
626  {
629  }
630 
632  }
633  }
634 }
635 
636 void uStepperS::setControlThreshold(float threshold)
637 {
638  this->controlThreshold = threshold;
639 }
641 {
642  cli();
643  this->pidDisabled = 0;
644  sei();
645 }
646 
648 {
649  cli();
650  this->pidDisabled = 1;
651  sei();
652 }
653 
655 {
656  this->enablePid();
657 }
658 
660 {
661  this->disablePid();
662 }
663 
664 float uStepperS::moveToEnd(bool dir, float rpm, int8_t threshold, uint32_t timeOut)
665 {
666  uint32_t timeOutStart = micros();
667  // Lowest reliable speed for stallguard
668  if (rpm < 10.0)
669  rpm = 10.0;
670 
671  if(dir == CW)
672  this->setRPM(abs(rpm));
673  else
674  this->setRPM(-abs(rpm));
675 
676  delay(100);
677 
678  this->isStalled();
679  // Enable stallguard to detect hardware stop (use driver directly, as to not override user stall settings)
680  pointer->driver.enableStallguard( threshold, true, rpm );
681 
682  float length = this->encoder.getAngleMoved();
683 
684  while( !this->isStalled() ){
685  delay(1);
686  if((micros() - timeOutStart) > (timeOut * 1000))
687  {
688  break; // TimeOut !! break out and exit
689  }
690  }
691  this->stop();
692  pointer->driver.clearStall();
693 
694  // Return to normal operation
696 
697  length -= this->encoder.getAngleMoved();
698  delay(1000);
699  return abs(length);
700 }
701 
703 {
704  return this->currentPidError;
705 }
706 
707 float uStepperS::pid(float error)
708 {
709  float u;
710  float limit = abs(this->currentPidSpeed) + 10000.0;
711  static float integral;
712  static bool integralReset = 0;
713  static float errorOld, differential = 0.0;
714 
715  this->currentPidError = error;
716 
717  u = error*this->pTerm;
718 
719  if(u > 0.0)
720  {
721  if(u > limit)
722  {
723  u = limit;
724  }
725  }
726  else if(u < 0.0)
727  {
728  if(u < -limit)
729  {
730  u = -limit;
731  }
732  }
733 
734  integral += error*this->iTerm;
735 
736  if(integral > 200000.0)
737  {
738  integral = 200000.0;
739  }
740  else if(integral < -200000.0)
741  {
742  integral = -200000.0;
743  }
744 
745  if(error > -10 && error < 10)
746  {
747  if(!integralReset)
748  {
749  integralReset = 1;
750  integral = 0;
751  }
752  }
753  else
754  {
755  integralReset = 0;
756  }
757 
758  u += integral;
759 
760  differential *= 0.9;
761  differential += 0.1*((error - errorOld)*this->dTerm);
762 
763  errorOld = error;
764 
765  u += differential;
766 
767  u *= this->stepsPerSecondToRPM * 16.0;
768  this->setRPM(u);
769  this->driver.setDeceleration( 0xFFFE );
770  this->driver.setAcceleration( 0xFFFE );
771 }
772 
774 {
775  this->pTerm = P;
776 }
777 
779 {
780  this->iTerm = I * ENCODERINTPERIOD;
781 }
782 
784 {
785  this->dTerm = D * ENCODERINTFREQ;
786 }
787 
788 void uStepperS::invertDropinDir(bool invert)
789 {
790  this->invertPidDropinDirection = invert;
791 }
792 
793 void uStepperS::parseCommand(String *cmd)
794 {
795  uint8_t i = 0;
796  String value;
797 
798  if(cmd->charAt(2) == ';')
799  {
800  Serial.println("COMMAND NOT ACCEPTED");
801  return;
802  }
803 
804  value.remove(0);
805  /****************** SET P Parameter ***************************
806  * *
807  * *
808  **************************************************************/
809  if(cmd->substring(0,2) == String("P="))
810  {
811  for(i = 2;;i++)
812  {
813  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
814  {
815  value.concat(cmd->charAt(i));
816  }
817  else if(cmd->charAt(i) == '.')
818  {
819  value.concat(cmd->charAt(i));
820  i++;
821  break;
822  }
823  else if(cmd->charAt(i) == ';')
824  {
825  break;
826  }
827  else
828  {
829  Serial.println("COMMAND NOT ACCEPTED");
830  return;
831  }
832  }
833 
834  for(;;i++)
835  {
836  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
837  {
838  value.concat(cmd->charAt(i));
839  }
840  else if(cmd->charAt(i) == ';')
841  {
842  Serial.print("COMMAND ACCEPTED. P = ");
843  Serial.println(value.toFloat(),4);
844  this->dropinSettings.P.f = value.toFloat();
845  this->saveDropinSettings();
846  this->setProportional(value.toFloat());
847  return;
848  }
849  else
850  {
851  Serial.println("COMMAND NOT ACCEPTED");
852  return;
853  }
854  }
855  }
856 
857 /****************** SET I Parameter ***************************
858  * *
859  * *
860  **************************************************************/
861  else if(cmd->substring(0,2) == String("I="))
862  {
863  for(i = 2;;i++)
864  {
865  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
866  {
867  value.concat(cmd->charAt(i));
868  }
869  else if(cmd->charAt(i) == '.')
870  {
871  value.concat(cmd->charAt(i));
872  i++;
873  break;
874  }
875  else if(cmd->charAt(i) == ';')
876  {
877  break;
878  }
879  else
880  {
881  Serial.println("COMMAND NOT ACCEPTED");
882  return;
883  }
884  }
885 
886  for(;;i++)
887  {
888  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
889  {
890  value.concat(cmd->charAt(i));
891  }
892  else if(cmd->charAt(i) == ';')
893  {
894  Serial.print("COMMAND ACCEPTED. I = ");
895  Serial.println(value.toFloat(),4);
896  this->dropinSettings.I.f = value.toFloat();
897  this->saveDropinSettings();
898  this->setIntegral(value.toFloat());
899  return;
900  }
901  else
902  {
903  Serial.println("COMMAND NOT ACCEPTED");
904  return;
905  }
906  }
907  }
908 
909 /****************** SET D Parameter ***************************
910  * *
911  * *
912  **************************************************************/
913  else if(cmd->substring(0,2) == String("D="))
914  {
915  for(i = 2;;i++)
916  {
917  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
918  {
919  value.concat(cmd->charAt(i));
920  }
921  else if(cmd->charAt(i) == '.')
922  {
923  value.concat(cmd->charAt(i));
924  i++;
925  break;
926  }
927  else if(cmd->charAt(i) == ';')
928  {
929  break;
930  }
931  else
932  {
933  Serial.println("COMMAND NOT ACCEPTED");
934  return;
935  }
936  }
937 
938  for(;;i++)
939  {
940  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
941  {
942  value.concat(cmd->charAt(i));
943  }
944  else if(cmd->charAt(i) == ';')
945  {
946  Serial.print("COMMAND ACCEPTED. D = ");
947  Serial.println(value.toFloat(),4);
948  this->dropinSettings.D.f = value.toFloat();
949  this->saveDropinSettings();
950  this->setDifferential(value.toFloat());
951  return;
952  }
953  else
954  {
955  Serial.println("COMMAND NOT ACCEPTED");
956  return;
957  }
958  }
959  }
960 
961 /****************** invert Direction ***************************
962  * *
963  * *
964  **************************************************************/
965  else if(cmd->substring(0,6) == String("invert"))
966  {
967  if(cmd->charAt(6) != ';')
968  {
969  Serial.println("COMMAND NOT ACCEPTED");
970  return;
971  }
972  if(this->invertPidDropinDirection)
973  {
974  Serial.println(F("Direction normal!"));
975  this->dropinSettings.invert = 0;
976  this->saveDropinSettings();
977  this->invertDropinDir(0);
978  return;
979  }
980  else
981  {
982  Serial.println(F("Direction inverted!"));
983  this->dropinSettings.invert = 1;
984  this->saveDropinSettings();
985  this->invertDropinDir(1);
986  return;
987  }
988  }
989 
990  /****************** get Current Pid Error ********************
991  * *
992  * *
993  **************************************************************/
994  else if(cmd->substring(0,5) == String("error"))
995  {
996  if(cmd->charAt(5) != ';')
997  {
998  Serial.println("COMMAND NOT ACCEPTED");
999  return;
1000  }
1001  Serial.print(F("Current Error: "));
1002  Serial.print(this->getPidError());
1003  Serial.println(F(" Steps"));
1004  }
1005 
1006  /****************** Get run/hold current settings ************
1007  * *
1008  * *
1009  **************************************************************/
1010  else if(cmd->substring(0,7) == String("current"))
1011  {
1012  if(cmd->charAt(7) != ';')
1013  {
1014  Serial.println("COMMAND NOT ACCEPTED");
1015  return;
1016  }
1017  Serial.print(F("Run Current: "));
1018  Serial.print(ceil(((float)this->driver.current)/0.31));
1019  Serial.println(F(" %"));
1020  Serial.print(F("Hold Current: "));
1021  Serial.print(ceil(((float)this->driver.holdCurrent)/0.31));
1022  Serial.println(F(" %"));
1023  }
1024 
1025  /****************** Get PID Parameters ***********************
1026  * *
1027  * *
1028  **************************************************************/
1029  else if(cmd->substring(0,10) == String("parameters"))
1030  {
1031  if(cmd->charAt(10) != ';')
1032  {
1033  Serial.println("COMMAND NOT ACCEPTED");
1034  return;
1035  }
1036  Serial.print(F("P: "));
1037  Serial.print(this->dropinSettings.P.f,4);
1038  Serial.print(F(", "));
1039  Serial.print(F("I: "));
1040  Serial.print(this->dropinSettings.I.f,4);
1041  Serial.print(F(", "));
1042  Serial.print(F("D: "));
1043  Serial.println(this->dropinSettings.D.f,4);
1044  }
1045 
1046  /****************** Help menu ********************************
1047  * *
1048  * *
1049  **************************************************************/
1050  else if(cmd->substring(0,4) == String("help"))
1051  {
1052  if(cmd->charAt(4) != ';')
1053  {
1054  Serial.println("COMMAND NOT ACCEPTED");
1055  return;
1056  }
1057  this->dropinPrintHelp();
1058  }
1059 
1060 /****************** SET run current ***************************
1061  * *
1062  * *
1063  **************************************************************/
1064  else if(cmd->substring(0,11) == String("runCurrent="))
1065  {
1066  for(i = 11;;i++)
1067  {
1068  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1069  {
1070  value.concat(cmd->charAt(i));
1071  }
1072  else if(cmd->charAt(i) == '.')
1073  {
1074  value.concat(cmd->charAt(i));
1075  i++;
1076  break;
1077  }
1078  else if(cmd->charAt(i) == ';')
1079  {
1080  break;
1081  }
1082  else
1083  {
1084  Serial.println("COMMAND NOT ACCEPTED");
1085  return;
1086  }
1087  }
1088 
1089  for(;;i++)
1090  {
1091  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1092  {
1093  value.concat(cmd->charAt(i));
1094  }
1095  else if(cmd->charAt(i) == ';')
1096  {
1097  if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1098  {
1099  i = (uint8_t)value.toFloat();
1100  break;
1101  }
1102  else
1103  {
1104  Serial.println("COMMAND NOT ACCEPTED");
1105  return;
1106  }
1107  }
1108  else
1109  {
1110  Serial.println("COMMAND NOT ACCEPTED");
1111  return;
1112  }
1113  }
1114 
1115  Serial.print("COMMAND ACCEPTED. runCurrent = ");
1116  Serial.print(i);
1117  Serial.println(F(" %"));
1118  this->dropinSettings.runCurrent = i;
1119  this->saveDropinSettings();
1120  this->setCurrent(i);
1121  }
1122 
1123  /****************** SET run current ***************************
1124  * *
1125  * *
1126  **************************************************************/
1127  else if(cmd->substring(0,12) == String("holdCurrent="))
1128  {
1129  for(i = 12;;i++)
1130  {
1131  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1132  {
1133  value.concat(cmd->charAt(i));
1134  }
1135  else if(cmd->charAt(i) == '.')
1136  {
1137  value.concat(cmd->charAt(i));
1138  i++;
1139  break;
1140  }
1141  else if(cmd->charAt(i) == ';')
1142  {
1143  break;
1144  }
1145  else
1146  {
1147  Serial.println("COMMAND NOT ACCEPTED");
1148  return;
1149  }
1150  }
1151 
1152  for(;;i++)
1153  {
1154  if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1155  {
1156  value.concat(cmd->charAt(i));
1157  }
1158  else if(cmd->charAt(i) == ';')
1159  {
1160  if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1161  {
1162  i = (uint8_t)value.toFloat();
1163  break;
1164  }
1165  else
1166  {
1167  Serial.println("COMMAND NOT ACCEPTED");
1168  return;
1169  }
1170  }
1171  else
1172  {
1173  Serial.println("COMMAND NOT ACCEPTED");
1174  return;
1175  }
1176  }
1177 
1178  Serial.print("COMMAND ACCEPTED. holdCurrent = ");
1179  Serial.print(i);
1180  Serial.println(F(" %"));
1181  this->dropinSettings.holdCurrent = i;
1182  this->saveDropinSettings();
1183  this->setHoldCurrent(i);
1184  }
1185 
1186  /****************** DEFAULT (Reject!) ************************
1187  * *
1188  * *
1189  **************************************************************/
1190  else
1191  {
1192  Serial.println("COMMAND NOT ACCEPTED");
1193  return;
1194  }
1195 
1196 }
1197 
1199 {
1200  static String stringInput;
1201  static uint32_t t = millis();
1202 
1203  while(1)
1204  {
1205  while(!Serial.available())
1206  {
1207  delay(1);
1208  if((millis() - t) >= 500)
1209  {
1210  stringInput.remove(0);
1211  t = millis();
1212  }
1213  }
1214  t = millis();
1215  stringInput += (char)Serial.read();
1216  if(stringInput.lastIndexOf(';') > -1)
1217  {
1218  this->parseCommand(&stringInput);
1219  stringInput.remove(0);
1220  }
1221  }
1222 }
1223 
1225 {
1226  Serial.println(F("uStepper S Dropin !"));
1227  Serial.println(F(""));
1228  Serial.println(F("Usage:"));
1229  Serial.println(F("Show this command list: 'help;'"));
1230  Serial.println(F("Get PID Parameters: 'parameters;'"));
1231  Serial.println(F("Set Proportional constant: 'P=10.002;'"));
1232  Serial.println(F("Set Integral constant: 'I=10.002;'"));
1233  Serial.println(F("Set Differential constant: 'D=10.002;'"));
1234  Serial.println(F("Invert Direction: 'invert;'"));
1235  Serial.println(F("Get Current PID Error: 'error;'"));
1236  Serial.println(F("Get Run/Hold Current Settings: 'current;'"));
1237  Serial.println(F("Set Run Current (percent): 'runCurrent=50.0;'"));
1238  Serial.println(F("Set Hold Current (percent): 'holdCurrent=50.0;'"));
1239  Serial.println(F(""));
1240  Serial.println(F(""));
1241 }
1242 
1244 {
1245  dropinCliSettings_t tempSettings;
1246 
1247  EEPROM.get(0,tempSettings);
1248 
1249  if(this->dropinSettingsCalcChecksum(&tempSettings) != tempSettings.checksum)
1250  {
1251  return 0;
1252  }
1253 
1254  this->dropinSettings = tempSettings;
1255  this->setProportional(this->dropinSettings.P.f);
1256  this->setIntegral(this->dropinSettings.I.f);
1257  this->setDifferential(this->dropinSettings.D.f);
1258  this->invertDropinDir((bool)this->dropinSettings.invert);
1259  this->setCurrent(this->dropinSettings.runCurrent);
1261  return 1;
1262 }
1263 
1265 {
1267 
1268  EEPROM.put(0,this->dropinSettings);
1269 }
1270 
1272 {
1273  uint8_t i;
1274  uint8_t checksum = 0xAA;
1275  uint8_t *p = (uint8_t*)settings;
1276 
1277  for(i=0; i < 15; i++)
1278  {
1279  checksum ^= *p++;
1280  }
1281 
1282  return checksum;
1283 }
void setAcceleration(uint32_t acceleration)
Set motor acceleration.
void setDirection(bool direction)
void readMotorStatus(void)
volatile int32_t xTarget
void setDeceleration(uint32_t deceleration)
Set motor deceleration.
int32_t getPosition(void)
Returns the current position of the motor driver.
void disableStallguard(void)
void updateCurrent(void)
Writes the current setting registers of the motor driver
void setShaftDirection(bool direction)
Set motor driver direction.
int32_t writeRegister(uint8_t address, uint32_t datagram)
Write a register of the motor driver.
void init(uStepperS *_pointer)
Initiation of the motor driver.
void enableStallguard(int8_t threshold, bool stopOnStall, float rpm)
int32_t readRegister(uint8_t address)
Reads a register from the motor driver.
void setVelocity(uint32_t velocity)
Set motor velocity.
void clearStall(void)
int32_t getVelocity(void)
Returns the current speed of the motor driver.
void setRampMode(uint8_t mode)
Set motor driver to position mode or velocity mode.
void setPosition(int32_t position)
Set the motor position.
volatile int32_t angleMoved
uint16_t captureAngle(void)
Capture the current shaft angle.
float getAngleMoved(bool filtered=true)
Returns the angle moved from reference position in degrees.
void setHome(float initialAngle=0)
Define new reference(home) position.
void init(uStepperS *_pointer)
Initiation of the encoder.
volatile posFilter_t encoderFilter
volatile uint8_t Beta
Prototype of class for accessing all features of the uStepper S in a single object.
Definition: uStepperS.h:279
void stop(bool mode=HARD)
Stop the motor.
Definition: uStepperS.cpp:505
bool loadDropinSettings(void)
Definition: uStepperS.cpp:1243
float angleToStep
Definition: uStepperS.h:734
void setMaxVelocity(float velocity)
Set the maximum velocity of the stepper motor.
Definition: uStepperS.cpp:426
void saveDropinSettings(void)
Definition: uStepperS.cpp:1264
void setup(uint8_t mode=NORMAL, uint16_t stepsPerRevolution=200, float pTerm=10.0, float iTerm=0.0, float dTerm=0.0, uint16_t dropinStepSize=16, bool setHome=true, uint8_t invert=0, uint8_t runCurrent=50, uint8_t holdCurrent=30)
Initializes the different parts of the uStepper S object.
Definition: uStepperS.cpp:167
float angleMoved(void)
Get the angle moved from reference position in degrees.
Definition: uStepperS.cpp:500
float moveToEnd(bool dir, float rpm=40.0, int8_t threshold=4, uint32_t timeOut=100000)
Moves the motor to its physical limit, without limit switch.
Definition: uStepperS.cpp:664
volatile posFilter_t externalStepInputFilter
Definition: uStepperS.h:746
volatile float controlThreshold
Definition: uStepperS.h:760
float pid(float error)
Definition: uStepperS.cpp:707
float getDriverRPM(void)
Get the RPM from driver.
Definition: uStepperS.cpp:94
void parseCommand(String *cmd)
This method is used for the dropinCli to take in user commands.
Definition: uStepperS.cpp:793
void setDifferential(float D)
This method is used to change the PID differential parameter D.
Definition: uStepperS.cpp:783
void dropinCli()
This method is used to tune Drop-in parameters. After tuning uStepper S, the parameters are saved in ...
Definition: uStepperS.cpp:1198
void setMaxDeceleration(float deceleration)
Set the maximum deceleration of the stepper motor.
Definition: uStepperS.cpp:449
float stepsPerSecondToRPM
Definition: uStepperS.h:743
float maxAcceleration
Definition: uStepperS.h:730
bool isStalled(void)
This method returns a bool variable indicating wether the motor is stalled or not....
Definition: uStepperS.cpp:343
void setMaxAcceleration(float acceleration)
Set the maximum acceleration of the stepper motor.
Definition: uStepperS.cpp:437
void setProportional(float P)
This method is used to change the PID proportional parameter P.
Definition: uStepperS.cpp:773
bool invertPidDropinDirection
Definition: uStepperS.h:732
void disableStallguard(void)
Disables the builtin stallguard offered from TMC5130, and reenables StealthChop.
Definition: uStepperS.cpp:331
void setIntegral(float I)
This method is used to change the PID integral parameter I.
Definition: uStepperS.cpp:778
float rpmToVelocity
Definition: uStepperS.h:733
void setHoldCurrent(double current)
Set motor hold current.
Definition: uStepperS.cpp:473
float dTerm
Definition: uStepperS.h:756
void setRPM(float rpm)
Set the velocity in rpm.
Definition: uStepperS.cpp:383
uint16_t fullSteps
Definition: uStepperS.h:737
void disablePid(void)
This method disables the PID until calling enablePid.
Definition: uStepperS.cpp:647
volatile bool pidDisabled
Definition: uStepperS.h:758
float RPMToStepsPerSecond
Definition: uStepperS.h:744
void moveAngle(float angle)
Makes the motor rotate a specific angle relative to the current position.
Definition: uStepperS.cpp:285
float stepTime
Definition: uStepperS.h:716
float maxDeceleration
Definition: uStepperS.h:731
void enableClosedLoop(void)
This method reenables the closed loop mode after being disabled.
Definition: uStepperS.cpp:654
void filterSpeedPos(posFilter_t *filter, int32_t steps)
Definition: uStepperS.cpp:522
void setBrakeMode(uint8_t mode, float brakeCurrent=25.0)
Definition: uStepperS.cpp:362
float velToRpm
Definition: uStepperS.h:718
volatile uint8_t mode
Definition: uStepperS.h:751
float pTerm
Definition: uStepperS.h:752
friend void interrupt0(void)
Used by dropin feature to take in step pulses.
Definition: uStepperS.cpp:558
uStepperEncoder encoder
Definition: uStepperS.h:291
void enableStallguard(int8_t threshold=4, bool stopOnStall=false, float rpm=10.0)
Enable TMC5130 StallGuard.
Definition: uStepperS.cpp:320
void checkOrientation(float distance=10)
This method is used to check the orientation of the motor connector.
Definition: uStepperS.cpp:101
float maxVelocity
Definition: uStepperS.h:724
float currentPidSpeed
Definition: uStepperS.h:748
int8_t stallThreshold
Definition: uStepperS.h:770
void setControlThreshold(float threshold)
This method sets the control threshold for the closed loop position control in microsteps - i....
Definition: uStepperS.cpp:636
void setSPIMode(uint8_t mode)
Definition: uStepperS.cpp:400
void moveToAngle(float angle)
Makes the motor rotate to a specific absolute angle.
Definition: uStepperS.cpp:302
uint16_t dropinStepSize
Definition: uStepperS.h:739
void clearStall(void)
Clear the stallguard, reenabling the motor to return to its previous operation.
Definition: uStepperS.cpp:338
dropinCliSettings_t dropinSettings
Definition: uStepperS.h:791
void init(void)
Internal function to prepare the uStepperS in the constructor.
Definition: uStepperS.cpp:59
void runContinous(bool dir)
Make the motor rotate continuously.
Definition: uStepperS.cpp:487
uint8_t SPI(uint8_t data)
Definition: uStepperS.cpp:415
bool stallStop
Definition: uStepperS.h:773
void enablePid(void)
This method reenables the PID after being disabled.
Definition: uStepperS.cpp:640
void setCurrent(double current)
Set motor output current.
Definition: uStepperS.cpp:460
float rpmToVel
Definition: uStepperS.h:717
void dropinPrintHelp()
This method is used to print the dropinCli menu explainer:
Definition: uStepperS.cpp:1224
volatile float currentPidError
Definition: uStepperS.h:767
volatile bool shaftDir
Definition: uStepperS.h:779
uint8_t dropinSettingsCalcChecksum(dropinCliSettings_t *settings)
Definition: uStepperS.cpp:1271
int32_t stepCnt
Definition: uStepperS.h:741
void invertDropinDir(bool invert)
This method is used to invert the drop-in direction pin interpretation.
Definition: uStepperS.cpp:788
uStepperDriver driver
Definition: uStepperS.h:288
float iTerm
Definition: uStepperS.h:754
void disableClosedLoop(void)
This method disables the closed loop mode until calling enableClosedLoop.
Definition: uStepperS.cpp:659
uStepperS()
Constructor of uStepper class.
Definition: uStepperS.cpp:36
void moveSteps(int32_t steps)
Make the motor perform a predefined number of steps.
Definition: uStepperS.cpp:270
bool getMotorState(uint8_t statusType=POSITION_REACHED)
Get the current motor driver state.
Definition: uStepperS.cpp:84
bool stallEnabled
Definition: uStepperS.h:776
uint16_t microSteps
Definition: uStepperS.h:736
float getPidError(void)
This method returns the current PID error.
Definition: uStepperS.cpp:702
Struct to store dropin settings.
Definition: uStepperS.h:187
floatBytes_t D
Definition: uStepperS.h:190
floatBytes_t I
Definition: uStepperS.h:189
floatBytes_t P
Definition: uStepperS.h:188
Struct for encoder velocity estimator.
Definition: uStepperS.h:203
float velIntegrator
Definition: uStepperS.h:206
float posEst
Definition: uStepperS.h:205
float posError
Definition: uStepperS.h:204
float velEst
Definition: uStepperS.h:207
#define RAMP_STAT
#define VACTUAL
#define PWM_AMPL(n)
#define PWM_GRAD(n)
#define FREEWHEEL(n)
#define XACTUAL
#define PWM_AUTOSCALE(n)
#define XTARGET
#define PWM_FREQ(n)
#define PWMCONF
#define ACCELERATIONCONVERSION
#define VELOCITY_MODE_POS
#define VELOCITYCONVERSION
#define ENCODERDATATOSTEP
void interrupt0(void)
Used by dropin feature to take in step pulses.
Definition: uStepperS.cpp:558
void interrupt1(void)
Used by dropin feature to take in enable signal.
Definition: uStepperS.cpp:546
uStepperS * pointer
Definition: uStepperS.cpp:34
void TIMER1_COMPA_vect(void)
Definition: uStepperS.cpp:593
#define ENCODERINTPERIOD
Definition: uStepperS.h:243
#define SD_MODE
Definition: uStepperS.h:219
#define FREEWHEELBRAKE
Definition: uStepperS.h:156
#define CS_ENCODER
Definition: uStepperS.h:223
#define HARD
Definition: uStepperS.h:215
#define PULSEFILTERKI
Definition: uStepperS.h:245
#define DRV_ENN
Definition: uStepperS.h:218
#define CLOCKFREQ
Definition: uStepperS.h:236
#define DROPIN
Definition: uStepperS.h:232
#define SCK1
Definition: uStepperS.h:228
#define ENCODERINTFREQ
Definition: uStepperS.h:242
#define COOLBRAKE
Definition: uStepperS.h:157
#define CW
Definition: uStepperS.h:160
#define CS_DRIVER
Definition: uStepperS.h:222
#define CLOSEDLOOP
Definition: uStepperS.h:233
#define MOSI_ENC
Definition: uStepperS.h:226
#define MOSI1
Definition: uStepperS.h:225
#define PULSEFILTERKP
Definition: uStepperS.h:244