Scribd
Cargar
106 vistas4 páginas

Arduino - Control Proporcional

Este documento presenta un programa PID para controlar la posición de un vehículo mediante la lectura de valores de aceleración de una IMU y el envío de señales PWM a los motores. Inicializa las variables, configura la comunicación I2C con la IMU, y en el bucle principal lee los datos, calcula el error, la señal de control y envía los valores PWM a los motores para mantener la posición referenciada.

Cargado por

LuCaz TJ
Derechos de autor
© © All Rights Reserved
Nos tomamos en serio los derechos de los contenidos. Si sospechas que se trata de tu contenido, reclámalo aquí.
Formatos disponibles
Descarga como DOCX, PDF, TXT o lee en línea desde Scribd
106 vistas4 páginas

Arduino - Control Proporcional

Este documento presenta un programa PID para controlar la posición de un vehículo mediante la lectura de valores de aceleración de una IMU y el envío de señales PWM a los motores. Inicializa las variables, configura la comunicación I2C con la IMU, y en el bucle principal lee los datos, calcula el error, la señal de control y envía los valores PWM a los motores para mantener la posición referenciada.

Cargado por

LuCaz TJ
Derechos de autor
© © All Rights Reserved
Nos tomamos en serio los derechos de los contenidos. Si sospechas que se trata de tu contenido, reclámalo aquí.
Formatos disponibles
Descarga como DOCX, PDF, TXT o lee en línea desde Scribd

// programa PID desde cero

#include <Wire.h>

//Direccion I2C de la IMU


#define MPU 0x68

//MPU-6050 da los valores en enteros de 16 bits


//Valores sin refinar
int16_t AcX;

int TiempoMuestreo=1;
milisegundos

// tiempo de muestreo Se encuentra en

unsigned long pasado=0;


// tiempo pasado (Se hace para
asegurar tiempo de muestreo)
unsigned long ahora;
float Ref=-0.2;
double Y;
double error;
double U;

// referencia
// Salida
// error
// Seal control

// constantes del controlador


int Kp=2000;

int llantaI=10;

// llanta Izquierda

int llantaD=11;

// llanta derecha

void setup()
{
Wire.begin();

Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
}

void loop() {

//Leer los valores del Acelerometro de la IMU


Wire.beginTransmission(MPU);
Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); //A partir del 0x3B, se piden 6
registros
AcX=Wire.read()<<8|Wire.read();

Y=float(AcX)/17000;

ahora=millis();

//Cada valor ocupa 2 registros

// medicin de la planta

// tiempo actual

int CambioTiempo= ahora-pasado;


actual- pasado

// diferencia de tiempo

if(CambioTiempo >= TiempoMuestreo)


muestreo

// si se supera el tiempo de

{
error = Ref - Y;

// error (Lazo cerrado)-- Feedback

U=Kp*error;

// Seal de control

pasado=ahora;

// actualizar tiempo

if (U < -255)

// lmites de saturacin de la seal de control

{
U=-255;
}
if (U > 255)

// lmites de saturacin de la seal de control

{
U=255;
}

int atras=0;
int adelante=0;

if (U>0){
adelante=U;
// PWM de los motores atras y adelante (las dos
ruedas funcionan al tiempo)
}
if (U<0){
atras =abs(U);
}
Serial.println(U);
if (adelante != 0){ // no se permite que se envie el comando hacia
adelante y hacia atras al tiempo
if (atras == 0){
analogWrite(llantaI,adelante);
digitalWrite(6,LOW);

digitalWrite(7,HIGH);

analogWrite(llantaD,adelante);
digitalWrite(4,LOW);
digitalWrite(5,HIGH);
}
}

if (atras != 0){
// no se permite que se envie el comando hacia
adelante y hacia atras al tiempo
if (adelante == 0){
analogWrite(llantaI,atras);
digitalWrite(6,HIGH);
digitalWrite(7,LOW);

analogWrite(llantaD,atras);
digitalWrite(4,HIGH);
digitalWrite(5,LOW);
}
}

También podría gustarte