#include <PID_v1.h>
#define ACCELERATION_TARGET_SPEED 120 // PWM
#define DECELERATION_TARGET1_SPEED 60 // PWM
#define DECELERATION_TARGET2_SPEED 0 // PWM
#define MOTOR_CONTROL_PIN 25 // pwm pin
#define MOTOR_ENCODER_FEEDBACK_PIN 34 // encoder feedback pin
double Kp = 2.0, Ki = 5.0, Kd = 1.0; // PID parameters
double target = 60; // set desired pwm speed
double input = 0; // current speed
double output = 0; // control output
// Create PID controller
PID motorPID(&input, &output, &target, Kp, Ki, Kd, DIRECT);
unsigned long lastUpdate = 0;
unsigned long lastUpdate1 = 0;
void setup() {
Serial.begin(115200);
pinMode(MOTOR_CONTROL_PIN, OUTPUT);
pinMode(MOTOR_ENCODER_FEEDBACK_PIN, INPUT);
motorPID.SetMode(AUTOMATIC); // Initialize PID controller
motorPID.SetOutputLimits(0, 255); // Output range for motor speed
lastUpdate = millis();
}
// double Kp = 5.0, Ki = 2.0, Kd = 0.5;
// 41636
// 40000
void loop() {
if (millis() - lastUpdate1 > 100) {
setAcceleration();
lastUpdate1 = millis();
}
}
void setAcceleration() {
int input1 = analogRead(MOTOR_ENCODER_FEEDBACK_PIN);
input = map(input1, 0, 4095, 0, 255);
motorPID.Compute();
int output1 = map(output, 0, 255, 0, target);
analogWrite(MOTOR_CONTROL_PIN, output1);
Serial.print(input);
Serial.print(" | ");
Serial.print(output);
Serial.print(" | ");
Serial.println(output1);
if (output1 >= 60) {
Serial.println("reached!");
lastUpdate = millis() - lastUpdate;
Serial.println(lastUpdate);
}
}