Measure ramping calculation (arduino)

calculation.ino

#include <stdio.h>
#include <stdint.h>
#include <math.h>
#define ramplen(oo,v0,v1,a ,stepmm) oo=(v1*v1-v0*v0)* stepmm/(2*a);
#define stepmm 50


uint16_t int_inv_sqrt(uint16_t a) {
  /// 16bits inverse (much faster than doing a full 32bits inverse)
  /// the 0xFFFFU instead of 0x10000UL hack allows using 16bits and 8bits
  /// variable for the first 8 steps without overflowing and it seems to
  /// give better results for the ramping equation too :)
  uint8_t z = 0, i;
  uint16_t x, j;
  uint32_t q = ((uint32_t)(0xFFFFU / a)) << 8;

  for (i = 0x80; i; i >>= 1) {
    uint16_t y;

    z |= i;
    y = (uint16_t)z * z;
    if (y > (q >> 8))
      z ^= i;
  }

  x = z << 4;
  for (j = 0x8; j; j >>= 1) {
    uint32_t y;

    x |= j;
    y = (uint32_t)x * x;
    if (y > q)
      x ^= j;
  }

  return x;
}


float InvSqrt(float x) {

  int32_t* i = (int32_t*)&x;           // store floating-point bits in integer
  *i = 0x5f335a86  - (*i >> 1);    // initial guess for Newton's method
  return x;
}
void setup() {
  Serial.begin(115200);
}
float fe = 100;
float fs = 0;
float a = 200;
long totalstep;
  long dl;

void demo0() {
  ramplen(totalstep, fs, fe, a, stepmm);
  long f = fs;

  long ta = (fe * fe - fs * fs);
  long acx = ta / totalstep;

  Serial.println("\nStd Sqrt loop:\n");
  Serial.print("Totalstep:");
  Serial.println(totalstep);
  Serial.print("TA:");
  Serial.println(ta);
  Serial.print("ACC:");
  Serial.println(a);
  Serial.print("Target F:");
  Serial.println(fe);
  long m = micros();
  long ts = totalstep;
  ta = fs * fs;
  dl = 1000000;
  float stepdiv = 1000000.f / stepmm;

  while (totalstep--) {
    f = dl + micros();
    ta += acx;
    if (ta > 1)dl = stepdiv / sqrt(ta); else dl = 1000000;
  }
  Serial.println(f - micros());
  f = stepdiv / dl;
  m = micros() - m;
  Serial.print("Final F:");
  Serial.println(f);
  Serial.print("Time/step (us):");
  Serial.println(m / ts);

}

void demo1() {
  ramplen(totalstep, fs, fe, a, stepmm);
  long f = fs;

  long ta = (fe * fe - fs * fs);
  long acx = ta / totalstep;

  Serial.println("\nFast InvSqrt loop:\n");
  Serial.print("Totalstep:");
  Serial.println(totalstep);
  Serial.print("TA:");
  Serial.println(ta);
  Serial.print("ACC:");
  Serial.println(acx);
  Serial.print("Target F:");
  Serial.println(fe);
  long m = micros();
  long ts = totalstep;
  ta = fs * fs;
  dl = 1000000;
  float stepdiv = 1000000.f / stepmm;

  while (totalstep--) {

    f = dl + micros();
    ta += acx;
    if (ta > 1)dl = stepdiv * InvSqrt(ta); else dl = 1000000;
  }
  Serial.println(f - micros());
  f = stepdiv / dl;
  m = micros() - m;
  Serial.print("Final F:");
  Serial.println(f);
  Serial.print("Time/step (us):");
  Serial.println(m / ts);

}



void demo2() {
   ramplen(totalstep, fs, fe, a, stepmm);
  long f = fs;

  long ta = (fe * fe - fs * fs);
  long acx = ta / totalstep;

  Serial.println("\nTeacup F loop:\n");
  Serial.print("Totalstep:");
  Serial.println(totalstep);
  Serial.print("TA:");
  Serial.println(ta);
  Serial.print("ACC:");
  Serial.println(acx);
  Serial.print("Target F:");
  Serial.println(fe);
  long m = micros();
  long ts = totalstep;
  ta = ts;
  uint32_t c0p = (uint32_t)(16000000.f / sqrt((float)stepmm * acx / 2000.));

  while (totalstep--) {
    f = dl + micros();
    ta --;
    if (ta > 0)dl = uint32_t(c0p * int_inv_sqrt(ta)) >> 13; // else dl = c0p;
  }
  Serial.println(f - micros());
  f = 16000000 / dl;
  m = micros() - m;
  Serial.print("Final F:");
  Serial.println(f);
  Serial.print("Time/step (us):");  
  Serial.println(m / ts);

}

void loop() {
  // put your main code here, to run repeatedly:
  Serial.println("--------------------------------------------------");
  demo0();
  demo1();
  demo2();
  delay(3000);

}