PID_v1 solo computa una instancia?

International Español Software
1

Hola,

Tengo un problema con la libreria PID_v1 del Arduino.

Declaración

//PID
float outputRollStable;
float outputPitchStable;
float outputYawStable;

float targetYaw
float targetPitch;
float targetRoll;

PID yawReg(&ypr[0], &outputYawStable, &targetYaw, YAW_P_VAL, YAW_I_VAL, YAW_D_VAL, DIRECT);
PID pitchReg(&ypr[1], &outputPitchStable, &targetPitch, PITCH_P_VAL, PITCH_I_VAL, PITCH_D_VAL, REVERSE);
PID rollReg(&ypr[2], &outputRollStable, &targetRoll, ROLL_P_VAL, ROLL_I_VAL, ROLL_D_VAL, REVERSE);

Función de inicialización en setup()

bool initPID()
{

	yawReg.SetMode(AUTOMATIC);
	pitchReg.SetMode(AUTOMATIC);
	rollReg.SetMode(AUTOMATIC);

	yawReg.SetOutputLimits(-YAW_PID_LIMIT, YAW_PID_LIMIT);
	pitchReg.SetOutputLimits(-PITCH_PID_LIMIT, PITCH_PID_LIMIT);
	rollReg.SetOutputLimits(-ROLL_PID_LIMIT, ROLL_PID_LIMIT);

	//yawReg.SetSampleTime(10);
	//pitchReg.SetSampleTime(10);
	//rollReg.SetSampleTime(10);

	return true;
}

Compute

void computePID()
{
	//RX
	channel[THRO] = receiverInputChannel3;
	channel[YAW] = receiverInputChannel4;
	channel[PITCH] = receiverInputChannel2;
	channel[ROLL] = receiverInputChannel1;

	targetYaw = map((float)channel[YAW], RC_MIN, RC_MAX, -YAW_PID_INFLUENCE, YAW_PID_INFLUENCE);
	targetPitch = map((float)channel[PITCH], RC_MIN, RC_MAX, -PITCH_PID_INFLUENCE, PITCH_PID_INFLUENCE);
	targetRoll = map((float)channel[ROLL], RC_MIN, RC_MAX, -ROLL_PID_INFLUENCE, ROLL_PID_INFLUENCE);

	yawReg.Compute();
	pitchReg.Compute();
	rollReg.Compute();
	

}

Cuando voy a ver los valores del output, solo me hace el rollReg.

Por lo que visto, si en la declación pongo el último el pitch, solo hace el pitch.

//PID
float outputRollStable;
float outputPitchStable;
float outputYawStable;

float targetYaw
float targetPitch;
float targetRoll;

PID yawReg(&ypr[0], &outputYawStable, &targetYaw, YAW_P_VAL, YAW_I_VAL, YAW_D_VAL, DIRECT);
PID rollReg(&ypr[2], &outputRollStable, &targetRoll, ROLL_P_VAL, ROLL_I_VAL, ROLL_D_VAL, REVERSE);
PID pitchReg(&ypr[1], &outputPitchStable, &targetPitch, PITCH_P_VAL, PITCH_I_VAL, PITCH_D_VAL, REVERSE);

(ahora el pitch funciona pero el resto no)

Lo único que he modificado de la librería es que en vez de doubles sean floats, adjunto la librería.

Los valores de la radio funcionan bien, de echo, los pids funcionan (todos) cuando usa la IMU (ypr[3]) pero solo uno al de la radio según el orden.

Alguna idea? No hay funciones estáticas y son punteros... No veo pq no funciona

Gracias.

PID.cpp (6.4 KB)

PID.h (3.47 KB)

2

Hola,

Nada, no lo veo, es cambiar el orden de la declaración y va uno u otro.

Solo cambiando el orden de estas dos líneas:

PID rollReg(&ypr[2], &outputRollStable, &targetRoll, ROLL_P_VAL, ROLL_I_VAL, ROLL_D_VAL, REVERSE);
PID pitchReg(&ypr[1], &outputPitchStable, &targetPitch, PITCH_P_VAL, PITCH_I_VAL, PITCH_D_VAL, REVERSE);

hace q vaya uno u otro :confused:

3

Por ir delimitando si el problema está en la modificación de la librería o en tu código...
¿Con la versión original de la librería sí te funcionaba?
Tal y como la veo y has comentado, no veo miembros estáticos, así que debería funcionar con múltiples instancias, por lo que supongo que el posible fallo esté en tu código. Lo que pasa es que no está completo y no puedo seguir todas las declaraciones de tus variables (especialmente los arrays ypr y channel) ni tus define. Sí que me parece que las tres sentencias como la que sigue no están bien construidas:
targetYaw = map((float)channel[YAW], RC_MIN, RC_MAX, -YAW_PID_INFLUENCE, YAW_PID_INFLUENCE);
ya que si no me equivoco map toma y devuelve valores long. Lo lógico sería, si acaso:
targetYaw = (float) map(channel[YAW], RC_MIN, RC_MAX, -YAW_PID_INFLUENCE, YAW_PID_INFLUENCE);

De todas formas sería conveniente que monitorices por serial los valores de los tres pid y muestres la salida (entre etiquetas code) a ver qué ocurre realmente.
Saludos.