and I used this to detect the problem arises only connect the bluetooth, is the same code as the first, only allowing the bluetooth connect.
#include <SoftwareSerial.h>
#define TxD 7
#define RxD 8
#define EN 2
int dato = 0;
// cambiamos los pines de rxd y txd
SoftwareSerial Bluetooth(TxD,RxD); // RX, TX recorder que se cruzan
#include<Servo.h>
//acelerometro grioscopio
#include <Wire.h>
#define BAUD 9600 //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
Servo motor1, motor2, motor3, motor4; //Crear un objeto de clase servo
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
int vel = 1000; //amplitud del pulso
int i,j,k,l=1000;
int a,b=0;
int aceleracion;
int media;
byte recibiendoByte ;
boolean iniciado = false;
int vel1;
//Angulos
float Acc[2];
float Gy[2];
float Angle[2];
void setup()
{
Bluetooth.begin(BAUD);
pinMode(EN ,OUTPUT);
digitalWrite(EN,HIGH);
Serial.begin(BAUD);
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
//Asignar un pin al ESC
motor1.attach(9);
motor2.attach(10);
motor3.attach(6);
motor4.attach(5);
Serial.println (" Conecte la bateria \n una vez conectada escuchara una serie de pitidos \n pulse 'A' para arrancar");
while ( iniciado==false ){
recibiendoByte = Serial.read(); // Leemos el Byte recibido
if (recibiendoByte == 65 || recibiendoByte ==97) { // A o a Mayusculas o minusculas
iniciado=true;
Serial.println(" Escuchara un pitido de confirmacion y varios pitidos que indican el numero de celulas de la bateria ");
//Activar el ESC
motor1.writeMicroseconds(1000); //1000 = 1ms
motor2.writeMicroseconds(1000);
motor3.writeMicroseconds(1000);
motor4.writeMicroseconds(1000);
//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
}
}
Serial.println(" Motor preparado ") ;
Serial.println(" escriva la velocidad (1000-2000) ");
Serial.setTimeout(10);
}
void loop()
{
{
dato = Bluetooth.read();
// .read devuelve -1 si no hay nada
if (dato != -1)
{
Serial.println(dato);
// delay(100);
}
}
//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];
//Mostrar los valores por consola
// Serial.print("Angle X: "); Serial.print(Angle[0]); Serial.print("\n");
// Serial.print("Angle Y: "); Serial.print(Angle[1]); Serial.print("\n------------\n");
// indicamos la velocidad de los motores angulo Z
//vel1 existe porque entra dos veces al bucle
if(Serial.available() > 0)
{
vel1 = Serial.parseInt(); //Leer un entero por serial
Serial.print(vel);
if(vel1 != 0)
{
vel=vel1;
// calculamos la media de los valores i j k l
media=(i+j+k+l)/4;
// medimos cual es la "aceleracion" para calcular la diferencia entre la velociad deseada y los valores actuales
aceleracion=vel-media;
l=l+aceleracion;
i=i+aceleracion;
j=j+aceleracion;
k=k+aceleracion;
Serial.print("dando velocidad ");
}
}
delay(10); //Nuestra dt sera, pues, 0.010, que es el intervalo de tiempo en cada medida
// Estabilidad angulo X
if( Angle[0]-a>2.5)
{
i++,j++,k--,l--;
Serial.print("Angle X+ ");Serial.print("\n");
}
if( Angle[0]-a<-2.5)
{
i--,j--,k++,l++;
Serial.print("Angle X- ");Serial.print("\n");
}
// if( Angle[0]-a>10)
// {
// i=i+5,j=j+5,k=k-10,l=l-10;
// }
// if( Angle[0]-a<-10)
// {
// i=i-10,j=j-10,k=k+10,l=l+10;
// }
// if( Angle[0]-0>20)
// {
// i=i+10,j=j+10,k=k-10,l=l-10;
// Serial.print("Angle X+20 ");Serial.print("\n");
// }
// if( Angle[0]-0<-20)
// {
// i=i-10,j=j-10,k=k+10,l=l+10;
// Serial.print("Angle X-20 ");Serial.print("\n");
// }
// Estabilidad angulo y
if( Angle[1]-b > 2.5)
{
i++,j--,k++,l--;
Serial.print("Angle Y+ ");Serial.print("\n");
}
if( Angle[1]-b < -2.5)
{
i--,j++,k--,l++;
Serial.print("Angle Y- ");Serial.print("\n");
}
// if( Angle[1]-a>10)
// {
// i=i+10,j=j-10,k=k+10,l=l-10;
// }
// if( Angle[1]-a<-10)
// {
// i=i-10,j=j+10,k=k-10,l=l+10;
// }
// if( Angle[1]-0>20)
// {
// i=i+10,j=j-10,k=k+10,l=l-10;
// Serial.print("Angle Y+20 ");Serial.print("\n");
// }
// if( Angle[1]-0<-20)
// {
// i=i-10,j=j+10,k=k-10,l=l+10;
// Serial.print("Angle Y-20 ");Serial.print("\n");
// }
// marcar maximos y minimos
if( i>1500)
{i=1500;
}
if (i<1000)
{i=1000;
}
if( j>1500)
{j=1500;
}
if (j<1000)
{j=1000;
}
if( k>1500)
{k=1500;
}
if (k<1000)
{k=1000;
}
if( l>1500)
{l=1500;
}
if (l<1000)
{l=1000;
}
if( Angle[0]-a<2.5 && Angle[0]-a>-2.5&& Angle[1]-b<2.5 && Angle[1]-b>-2.5)
{
i=vel;
j=vel;
k=vel;
l=vel;
Serial.print("estabilizado");
}
motor1.writeMicroseconds(i);
motor2.writeMicroseconds(j);
motor3.writeMicroseconds(k);
motor4.writeMicroseconds(l);
}
maybe the code is not optimized as much as possible, but it works, and the problem is bluetooth
Thanks for the help
