Notes

// implemented and tested with Arduino IDE 2.0.3 and 2.1.0
// requires Adafruit Motor shield library
//
// Arduino Uno R3 clone by AZ delivery
// AZ Delivery L293D motor shield
// ultrasonic distance sensor HC-SR04 by AZ Delivery
// load weight: 6g, 13x13cm


// for motor shield
#include <AFMotor.h>
AF_DCMotor motor(4);

// for ultrasonic distance sensor
const int echoPin = 2;
const int trigPin = 3;
long duration;
float distance;

enum Direction {UP, DOWN};

float position_down; // in cm
float position_up; // in cm
float up_setpoint = 40; // in cm
float down_setpoint = 25; // in cm

int time_up_init = 130; //in milliseconds
int time_down_init = 170; //in milliseconds
float maxHeight = 60; // in cm (higher values are treated as measurement errors)
float minHeight = 8; // in cm (lower values are treated as measurement errors)

int time_up = time_up_init; //in milliseconds
int time_down = time_down_init; //in milliseconds
int speed_up = 30; // pwm ration 0 to 255
int speed_down = 1; // pwm ration 0 to 255

float lastDistanceUp = 0; // in cm
float lastDistanceDown = 0; // in cm

// PID parameters
float Kp = 5.0;
float Ki = 4.0;
float Kd = 0.5;

// Initialize PID variables
float integralUp = 0, integralDown = 0;
float previous_error_up = 0, previous_error_down = 0;

void setup() {
Serial.begin(9600); // set up Serial library at 9600 bps

Serial.println("---");
Serial.println("---");
Serial.println("---");
Serial.println("Arduino resarted");

// for ultrasonic distance sensor
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);

motor.run(RELEASE);

position_down = measureDistance();

// CSV header
Serial.println("position up;position down;distance up;distance down;time up;time down;speed up;speed down");
}

void loop()
{
// move up
move(UP, time_up, speed_up);

// wait a second...
delay(8000);

position_up = measureDistance();
lastDistanceUp = position_up - position_down;

// move down
move(DOWN, time_down, speed_down);

// wait a second...
delay(8000);
position_down = measureDistance();
lastDistanceDown = position_up - position_down;

controlParameters();

printCycleInfos();
}

void move(Direction direction, int time, int speed)
{
if(direction == UP)
{
motor.run(FORWARD);
}
else
{
motor.run(BACKWARD);
}

motor.setSpeed(speed);
delay(time);

// stop downward movement by inverse action
if(direction == DOWN)
{
motor.run(FORWARD);
motor.setSpeed(40);
delay(time/2);
}

// stop motor
motor.run(RELEASE);

}

void controlParameters() {
// Calculate error
float error_up = up_setpoint - position_up;
float error_down = down_setpoint - position_down;

// Update integral
integralUp += error_up;
integralDown += error_down;

// Calculate derivative
float derivative_up = error_up - previous_error_up;
float derivative_down = error_down - previous_error_down;

// Compute PID output
float output_up = Kp * error_up + Ki * integralUp + Kd * derivative_up;
float output_down = Kp * error_down + Ki * integralDown + Kd * derivative_down;

// Update previous error
previous_error_up = error_up;
previous_error_down = error_down;

// Use PID output to adjust times
time_up = constrain(time_up_init + (int)output_up, 1, 1000);
// time_down = constrain(time_down_init + (int)output_down, 1, 1000);

// Safety checks for measurement errors
if (position_up > maxHeight || position_up < minHeight || position_down > maxHeight || position_down < minHeight) {
Serial.println("measurement error");
integralUp = integralDown = 0; // Reset integral term on error
previous_error_up = previous_error_down = 0; // Reset derivative term on error
time_up = time_up_init;
time_down = time_down_init;
}
}

float measureDistance()
{
int before = millis();
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
int after = millis();
//Serial.print("measurement duration: ");
//Serial.println(after - before);
distance = duration * 0.034 / 2;
return distance;
}

void printCycleInfos()
{
Serial.print(position_up);
Serial.print(";");
Serial.print(position_down);
Serial.print("; ");
Serial.print(lastDistanceUp);
Serial.print(";");
Serial.print(lastDistanceDown);
Serial.print("; ");
Serial.print(time_up);
Serial.print(";");
Serial.print(time_down);
Serial.print("; ");
Serial.print(speed_up);
Serial.print(";");
Serial.println(speed_down);
}