pid thing

//this is not the whole thing, too much stuff happening just to make the ARM processor run



uint32_t lastTime, lastFpsTime, delayFps = 0;
int fps = 200, sampleTime=100, targetFps = 125, outMax = 1000, outMin = -1000;
double lastInput, ITerm;
double kp = 5;
double ki = 0.1;
double kd = 1;


void initPID(){
  ITerm = delayFps;
  lastInput = fps;
  if(ITerm > outMax) ITerm = outMax; //there's a special place in hell for this type of coding
  else if(ITerm < outMin) ITerm = outMin;
  lastTime = HAL_GetTick() - sampleTime;
}

int calcPID(double input){
  uint32_t now = HAL_GetTick();
  uint32_t deltaT = now - lastTime;
  if(deltaT > sampleTime){
    double error = targetFps - input;

    ITerm+= (ki * sampleTime / 1000.0 * error);

    if(ITerm > outMax) ITerm= outMax;
    else if(ITerm < outMin) ITerm= outMin;

    double dInput = (input - lastInput);

    int result = kp * error + ITerm- kd  / sampleTime / 1000 * dInput;

    if(result > outMax) result = outMax;
    else if(result < outMin) result = outMin;

    lastTime = HAL_GetTick();

    return result;
  }
  return 0;
}


void main(void)
{
  initPID();
  lastFpsTime = HAL_GetTick();
  int frameCounter = 0;

  while (1)
  {
    //cumpute the extremities of the ball: left, right, top, bottom
    static int marginX, marginY;
    marginX = circleX + directionX + directionX*circleR;
    marginY = circleY + directionY + directionY*circleR;


    //between moving a pixel in any direction, delay for "delayFPS" * "whatever time it takes to compute a random number, a division and a sum" (rand()%1000+1000)
    for(int i=0;i<=delayFps;i++)
      const int gretfd = rand()%1000+1000;
    //after the delay clear the current circle and eventually draw the next circle at the new position
    display.fillFastCircle(circleX, circleY, circleR, BLACK);

    //check if the ball has hit one of the margins; if it's at a margin, change the movement direction
    if(marginX == 0 || marginX == 127 || (circleY <= 13 && marginX <= 20)){
      directionX *= -1;
      fill = !fill;
    }
    if(marginY == 0 || marginY == 31 || (circleX <= 23 && marginY <= 10)){
      directionY *= -1;
      fill = !fill;
    }
    circleX += directionX;

    if(divideY > divThreshold){
      circleY += directionY;
      divideY = 0;
      divThreshold = rand() % 4;
    }
    else{
      divideY++;
    }

    //draw the circle at the new position
    if(fill)
      display.fillFastCircle(circleX, circleY, circleR, WHITE);
    else
      display.drawFastCircle(circleX, circleY, circleR, WHITE);

    //compute number of pixels the ball will travel in a second
    fps = 1000 / (HAL_GetTick() - lastFpsTime); //HAL_GetTick() returns how many milliseconds have passed since the program began running
    if(fps>=1000){
      fps = 999;
    }
    lastFpsTime = HAL_GetTick();

    //once every 80 frames interrupt everything and update the on-screen fps counter
    if(frameCounter >= 80){
      HAL_SuspendTick(); //suspends the tick counter
      char buf[5] = "";
      itoa(fps, buf, 10);
      drawFPSFrame();
      display.print(buf,2,2,1,WHITE,BLACK);
      frameCounter = 0;
      HAL_ResumeTick(); //resumes the tick counter
    }
    else{
      frameCounter++;
    }
    //adjust the delay to achieve the desired pixels per second movement speed of the ball
    delayFps -= calcPID(fps);
  }
}