Robot Equilibrio Automático.

[Claudio]

Boas Pessoal.

Estou a tentar implementar um sistema PID utilizando a biblioteca AutoPID, num robot com dois motores DC para que o mesmo se auto equilibre consoante a leitura que um acelerometro GPY521 trasmite á minha arduino uno. O problema que enfrento é que consigo ter resposta dos motores numa direcção, mas não na outra. Não estou a conseguir por mim resolver o problema, pelo que vim aqui pedir alguma ajuda para o que possa estar a fazer mal.

Eis o meu código:
#include <Wire.h>
#include <AutoPID.h>

const int MPU6050_addr = 0x68; // I2C address do sensor MPU-6050

int16_t accelerometer_x, accelerometer_y, accelerometer_z;
double setpoint =0; // Angulo de inclinação desejado (zero).
double input =0; // ANgulo de inclinação presente.
double output =0;
int16_t initial_accel_y;
int16_t accel_y;

// Motor control 1
const int m1Pin1 = A0;
const int m1Pin2 = A1;
const int m1_enable = 5;

// Motor control 2
const int m2Pin1 = A2;
const int m2Pin2 = A3;
const int m2_enable = 6;

// Instanciar AutoPID
AutoPID pid(&input, &setpoint, &output, 0, 255, 500, 1, 600); // a função incorpora os argumentos: moradas das variaveis input, setpoint e output, valores minimo e maximo de analogWrite e por ultimo valores de Kp, Ki e Kd.

void setup() {
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
delay(1000);

pinMode(m1Pin1, OUTPUT);
pinMode(m1Pin2, OUTPUT);
pinMode(m2Pin1, OUTPUT);
pinMode(m2Pin2, OUTPUT);

initial_accel_y = readAccelerometerY();

Serial.begin(9600);
}

void loop() {
// Ler dados do acelerometro e calcular inclinação:
accel_y = readAccelerometerY();
input = (double)(accel_y – initial_accel_y) / 16384.0; // Converter para angulo

// Calcular PID output
pid.run();

// Ajustar resposta dos motores com base no PID:
if (input > 0) {

digitalWrite(m1Pin1, LOW);
digitalWrite(m1Pin2, HIGH);
analogWrite(m1_enable, output);

digitalWrite(m2Pin1, LOW);
digitalWrite(m2Pin2, HIGH);
analogWrite(m2_enable, output);
}

if (input < 0){
digitalWrite(m1Pin1, HIGH);
digitalWrite(m1Pin2, LOW);
analogWrite(m1_enable, output);

digitalWrite(m2Pin1, HIGH);
digitalWrite(m2Pin2, HIGH);
analogWrite(m2_enable, output);
}

Serial.print(“Input: “);
Serial.print(input);
Serial.print(” Output: “);
Serial.println(output);

delay(100); // Delay para estabilizar
}

int16_t readAccelerometerY() {
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x3D);
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true);

return Wire.read() << 8 | Wire.read();
}
Melhores Cumrpimentos.
Obrigado pela ajuda e bons projetos a todos.

[Autor]

Artigos