hey
I wrote a library for my Closed-Loop stepper project.
It works with an ams rotary encoder attached to the rear of the shaft and an usual Nema 17 Steppermotor and an usual Stepperdriver.
I used a lot of AccelStepper Library code to do so, and it works fine. yet the costs are too high and thats why its a bit slower than the AccelStepper Lib (I guess the sensor readings are the problem here) and i wonder how i can overcome this issue. it starts to do weird ticking noises and work less fluently.
could i just use like a Teensy to solve my problem or do i have to adjust the code? because 16Mhz seems to be too less ^^
maybe you have some ideas to speed up my code. im not that good at programming so i ask you
here the relavent part of the .cpp code:
void ServoMotor::init() {
settings = SPISettings(10000000, MSBFIRST, SPI_MODE1);
pinMode(_cs, OUTPUT);
SPI.begin();
}
byte ServoMotor::spiCalcEvenParity(word value) {
byte cnt = 0;
byte i;
for (i = 0; i < 16; i++) {
if (value & 0x1)
{
cnt++;
}
value >>= 1;
}
return cnt & 0x1;
}
int ServoMotor::getStepsHR() {
rotationHR = (int)ServoMotor::read(AS5048A_ANGLE) * multForStepsHR;
if (rotationHR >= 0 && rotationHR <= 1000 && rotationHR_1 <= sprHR - 1 && rotationHR_1 >= sprHR - 1000)wrap_countHR++;
if (rotationHR_1 >= 0 && rotationHR_1 <= 1000 && rotationHR <= sprHR - 1 && rotationHR >= sprHR - 1000)wrap_countHR--;
stepsHR = rotationHR + (sprHR * wrap_countHR);
rotationHR_1 = rotationHR;
return stepsHR;
}
int ServoMotor::getStepsAbsolut() {
rotationAbs = (int)ServoMotor::read(AS5048A_ANGLE) * multForSteps;
if (rotationAbs >= 0 && rotationAbs <= 10 && rotationAbs_1 <= spr - 1 && rotationAbs_1 >= spr - 10)wrap_countAbs++;
if (rotationAbs_1 >= 0 && rotationAbs_1 <= 10 && rotationAbs <= spr - 1 && rotationAbs >= spr - 10)wrap_countAbs--;
stepsAbs = rotationAbs + (spr * wrap_countAbs);
rotationAbs_1 = rotationAbs;
return stepsAbs;
}
int ServoMotor::getStepCounter() {
rotationC = (int)ServoMotor::read(AS5048A_ANGLE) * multForSteps;
if (rotationC >= 0 && rotationC <= 10 && rotationC_1 <= spr - 1 && rotationC_1 >= spr - 10) wrap_countC++;
if (rotationC_1 >= 0 && rotationC_1 <= 10 && rotationC <= spr - 1 && rotationC >= spr - 10) wrap_countC--;
stepsC = rotationC + (spr * wrap_countC);
rotationC_1 = rotationC;
return -(stepsC - position);
}
void ServoMotor::setZeroPosition() {
position = ServoMotor::getStepCounter();
_targetPos = _currentPos = 0;
_n = 0;
_stepInterval = 0;
_speed = 0.0;
}
word ServoMotor::read(word registerAddress) {
word command = 0b0100000000000000;
command = command | registerAddress;
command |= ((word)spiCalcEvenParity(command) << 15);
byte right_byte = command & 0xFF;
byte left_byte = ( command >> 8) & 0xFF;
SPI.beginTransaction(settings);
digitalWrite(_cs, LOW);
SPI.transfer(left_byte);
SPI.transfer(right_byte);
digitalWrite(_cs, HIGH);
digitalWrite(_cs, LOW);
left_byte = SPI.transfer(0x00);
right_byte = SPI.transfer(0x00);
digitalWrite(_cs, HIGH);
SPI.endTransaction();
return (( ( left_byte & 0xFF ) << 8 ) | ( right_byte & 0xFF )) & ~0xC000;
}
void ServoMotor::moveTo(long absolute)
{
if (_targetPos != absolute)
{
_targetPos = absolute;
computeNewSpeed();
}
}
void ServoMotor::move(long relative)
{
moveTo(_currentPos + relative);
}
void ServoMotor::stallDetection() {
unsigned long zeit = millis();
if (zeit - _lastStepTime2 >= dt) {
_currentPos = ServoMotor::getStepCounter();
y = ServoMotor::getStepsHR();
dV = (yw - y) - (_stepCounter-_stepCounter_1);
if (abs(dV) > 40 && abs(dV2) > 40)stallDetect = true;
else stallDetect = false;
_lastStepTime2 = zeit;
dV2 = dV;
yw = y;
_stepCounter_1=_stepCounter;
_stepCounter=0;
}
}
boolean ServoMotor::runSpeed()
{
if (!_stepInterval)
return false;
unsigned long time = micros();
if (time - _lastStepTime >= _stepInterval)
{
if (_direction == DIRECTION_CW)
{
_stepCounter++;
}
else
{
_stepCounter--;
}
step(_currentPos);
_lastStepTime = time;
return true;
}
else
{
return false;
}
}
long ServoMotor::distanceToGo()
{
return _targetPos - _currentPos;
}
void ServoMotor::computeNewSpeed()
{
long distanceTo = distanceToGo();
long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration));
_currentPos = ServoMotor::getStepCounter();
if (distanceTo == 0 && stepsToStop <= 1)
{
_stepInterval = 0;
_speed = 0.0;
_n = 0;
return;
}
if (distanceTo > 0)
{
// We are anticlockwise from the target
// Need to go clockwise from here, maybe decelerate now
if (_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW)
_n = -stepsToStop; // Start deceleration
}
else if (_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW)
_n = -_n; // Start accceleration
}
}
else if (distanceTo < 0)
{
// We are clockwise from the target
// Need to go anticlockwise from here, maybe decelerate
if (_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW)
_n = -stepsToStop; // Start deceleration
}
else if (_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW)
_n = -_n; // Start accceleration
}
}
// Need to accelerate or decelerate
if (_n == 0)
{
// First step from stopped
_cn = _c0;
_direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
}
else
{
// Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
_cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
_cn = max(_cn, _cmin);
}
_n++;
_stepInterval = _cn;
_speed = 1000000.0 / _cn;
if (_direction == DIRECTION_CCW)
_speed = -_speed;
}
boolean ServoMotor::run()
{
//ServoMotor::stallDetection();
if (distanceToGo() != 0) {
runSpeed();
computeNewSpeed();
}
computeNewSpeed();
return _speed != 0.0 || distanceToGo() != 0;
}
Theres SPI stuff going on and I have to use millis() and micros() for timing, maybe there are better solutions
thank you all