PID_v1 Library problem

Projects Programming
1

Hi,

I'm programming a fligh controller with Arduino Pro Mini 16Mhz

I'm having troubles instancing the PID library for yaw pitch roll.

Declaration

//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);

Init function 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 function

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();
	

}

The only PID that computes is rollReg.

That is because in the declaration is the last one. If I move the pitchReg after rollReg, then only roll will be computed.

//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);

(now pitch works)

The PID library is Arduino standard, just modified doubles for float (attached). The values of radio are ok.

Any idea? The functions aren't static, so I dont understand the behaviour. Maybe pointers?

Thanks you.

PID.cpp (6.4 KB)

PID.h (3.47 KB)

2

I forgive to put the defines:

//RC
#define RC_CHANNELS 4
#define RC_MIN 1000	
#define RC_MAX 2000
#define RC_HALF ((RC_MAX + RC_MIN) / 2)

//CHANNEL MAPPING
#define THRO 0		//3 Throttle
#define YAW 1		//4 Yaw
#define PITCH 2		//2 Pitch
#define ROLL 3		//1 Roll
#define AUX1 4		//5 Aux1
#define AUX2 5		//6 Aux2


//ESC CONFIGURATION
#define ESC_FL_PIN A0
#define ESC_FR_PIN A1
#define ESC_BR_PIN A2
#define ESC_BL_PIN A3

#define ESC_FL 0
#define ESC_FR 1
#define ESC_BR 2
#define ESC_BL 3

#define ESC_MIN 1100
#define ESC_HALF 1450
#define ESC_MAX 1800
#define ESC_ARM_DELAY 1000



static short ORIENTATION = 1; // ALLOWED VALUES 1 OR -1


//PID

#define YAW_PID_LIMIT		100.0
#define PITCH_PID_LIMIT		200.0
#define ROLL_PID_LIMIT		200.0


#define YAW_PID_INFLUENCE	30
#define PITCH_PID_INFLUENCE 30
#define ROLL_PID_INFLUENCE	30


#define PITCH_P_VAL 10.0f
#define PITCH_I_VAL 0.0f
#define PITCH_D_VAL 0.0f

#define ROLL_P_VAL 10.0f
#define ROLL_I_VAL 0.0f
#define ROLL_D_VAL 0.0f

#define YAW_P_VAL 0.0f
#define YAW_I_VAL 0.0f
#define YAW_D_VAL 0.0f
3

Anyone? :frowning: