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