Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-16 22:37:26 -07:00 committed by Pat Hickey
parent 753ac6c27b
commit 2f5b292d50
1 changed files with 27 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde
View File

@ -1,7 +1,7 @@
/* /*
Example of PID library. * Example of PID library.
2012 Code by Jason Short, Randy Mackay. DIYDrones.com * 2012 Code by Jason Short, Randy Mackay. DIYDrones.com
*/ */
// includes // includes
#include <Arduino_Mega_ISR_Registry.h> #include <Arduino_Mega_ISR_Registry.h>
@ -26,39 +26,39 @@ APM_RC_APM1 APM_RC;
// setup function // setup function
void setup() void setup()
{ {
Serial.begin(115200); Serial.begin(115200);
Serial.println("ArduPilot Mega AC_PID library test"); Serial.println("ArduPilot Mega AC_PID library test");
isr_registry.init(); isr_registry.init();
APM_RC.Init(&isr_registry); // APM Radio initialization APM_RC.Init(&isr_registry); // APM Radio initialization
delay(1000); delay(1000);
} }
// main loop // main loop
void loop() void loop()
{ {
// setup (unfortunately must be done here as we cannot create a global AC_PID object) // setup (unfortunately must be done here as we cannot create a global AC_PID object)
AC_PID pid(0, PSTR("TESTPID_"), TEST_P, TEST_I, TEST_D, TEST_IMAX * 100); AC_PID pid(0, PSTR("TESTPID_"), TEST_P, TEST_I, TEST_D, TEST_IMAX * 100);
uint16_t radio_in; uint16_t radio_in;
uint16_t radio_trim; uint16_t radio_trim;
int error; int error;
int control; int control;
float dt = 1000/50; float dt = 1000/50;
// display PID gains // display PID gains
Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
// capture radio trim // capture radio trim
radio_trim = APM_RC.InputCh(0); radio_trim = APM_RC.InputCh(0);
while( true ) { while( true ) {
radio_in = APM_RC.InputCh(0); radio_in = APM_RC.InputCh(0);
error = radio_in - radio_trim; error = radio_in - radio_trim;
control = pid.get_pid(error, dt); control = pid.get_pid(error, dt);
// display pid results // display pid results
Serial.printf("radio: %d\t err: %d\t pid:%d\n", radio_in, error, control); Serial.printf("radio: %d\t err: %d\t pid:%d\n", radio_in, error, control);
delay(50); delay(50);
} }
} }