Buenas gente!
Planteo el código que pienso probar:
lo voy comentando para que quede mas claro:
Declaraciones servo y diferentes variables para controlar IMU y obtener un ángulo
//motores
#include<Servo.h>
//acelerometro grioscopio
#include <Wire.h>
#define BAUD 20000 //velocidad puerto serial funciona hasta 38400
//Direccion I2C de la IMU
#define MPU 0x68
//Ratios de conversion
#define A_R 16384.0
#define G_R 131.0
//Conversion de radianes a grados 180/PI
#define RAD_A_DEG = 57.295779
// conexiones bluetooth
//#define TxD 0
//#define RxD 1
#define EN 2
//VCC -->5V
// conexiones giroscopio
//VCC -->5V
//GND -->GND
//(Valores por defecto en la libreria)
//SCL -->A5
//SDA -->A4
Servo motor1; //Crear un objeto de clase servo
int16_t
AcX, AcY, AcZ, GyX, GyY, GyZ;
int vel = 1000;
//amplitud del pulso
float
i=1000;
int a,b=0;
byte recibiendoByte ;
boolean iniciado = false;
//int vel1;
int dato = 0;
int desconexion = 0;
//Angulos
float Acc[2];
float Gy[2];
float Angle[2];
Declaraciones para el PID (esto supongo que esta bien)
// valores kp,ki,kd
float kp=1.5;
float ki=1.5;
float kd=1.5;
/* PID Data */
double Setpoint, Input, Output; //Definir vaiables del pid
PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT); //crear myPID para ya llamarlo con sus todos sus parametros
Void setup: consta de leer un dato por el bluetooth para comprobar que todo esta listo e iniciar el ESC del motor
Poner PID en automatico y Sample time que no le tengo muy claro y setpoint supongo que hay que poner lo deseado (angulo = 0º)
void setup()
{
Serial.begin(BAUD);
Bluetooth.begin(BAUD);
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
//Asignar pines bluetooth
pinMode(EN ,OUTPUT);
//Asignar un pin al ESC
motor1.attach(9);
//Preparar Bluetooth
digitalWrite(EN,HIGH);
while ( iniciado==false )
{
recibiendoByte = Serial.read();
// Leemos el Byte recibido
if (recibiendoByte == 65)
{
iniciado=true;
//Activar el ESC
motor1.writeMicroseconds(1000); //1000 = 1ms
//Cambia el 1000 anterior por 2000 si
//tu ESC se activa con un pulso de 2ms
delay(5000);
//Esperar 5 segundos para hacer la activacion
}
}
//turn the PID on
myPID.SetMode(AUTOMATIC);
// myPID.SetOutputLimits(**,**);
myPID.SetSampleTime(10); // este valor no lo tengo muy claro, pero supongo que cuanto ams pequeño mejor no?
Setpoint = 0; //angulo 0º
}
Void loop
Calculos para obtener angulo X e Y del IMU (aunque solo utilizaremos uno de ellos)
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(); //Cada valor ocupa 2 registros
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
//Se calculan los angulos Y, X respectivamente.
Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
//Leer los valores del Giroscopio
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU,4,true);
//A diferencia del Acelerometro, solo se piden 4 registros
GyX=Wire.read()<<8|Wire.read();
GyY=Wire.read()<<8|Wire.read();
//Calculo del angulo del Giroscopio
Gy[0] = GyX/G_R;
Gy[1] = GyY/G_R;
//Aplicar el Filtro Complementario
Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];
Leer datos por el bluetooth para cambiar kp ki kd
// leemos el dato que entre por el bluetooth y lo almacenamos en la variable "dato" para poder clasificarlo
dato = Serial.read();
// Ajustar kp ki kd con el Bluetooth
if (dato != -1)
{
// kp
//
if (dato==1)
{
kp=kp+0.5;
}
if (dato==2)
{
kp=kp-0.5;
}
// ki
//
if (dato==3)
{
ki=ki+0.5;
}
if (dato==4)
{
ki=ki-0.5;
}
// kd
//
if (dato==5)
{
kd=kd+0.5;
}
if (dato==6)
{
kd=kd-0.5;
}
Y aquí viene donde tengo mas dudas y espero que me podáis decir si es correcto o no o por donde van los tiros...
Notese que la función map es la primera vez que la uso, creo que me convierte los º a ms segun la relacción que le he dado: -45º 1070ms (motor parado) +45º 2000(motor al maximo), pero tampoco estoy muy seguro si así funcionará (tambien agradeceria un poco de luz en este asunto)
// PID
Input = Angle[0];
myPID.Compute();
i = map (Output,-45,45,1070,2000);
motor1.writeMicroseconds(i);
}
Saludos y gracias por anticipado
