NotesWhat is notes.io?

Notes brand slogan

Notes - notes.io

// 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;
// Up variables
double KI_up, KP_up, KD_up;
double yI_up, yP_up, yD_up;
double integ_error_up, deriv_error_up;
double previous_error_up = 0;
// Down variables
double KI_down, KP_down, KD_down;
double yI_down, yP_down, yD_down;
double integ_error_down, deriv_error_down;
double previous_error_down = 0;


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

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()
{
// measurement errors
if(position_up > maxHeight || position_up < minHeight || position_down > maxHeight || position_down < minHeight)
{
Serial.println("measurement error");
time_up = time_up_init;
time_down = time_down_init;
return;
}

float error_up = up_setpoint - position_up;
float error_down = -(down_setpoint - position_down);

// add your controller code here for adjusting time_up and time_down
// based on error_up and error_down etc.

KI_up = 10;
KP_up = 5;
KD_up = 1;

integ_error_up += error_up;
deriv_error_up = error_up - previous_error_up;

yI_up = integ_error_up * KI_up;
yP_up = error_up * KP_up;
yD_up = deriv_error_up * KD_up;

time_up = yI_up + yP_up + yD_up;

previous_error_up = error_up;

// Time down

KI_down = 3;
KP_down = 1;
KD_down = 1;

integ_error_down += error_down;
deriv_error_down = error_down - previous_error_down;

yI_down = integ_error_down * KI_down;
yP_down = error_down * KP_down;
yD_down = deriv_error_down * KD_down;

time_down = yI_down + yP_down + yD_down;

previous_error_down = error_down;
//Ki, Kp, Kd


// time_down = ...

if(time_up < 1)
time_up = 1;
if(time_down < 1)
time_down = 1;
}

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);
}
     
 
what is notes.io
 

Notes.io is a web-based application for taking notes. You can take your notes and share with others people. If you like taking long notes, notes.io is designed for you. To date, over 8,000,000,000 notes created and continuing...

With notes.io;

  • * You can take a note from anywhere and any device with internet connection.
  • * You can share the notes in social platforms (YouTube, Facebook, Twitter, instagram etc.).
  • * You can quickly share your contents without website, blog and e-mail.
  • * You don't need to create any Account to share a note. As you wish you can use quick, easy and best shortened notes with sms, websites, e-mail, or messaging services (WhatsApp, iMessage, Telegram, Signal).
  • * Notes.io has fabulous infrastructure design for a short link and allows you to share the note as an easy and understandable link.

Fast: Notes.io is built for speed and performance. You can take a notes quickly and browse your archive.

Easy: Notes.io doesn’t require installation. Just write and share note!

Short: Notes.io’s url just 8 character. You’ll get shorten link of your note when you want to share. (Ex: notes.io/q )

Free: Notes.io works for 12 years and has been free since the day it was started.


You immediately create your first note and start sharing with the ones you wish. If you want to contact us, you can use the following communication channels;


Email: [email protected]

Twitter: http://twitter.com/notesio

Instagram: http://instagram.com/notes.io

Facebook: http://facebook.com/notesio



Regards;
Notes.io Team

     
 
Shortened Note Link
 
 
Looding Image
 
     
 
Long File
 
 

For written notes was greater than 18KB Unable to shorten.

To be smaller than 18KB, please organize your notes, or sign in.