wiki.raudys.com

motor PID controler

Speed 20 (easy)

Speed 10 (hard)

non stable PID (depends on PID params):

https://www.youtube.com/watch?v=ng2PVOePN68

#include <Encoder.h>
#include <driver/dac.h>
 
Encoder encoder(19, 16);
 
void setup() {
  Serial.begin(115200);
  Serial.println("start");
 
  //  dac_output_enable(DAC_CHANNEL_1);
  ledcSetup(1, 5000, 8);
  ledcAttachPin(25, 1);
}
 
 
 
float motor_speed_dac = 0;
float speed_step = 1;
void loop() {
 
 
  //  dac_output_voltage(DAC_CHANNEL_1, int(motor_speed_dac));
  ledcWrite(1, motor_speed_dac);
 
  //if (motor_speed_dac == 255 or motor_speed_dac == 0) speed_step = -speed_step;
 
  //Serial.printf("%f\n", motor_speed_dac);
 
 
  auto pos = encoder.read();
  static auto prev_pos = pos;
 
  auto newtime = micros();
  static auto prev_newtime = newtime;
 
  float motor_speed = ((pos - prev_pos) / 44.0) / (newtime - prev_newtime) * 1000000.0;
 
  if (isnan(motor_speed)) motor_speed = 0;
//  if (motor_speed > 60) motor_speed = 60;
 
  float motor_speed_error = 20-motor_speed;
  static float motor_speed_error_prev = motor_speed_error;
  float motor_speed_error_diff = motor_speed_error_prev - motor_speed_error;
  float p_coef = 0.5;
  float i_coef = 0.01;
  float d_coef = 0.01;
  static float sum_dif = 0;
  sum_dif += motor_speed_error;
 
  motor_speed_dac += motor_speed_error * p_coef + sum_dif * i_coef + motor_speed_error_diff * d_coef;
 
  if (motor_speed_dac > 255) motor_speed_dac = 255;
  if (motor_speed_dac < 0) motor_speed_dac = 0;
 
//  if (motor_speed > 10) motor_speed_dac += -1;
//  if (motor_speed < 10) motor_speed_dac +=  1;
 
  delay(100);
  Serial.printf("%f,%f,%f,%f\n", motor_speed, motor_speed_dac, motor_speed_error, sum_dif);
 
  prev_newtime = newtime;
  prev_pos = pos;
 
  motor_speed_error_prev = motor_speed_error;
 
}