set_speed.ino

unsigned long last_time;

float set_speed(float omega_input){
  // time handler
  unsigned long now = millis();
  double timeChange = (double)(now - last_time);

  // PID Setup
  float omega_read_encoder = read_encoder_speed(); // set input or reading value
  error = omega_input - omega_read_encoder; // error = setpoint - input

  // PID handler
  integral += (error * timeChange);
  derivative = (error - last_error) / timeChange;
  proportional = error;
  mv = Kp * proportional + Ki * integral + Kd * derivative;

  // Set last
  last_error = error;
  last_time = now

  // Set ceil and floor
  if (mv > 255) mv = 255;
  if (mv < -255) mv = -255;

  motor_drive(mv);

}