Thanks to visit codestin.com
Credit goes to www.scribd.com

Scribd
Upload
213 views2 pages

Balanza Codigp

This document contains code for a PID controller that uses an ultrasonic sensor to control the position of a servo motor. It defines variables for reading sensor input, calculating distance and PID values, and controlling the servo motor to maintain a target distance. Functions are included to setup the hardware, run the control loop periodically, and calculate distance from the sensor readings.

Uploaded by

gitselsarahi.espinoza
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
213 views2 pages

Balanza Codigp

This document contains code for a PID controller that uses an ultrasonic sensor to control the position of a servo motor. It defines variables for reading sensor input, calculating distance and PID values, and controlling the servo motor to maintain a target distance. Functions are included to setup the hardware, run the control loop periodically, and calculate distance from the sensor readings.

Uploaded by

gitselsarahi.espinoza
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 2

#include <Wire.

h>
#include <Servo.h>

///////////////////////Inputs/outputs///////////////////////
int Analog_in = A0;
Servo myservo; // create servo object to control a servo, later attatched to D9
///////////////////////////////////////////////////////

////////////////////////Variables///////////////////////
int Read = 0;
float distance = 0.0;
float elapsedTime, time, timePrev; //Variables para control del tiempo
float distance_previous_error, distance_error;
int period = 50; //Refresh rate period of the loop is 50ms
///////////////////////////////////////////////////////

///////////////////PID constants///////////////////////
float kp=8; //Mine was 8 control proporcional
float ki=0.2; //Mine was 0.2 control integral
float kd=3100; //Mine was 3100 control derivativo
float distance_setpoint = 21; //Should be the distance from sensor to the
middle of the bar in mm
float PID_p, PID_i, PID_d, PID_total;
///////////////////////////////////////////////////////

void setup() {
//analogReference(EXTERNAL);
Serial.begin(9600);
myservo.attach(9); // PIn 9 del servo al objeto (barra)
myservo.write(125); // colocamos el servo en el angulo 125 para que el equilibrio
este en medio
pinMode(Analog_in,INPUT);
time = millis();
}

void loop() {
if (millis() > time+period)
{
time = millis();
distance = get_dist(100);
distance_error = distance_setpoint - distance;
PID_p = kp * distance_error;
float dist_diference = distance_error - distance_previous_error;
PID_d = kd*((distance_error - distance_previous_error)/period);

if(-3 < distance_error && distance_error < 3)


{
PID_i = PID_i + (ki * distance_error);
}
else
{
PID_i = 0;
}
PID_total = PID_p + PID_i + PID_d;
PID_total = map(PID_total, -150, 150, 0, 150);

if(PID_total < 20){PID_total = 20;}


if(PID_total > 160) {PID_total = 160; }

myservo.write(PID_total+30);
distance_previous_error = distance_error;
}
}

float get_dist(int n)
{
long sum=0;
for(int i=0;i<n;i++)
{
sum=sum+analogRead(Analog_in);
}
float adc=sum/n;
//float volts = analogRead(adc)*0.0048828125; // value from sensor * (5/1024)
//float volts = sum*0.003222656; // value from sensor * (3.3/1024) EXTERNAL
analog refference

float distance_cm = 17569.7 * pow(adc, -1.2062);


//float distance_cm = 13*pow(volts, -1);
return(distance_cm);
}

You might also like