speeding up my code

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. :confused:

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 :slight_smile:

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

faster parity code as it stops when no bits are set anymore, not extensively tested
(my gain was 25% but I have no representative test)

uint8_t evenParity(word value) {
  uint8_t par = 0;
  while (value)
  {
    if (value & 0x1) par++;
    value >>= 1;
  }
  return par & 0x01;
}

Do you have any idea which code part consumes the most time?
Did you make measurements?

This line in read can also be made faster

command |= ((word)spiCalcEvenParity(command) << 15); // always shift of 15 places and an OR

==>

if (spiCalcEvenParity(command) ) command |= (0x8000); // only when parity == 1 it does something simpler.

Won't speed up a factor two but all bits help.
Have you tried to set SPIbus to 16000000 ?

probably the sensor reading. i havent tested it extensivly but without the sensor it works fine, and the more often i read the sensor the slower it gets. i gonna try ur code snipped for the evenParity thanks.
yet i have to read it pretty often to make my program work^^

i dont think integer calculations slow it down too much or do they?

i wonder if its slow to use the arduino method millis() or micros() or if the integer or longs can overflow and slow the program down even more... :confused:
or if digitalWrite is maybe too slow...
Serial slows the program down a lot too

thanks for your reply

RWTH_MASCHI:
i dont think integer calculations slow it down too much or do they?

if you use a lot of them they add up. A good strategy of optimizing speed is to trade it for memory.
This can be done if you have a part that is calculated multiple times

e.g. supose you have 2 formulas

a = (b+c) / (d*e);

f = (g + h) / (d * e);

than it makes sense to define a temp var q = (d * e);


Another when doing math is to know that multiplication is faster than division.

float pi = 3.14159;
float x = y / 10.0; is slower than
float z = y * 0.1;

RWTH_MASCHI:
i wonder if its slow to use the arduino method millis() or micros() or if the integer or longs can overflow and slow the program down even more... :confused:
or if digitalWrite is maybe too slow...
Serial slows the program down a lot too

a call to millis() or micros() takes ~4 usec so not that much but these also can add up.

Serial slows things down, especially converting numbers to text tales time (in the past we did some optimizations) but they never made the default libs. That said you can always optimize the baudrate,
sending at 9600 baud is way slower than 230400 baud. Keeping error messages short also helps.

digitalWrite is relative slow as it uses a var as parameter. If you know the pin at compile time You can optimize these cals substatially. Google FastDigitalWrite of "Direct Port Manipulation"

Wrt the read function

word ServoMotor::read(word registerAddress) 
{
  word command = 0b0100000000000000 | registerAddress;

   if (spiCalcEvenParity(command) command |= 0x8000;

  byte right_byte = command & 0xFF;
  byte left_byte = command >> 8;

  // SPI.beginTransaction(settings);  move this line to setup or so, so it is done only once

  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);


  return (( left_byte & 0x3F ) << 8 ) | ( right_byte & 0xFF );  // mini optimization
}

found an even faster parity - a builtin one...

byte evenParity(word value) { return __builtin_popcount (value) & 1; }

about 40% faster in my testsketch

expressions like these can also be interesting to optimize

if (rotationHR >= 0 && rotationHR <= 1000 && rotationHR_1 <= sprHR - 1 && rotationHR_1 >= sprHR - 1000) wrap_countHR++;

assuming that rotation is always poitive this could be

if (rotationHR <= 1000 && rotationHR_1 <= sprHR - 1 && rotationHR_1 >= sprHR - 1000) wrap_countHR++;

then rotationHR_1 <= sprHR - 1 can be micro optimized

if (rotationHR <= 1000 && rotationHR_1 < sprHR && rotationHR_1 >= sprHR - 1000) wrap_countHR++;

furthermore yoou should put the part of the expression that fails most in the begin of the expression, so

if (rotationHR_1 < sprHR && rotationHR <= 1000 && rotationHR_1 >= sprHR - 1000) wrap_countHR++;

could be faster.

Remember, code not executed is the fastest :slight_smile:

this will work as u wrote it?

nice. I researched a bit and i think i just gonna use digitalWriteFast library.
I dont know too much about registers and stuff... think thatll still help out :slight_smile:

I went and changed my sketch as u adviced and it allready makes progress.

Thanks a lot. :slight_smile:

but what would happen if i use a teensy with 120Mhz?

No problems at all? or still slow if I dont change the code?

In my final programm i gonna use one I guess, ( I need it anyway because it has many many pins)

boolean ServoMotor::run()

{
  //ServoMotor::stallDetection();
  if (distanceToGo() != 0) {
    runSpeed();
    computeNewSpeed();
  }
  computeNewSpeed();
  return _speed != 0.0 || distanceToGo() != 0;
}

I think computeNewSpeed() is a relatively expensive function. There's a lot of floating-point math there. The code above calls it twice and I think that will give you the same answer on the second call (since the code hasn't taken any steps in between.)

Additionally, repeated calls to runSpeed() won't always take any steps on the motor, so that's going to waste time computing a speed which doesn't change.

Then try to call it even less often - maybe you can take two steps at constant speed before computing a new speed?

But until you show the entire program or do some measurements for yourself, this is just a guess. There may be other time-consuming code elsewhere in your code.

oh I didnt even notice that thanks.

the rest of the code are just funktions that wont get called often so its doesnt really matter.

now I found this digitalWriteFast library, but i have to set the pins to constants or #define them to make it workwork, this would make the digitalWrite around 60 times (!!!!) faster

how do i do that? if i want _cs to be a constant. it seems that i cant set them in the constructer :confused: