Perdonad, creo que no me expresado bien.
el código es el siguiente: (es un brazo con un motor y una hélice a un extremo que lo que consigue es equilibrarse)

Declarar variables librerias etc...
/Libreria motores
#include<Servo.h>
//Libreria PID
#include <PID_v1.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
// 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
int a,b,q=0;
int Reinicio=0;
byte recibiendoByte ;
boolean iniciado = false;
int dato = 0;
int desconexion = 0;
//Angulos
float Acc[2];
float Gy[2];
float Angle[2];
// valores kp,ki,kd
float kp=7;
float ki=10;
float kd=0.4;
/* 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
Setup:
aquí tengo un código de manera que hasta que no le mando la señal "A" (byte 65) no empieza el loop:
void setup()
{
Serial.begin(BAUD);
// //dar corriente al bluetooth
// pinMode(8, OUTPUT);
// digitalWrite(8,HIGH);
// 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(10);
//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); //
myPID.SetOutputLimits(1000,2000);
Setpoint = 0; //angulo 0º
}
Loop, basicamente lo que hace es activar el motor hasta que está en el angulo que le he indicado
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 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 dato por el ordenador
if(Serial.available() > 0)
{
cuenta=0;
dato = Serial.parseInt(); //Leer un entero por serial
}
if (dato==163)
{
while( Reinicio==0)
{
motor1.writeMicroseconds(1000);
dato = 0;
dato = Serial.read();
if (dato==163)
{
Reinicio = 1;
}
}
Reinicio = 0;
}
// PID
Input = Angle[0];
myPID.Compute();
motor1.writeMicroseconds(Output);
}
Vuelvo a explicar el problema:
El programa funciona perfectamente.
Si al activar el arduino antes de mandar la señal "A" para iniciar el programa doy al reset, el motor (que estaba parado) sigue parado.
Pero una vez iniciado el programa si le mando parar (mandando el numero 163) si quiero volver a iniciar el arduino desde 0 seria dando al reset.
Mi problema es que si doy al reset el motor (que ha estado funcionando, pero que ahora esta parado)se activa al máximo.
De esta manera, no puedo volver a iniciar mi programa nunca.