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