uStepper S
uStepperS.cpp
Go to the documentation of this file.
1/********************************************************************************************
2* File: uStepperS.cpp *
3* Version: 2.3.0 *
4* Date: December 27th, 2021 *
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
48uStepperS::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
59void 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
84bool 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
101void 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
167void 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){
263 }
264
265 this->pidDisabled = 0;
266
267 DDRB |= (1 << 4);
268}
269
270void 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
285void 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
302void 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
320void 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
348bool 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
362void 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
383void uStepperS::setRPM( float rpm)
384{
385 int32_t velocityDir = rpmToVelocity * rpm;
386
387 if(velocityDir > 0){
389 }else{
391 }
392
393 // The velocity cannot be signed
394 uint32_t velocity = abs(velocityDir);
395
396 driver.setVelocity( (uint32_t)velocity );
397}
398
399
400void 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
415uint8_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
426void 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
437void 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
449void 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
460void 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
473void 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
487void 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
501{
502 return this->encoder.getAngleMoved();
503}
504
505void 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 while(this->driver.readRegister(VACTUAL) != 0);
520 }
521 // Get current position
522 int32_t current = this->driver.getPosition();
523 // Set new position
524 this->driver.setPosition( current );
525}
526
527void uStepperS::filterSpeedPos(posFilter_t *filter, int32_t steps)
528{
529 if(this->mode != DROPIN)
530 {
531 filter->posEst += filter->velEst * ENCODERINTPERIOD * 0.5f;
532 }
533 else
534 {
535 filter->posEst += filter->velEst * ENCODERINTPERIOD;
536 }
537
538
539 filter->posError = (float)steps - filter->posEst;
540 if(this->mode != DROPIN)
541 {
542 filter->velIntegrator += filter->posError * PULSEFILTERKI * 0.5f;
543 }
544 else
545 {
546 filter->velIntegrator += filter->posError * PULSEFILTERKI;
547 }
548 filter->velEst = (filter->posError * PULSEFILTERKP) + filter->velIntegrator;
549}
550
551void interrupt1(void)
552{
553 if(PIND & 0x04)
554 {
555 PORTD |= (1 << 4);
556 }
557 else
558 {
559 PORTD &= ~(1 << 4);
560 }
561}
562
563void interrupt0(void)
564{
565 if(PIND & 0x04)
566 {
567 PORTD |= (1 << 4);
568 }
569 else
570 {
571 PORTD &= ~(1 << 4);
572 }
573 if((PINB & (0x08))) //CCW
574 {
576 {
577 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)
578 }
579 else
580 {
581 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)
582 }
583
584 }
585 else //CW
586 {
588 {
589 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)
590 }
591 else
592 {
593 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)
594 }
595 }
596}
597
599{
600
601 int32_t stepsMoved;
602 int32_t stepCntTemp;
603 float error;
604 float output;
605 sei();
606
608 stepsMoved = pointer->driver.getPosition();
609 if(pointer->mode == DROPIN)
610 {
611 cli();
612 stepCntTemp = pointer->stepCnt;
613 sei();
614
616
617 if(!pointer->pidDisabled)
618 {
619 error = (stepCntTemp - (int32_t)(pointer->encoder.angleMoved * ENCODERDATATOSTEP))/16;
621 pointer->pid(error);
622 }
623 return;
624 }
625 else if(pointer->mode == CLOSEDLOOP)
626 {
627 if(!pointer->pidDisabled)
628 {
631 {
634 }
635
637 }
638 }
639}
640
642{
643 this->controlThreshold = threshold;
644}
646{
647 cli();
648 this->pidDisabled = 0;
649 sei();
650}
651
653{
654 cli();
655 this->pidDisabled = 1;
656 sei();
657}
658
660{
661 this->enablePid();
662}
663
665{
666 this->disablePid();
667}
668
669float uStepperS::moveToEnd(bool dir, float rpm, int8_t threshold, uint32_t timeOut)
670{
671 uint32_t timeOutStart = micros();
672 // Lowest reliable speed for stallguard
673 if (rpm < 10.0)
674 rpm = 10.0;
675
676 if(dir == CW)
677 this->setRPM(abs(rpm));
678 else
679 this->setRPM(-abs(rpm));
680
681 delay(100);
682
683 this->isStalled();
684 // Enable stallguard to detect hardware stop (use driver directly, as to not override user stall settings)
685 pointer->driver.enableStallguard( threshold, true, rpm );
686
687 float length = this->encoder.getAngleMoved();
688
689 while( !this->isStalled() ){
690 delay(1);
691 if((micros() - timeOutStart) > (timeOut * 1000))
692 {
693 break; // TimeOut !! break out and exit
694 }
695 }
696 this->stop();
697 pointer->driver.clearStall();
698
699 // Return to normal operation
701
702 length -= this->encoder.getAngleMoved();
703 delay(1000);
704 return abs(length);
705}
706
708{
709 return this->currentPidError;
710}
711
712float uStepperS::pid(float error)
713{
714 float u;
715 float limit = abs(this->currentPidSpeed) + 10000.0;
716 static float integral;
717 static bool integralReset = 0;
718 static float errorOld, differential = 0.0;
719
720 this->currentPidError = error;
721
722 u = error*this->pTerm;
723
724 if(u > 0.0)
725 {
726 if(u > limit)
727 {
728 u = limit;
729 }
730 }
731 else if(u < 0.0)
732 {
733 if(u < -limit)
734 {
735 u = -limit;
736 }
737 }
738
739 integral += error*this->iTerm;
740
741 if(integral > 200000.0)
742 {
743 integral = 200000.0;
744 }
745 else if(integral < -200000.0)
746 {
747 integral = -200000.0;
748 }
749
750 if(error > -10 && error < 10)
751 {
752 if(!integralReset)
753 {
754 integralReset = 1;
755 integral = 0;
756 }
757 }
758 else
759 {
760 integralReset = 0;
761 }
762
763 u += integral;
764
765 differential *= 0.9;
766 differential += 0.1*((error - errorOld)*this->dTerm);
767
768 errorOld = error;
769
770 u += differential;
771
772 u *= this->stepsPerSecondToRPM * 16.0;
773 this->setRPM(u);
774 this->driver.setDeceleration( 0xFFFE );
775 this->driver.setAcceleration( 0xFFFE );
776}
777
779{
780 this->pTerm = P;
781}
782
784{
785 this->iTerm = I * ENCODERINTPERIOD;
786}
787
789{
790 this->dTerm = D * ENCODERINTFREQ;
791}
792
794{
795 this->invertPidDropinDirection = invert;
796}
797
798void uStepperS::parseCommand(String *cmd)
799{
800 uint8_t i = 0;
801 String value;
802
803 if(cmd->charAt(2) == ';')
804 {
805 Serial.println("COMMAND NOT ACCEPTED");
806 return;
807 }
808
809 value.remove(0);
810 /****************** SET P Parameter ***************************
811 * *
812 * *
813 **************************************************************/
814 if(cmd->substring(0,2) == String("P="))
815 {
816 for(i = 2;;i++)
817 {
818 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
819 {
820 value.concat(cmd->charAt(i));
821 }
822 else if(cmd->charAt(i) == '.')
823 {
824 value.concat(cmd->charAt(i));
825 i++;
826 break;
827 }
828 else if(cmd->charAt(i) == ';')
829 {
830 break;
831 }
832 else
833 {
834 Serial.println("COMMAND NOT ACCEPTED");
835 return;
836 }
837 }
838
839 for(;;i++)
840 {
841 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
842 {
843 value.concat(cmd->charAt(i));
844 }
845 else if(cmd->charAt(i) == ';')
846 {
847 Serial.print("COMMAND ACCEPTED. P = ");
848 Serial.println(value.toFloat(),4);
849 this->dropinSettings.P.f = value.toFloat();
850 this->saveDropinSettings();
851 this->setProportional(value.toFloat());
852 return;
853 }
854 else
855 {
856 Serial.println("COMMAND NOT ACCEPTED");
857 return;
858 }
859 }
860 }
861
862/****************** SET I Parameter ***************************
863 * *
864 * *
865 **************************************************************/
866 else if(cmd->substring(0,2) == String("I="))
867 {
868 for(i = 2;;i++)
869 {
870 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
871 {
872 value.concat(cmd->charAt(i));
873 }
874 else if(cmd->charAt(i) == '.')
875 {
876 value.concat(cmd->charAt(i));
877 i++;
878 break;
879 }
880 else if(cmd->charAt(i) == ';')
881 {
882 break;
883 }
884 else
885 {
886 Serial.println("COMMAND NOT ACCEPTED");
887 return;
888 }
889 }
890
891 for(;;i++)
892 {
893 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
894 {
895 value.concat(cmd->charAt(i));
896 }
897 else if(cmd->charAt(i) == ';')
898 {
899 Serial.print("COMMAND ACCEPTED. I = ");
900 Serial.println(value.toFloat(),4);
901 this->dropinSettings.I.f = value.toFloat();
902 this->saveDropinSettings();
903 this->setIntegral(value.toFloat());
904 return;
905 }
906 else
907 {
908 Serial.println("COMMAND NOT ACCEPTED");
909 return;
910 }
911 }
912 }
913
914/****************** SET D Parameter ***************************
915 * *
916 * *
917 **************************************************************/
918 else if(cmd->substring(0,2) == String("D="))
919 {
920 for(i = 2;;i++)
921 {
922 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
923 {
924 value.concat(cmd->charAt(i));
925 }
926 else if(cmd->charAt(i) == '.')
927 {
928 value.concat(cmd->charAt(i));
929 i++;
930 break;
931 }
932 else if(cmd->charAt(i) == ';')
933 {
934 break;
935 }
936 else
937 {
938 Serial.println("COMMAND NOT ACCEPTED");
939 return;
940 }
941 }
942
943 for(;;i++)
944 {
945 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
946 {
947 value.concat(cmd->charAt(i));
948 }
949 else if(cmd->charAt(i) == ';')
950 {
951 Serial.print("COMMAND ACCEPTED. D = ");
952 Serial.println(value.toFloat(),4);
953 this->dropinSettings.D.f = value.toFloat();
954 this->saveDropinSettings();
955 this->setDifferential(value.toFloat());
956 return;
957 }
958 else
959 {
960 Serial.println("COMMAND NOT ACCEPTED");
961 return;
962 }
963 }
964 }
965
966/****************** invert Direction ***************************
967 * *
968 * *
969 **************************************************************/
970 else if(cmd->substring(0,6) == String("invert"))
971 {
972 if(cmd->charAt(6) != ';')
973 {
974 Serial.println("COMMAND NOT ACCEPTED");
975 return;
976 }
978 {
979 Serial.println(F("Direction normal!"));
980 this->dropinSettings.invert = 0;
981 this->saveDropinSettings();
982 this->invertDropinDir(0);
983 return;
984 }
985 else
986 {
987 Serial.println(F("Direction inverted!"));
988 this->dropinSettings.invert = 1;
989 this->saveDropinSettings();
990 this->invertDropinDir(1);
991 return;
992 }
993 }
994
995 /****************** get Current Pid Error ********************
996 * *
997 * *
998 **************************************************************/
999 else if(cmd->substring(0,5) == String("error"))
1000 {
1001 if(cmd->charAt(5) != ';')
1002 {
1003 Serial.println("COMMAND NOT ACCEPTED");
1004 return;
1005 }
1006 Serial.print(F("Current Error: "));
1007 Serial.print(this->getPidError());
1008 Serial.println(F(" Steps"));
1009 }
1010
1011 /****************** Get run/hold current settings ************
1012 * *
1013 * *
1014 **************************************************************/
1015 else if(cmd->substring(0,7) == String("current"))
1016 {
1017 if(cmd->charAt(7) != ';')
1018 {
1019 Serial.println("COMMAND NOT ACCEPTED");
1020 return;
1021 }
1022 Serial.print(F("Run Current: "));
1023 Serial.print(ceil(((float)this->driver.current)/0.31));
1024 Serial.println(F(" %"));
1025 Serial.print(F("Hold Current: "));
1026 Serial.print(ceil(((float)this->driver.holdCurrent)/0.31));
1027 Serial.println(F(" %"));
1028 }
1029
1030 /****************** Get PID Parameters ***********************
1031 * *
1032 * *
1033 **************************************************************/
1034 else if(cmd->substring(0,10) == String("parameters"))
1035 {
1036 if(cmd->charAt(10) != ';')
1037 {
1038 Serial.println("COMMAND NOT ACCEPTED");
1039 return;
1040 }
1041 Serial.print(F("P: "));
1042 Serial.print(this->dropinSettings.P.f,4);
1043 Serial.print(F(", "));
1044 Serial.print(F("I: "));
1045 Serial.print(this->dropinSettings.I.f,4);
1046 Serial.print(F(", "));
1047 Serial.print(F("D: "));
1048 Serial.println(this->dropinSettings.D.f,4);
1049 }
1050
1051 /****************** Help menu ********************************
1052 * *
1053 * *
1054 **************************************************************/
1055 else if(cmd->substring(0,4) == String("help"))
1056 {
1057 if(cmd->charAt(4) != ';')
1058 {
1059 Serial.println("COMMAND NOT ACCEPTED");
1060 return;
1061 }
1062 this->dropinPrintHelp();
1063 }
1064
1065/****************** SET run current ***************************
1066 * *
1067 * *
1068 **************************************************************/
1069 else if(cmd->substring(0,11) == String("runCurrent="))
1070 {
1071 for(i = 11;;i++)
1072 {
1073 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1074 {
1075 value.concat(cmd->charAt(i));
1076 }
1077 else if(cmd->charAt(i) == '.')
1078 {
1079 value.concat(cmd->charAt(i));
1080 i++;
1081 break;
1082 }
1083 else if(cmd->charAt(i) == ';')
1084 {
1085 break;
1086 }
1087 else
1088 {
1089 Serial.println("COMMAND NOT ACCEPTED");
1090 return;
1091 }
1092 }
1093
1094 for(;;i++)
1095 {
1096 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1097 {
1098 value.concat(cmd->charAt(i));
1099 }
1100 else if(cmd->charAt(i) == ';')
1101 {
1102 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1103 {
1104 i = (uint8_t)value.toFloat();
1105 break;
1106 }
1107 else
1108 {
1109 Serial.println("COMMAND NOT ACCEPTED");
1110 return;
1111 }
1112 }
1113 else
1114 {
1115 Serial.println("COMMAND NOT ACCEPTED");
1116 return;
1117 }
1118 }
1119
1120 Serial.print("COMMAND ACCEPTED. runCurrent = ");
1121 Serial.print(i);
1122 Serial.println(F(" %"));
1123 this->dropinSettings.runCurrent = i;
1124 this->saveDropinSettings();
1125 this->setCurrent(i);
1126 }
1127
1128 /****************** SET run current ***************************
1129 * *
1130 * *
1131 **************************************************************/
1132 else if(cmd->substring(0,12) == String("holdCurrent="))
1133 {
1134 for(i = 12;;i++)
1135 {
1136 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1137 {
1138 value.concat(cmd->charAt(i));
1139 }
1140 else if(cmd->charAt(i) == '.')
1141 {
1142 value.concat(cmd->charAt(i));
1143 i++;
1144 break;
1145 }
1146 else if(cmd->charAt(i) == ';')
1147 {
1148 break;
1149 }
1150 else
1151 {
1152 Serial.println("COMMAND NOT ACCEPTED");
1153 return;
1154 }
1155 }
1156
1157 for(;;i++)
1158 {
1159 if(cmd->charAt(i) >= '0' && cmd->charAt(i) <= '9')
1160 {
1161 value.concat(cmd->charAt(i));
1162 }
1163 else if(cmd->charAt(i) == ';')
1164 {
1165 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1166 {
1167 i = (uint8_t)value.toFloat();
1168 break;
1169 }
1170 else
1171 {
1172 Serial.println("COMMAND NOT ACCEPTED");
1173 return;
1174 }
1175 }
1176 else
1177 {
1178 Serial.println("COMMAND NOT ACCEPTED");
1179 return;
1180 }
1181 }
1182
1183 Serial.print("COMMAND ACCEPTED. holdCurrent = ");
1184 Serial.print(i);
1185 Serial.println(F(" %"));
1186 this->dropinSettings.holdCurrent = i;
1187 this->saveDropinSettings();
1188 this->setHoldCurrent(i);
1189 }
1190
1191 /****************** DEFAULT (Reject!) ************************
1192 * *
1193 * *
1194 **************************************************************/
1195 else
1196 {
1197 Serial.println("COMMAND NOT ACCEPTED");
1198 return;
1199 }
1200
1201}
1202
1204{
1205 static String stringInput;
1206 static uint32_t t = millis();
1207
1208 while(1)
1209 {
1210 while(!Serial.available())
1211 {
1212 delay(1);
1213 if((millis() - t) >= 500)
1214 {
1215 stringInput.remove(0);
1216 t = millis();
1217 }
1218 }
1219 t = millis();
1220 stringInput += (char)Serial.read();
1221 if(stringInput.lastIndexOf(';') > -1)
1222 {
1223 this->parseCommand(&stringInput);
1224 stringInput.remove(0);
1225 }
1226 }
1227}
1228
1230{
1231 Serial.println(F("uStepper S Dropin !"));
1232 Serial.println(F(""));
1233 Serial.println(F("Usage:"));
1234 Serial.println(F("Show this command list: 'help;'"));
1235 Serial.println(F("Get PID Parameters: 'parameters;'"));
1236 Serial.println(F("Set Proportional constant: 'P=10.002;'"));
1237 Serial.println(F("Set Integral constant: 'I=10.002;'"));
1238 Serial.println(F("Set Differential constant: 'D=10.002;'"));
1239 Serial.println(F("Invert Direction: 'invert;'"));
1240 Serial.println(F("Get Current PID Error: 'error;'"));
1241 Serial.println(F("Get Run/Hold Current Settings: 'current;'"));
1242 Serial.println(F("Set Run Current (percent): 'runCurrent=50.0;'"));
1243 Serial.println(F("Set Hold Current (percent): 'holdCurrent=50.0;'"));
1244 Serial.println(F(""));
1245 Serial.println(F(""));
1246}
1247
1249{
1250 dropinCliSettings_t tempSettings;
1251
1252 EEPROM.get(0,tempSettings);
1253
1254 if(this->dropinSettingsCalcChecksum(&tempSettings) != tempSettings.checksum)
1255 {
1256 return 0;
1257 }
1258
1259 this->dropinSettings = tempSettings;
1260 this->setProportional(this->dropinSettings.P.f);
1261 this->setIntegral(this->dropinSettings.I.f);
1262 this->setDifferential(this->dropinSettings.D.f);
1263 this->invertDropinDir((bool)this->dropinSettings.invert);
1264 this->setCurrent(this->dropinSettings.runCurrent);
1266 return 1;
1267}
1268
1270{
1272
1273 EEPROM.put(0,this->dropinSettings);
1274}
1275
1277{
1278 uint8_t i;
1279 uint8_t checksum = 0xAA;
1280 uint8_t *p = (uint8_t*)settings;
1281
1282 for(i=0; i < 15; i++)
1283 {
1284 checksum ^= *p++;
1285 }
1286
1287 return checksum;
1288}
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:1248
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:1269
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:669
volatile posFilter_t externalStepInputFilter
Definition: uStepperS.h:746
volatile float controlThreshold
Definition: uStepperS.h:760
float pid(float error)
Definition: uStepperS.cpp:712
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:798
void setDifferential(float D)
This method is used to change the PID differential parameter D.
Definition: uStepperS.cpp:788
void dropinCli()
This method is used to tune Drop-in parameters. After tuning uStepper S, the parameters are saved in ...
Definition: uStepperS.cpp:1203
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:778
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:783
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:652
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:659
void filterSpeedPos(posFilter_t *filter, int32_t steps)
Definition: uStepperS.cpp:527
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:563
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:641
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:645
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:1229
volatile float currentPidError
Definition: uStepperS.h:767
volatile bool shaftDir
Definition: uStepperS.h:779
uint8_t dropinSettingsCalcChecksum(dropinCliSettings_t *settings)
Definition: uStepperS.cpp:1276
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:793
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:664
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:707
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:563
void interrupt1(void)
Used by dropin feature to take in enable signal.
Definition: uStepperS.cpp:551
uStepperS * pointer
Definition: uStepperS.cpp:34
void TIMER1_COMPA_vect(void)
Definition: uStepperS.cpp:598
Function prototypes and definitions for the uStepper S library.
#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