From faf3415e5e160d52107fea6d8228b911e6be68a7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 27 May 2014 22:22:21 +0900 Subject: [PATCH] AC_PID: example sketch prints individual P, I and D values --- .../AC_PID/examples/AC_PID_test/AC_PID_test.pde | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde b/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde index cf05a78168..837cd72c55 100644 --- a/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde +++ b/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde @@ -36,12 +36,12 @@ void loop() AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100); uint16_t radio_in; uint16_t radio_trim; - int error; - int control; + int16_t error; + float control_P, control_I, control_D; float dt = 1000/50; // display PID gains - hal.console->printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); + hal.console->printf("P %f I %f D %f imax %f\n", (float)pid.kP(), (float)pid.kI(), (float)pid.kD(), (float)pid.imax()); // capture radio trim radio_trim = hal.rcin->read(0); @@ -49,10 +49,15 @@ void loop() while( true ) { radio_in = hal.rcin->read(0); error = radio_in - radio_trim; - control = pid.get_pid(error, dt); + control_P = pid.get_p(error); + control_I = pid.get_i(error, dt); + control_D = pid.get_d(error, dt); // display pid results - hal.console->printf("radio: %d\t err: %d\t pid:%d\n", radio_in, error, control); + hal.console->printf("radio: %d\t err: %d\t pid:%4.2f (p:%4.2f i:%4.2f d:%4.2f)\n", + (int)radio_in, (int)error, + (float)(control_P+control_I+control_D), + (float)control_P, (float)control_I, (float)control_D); hal.scheduler->delay(50); } }