diff --git a/libraries/AP_PID/examples/AP_pid/AP_pid.pde b/libraries/AP_PID/examples/AP_pid/AP_pid.pde index 5d7cf59df4..9ce0bfcc02 100644 --- a/libraries/AP_PID/examples/AP_pid/AP_pid.pde +++ b/libraries/AP_PID/examples/AP_pid/AP_pid.pde @@ -1,51 +1,51 @@ -/* - Example of PID library. - 2010 Code by Jason Short. DIYDrones.com -*/ - -#include -#include -#include -#include // ArduPilot RC Library -#include // ArduPilot Mega RC Library - -long radio_in; -long radio_trim; - -Arduino_Mega_ISR_Registry isr_registry; -AP_PID pid; -APM_RC_APM1 APM_RC; - -void setup() -{ - Serial.begin(115200); - Serial.println("ArduPilot Mega AP_PID library test"); - - isr_registry.init(); - APM_RC.Init(&isr_registry); // APM Radio initialization - - delay(1000); - //rc.trim(); - radio_trim = APM_RC.InputCh(0); - - pid.kP(1); - pid.kI(0); - pid.kD(0.5); - pid.imax(50); - pid.kP(0); - pid.kI(0); - pid.kD(0); - pid.imax(0); - Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); -} - -void loop() -{ - delay(20); - //rc.read_pwm(); - long error = APM_RC.InputCh(0) - radio_trim; - long control = pid.get_pid(error, 20, 1); - - Serial.print("control: "); - Serial.println(control,DEC); -} +/* + * Example of PID library. + * 2010 Code by Jason Short. DIYDrones.com + */ + +#include +#include +#include +#include // ArduPilot RC Library +#include // ArduPilot Mega RC Library + +long radio_in; +long radio_trim; + +Arduino_Mega_ISR_Registry isr_registry; +AP_PID pid; +APM_RC_APM1 APM_RC; + +void setup() +{ + Serial.begin(115200); + Serial.println("ArduPilot Mega AP_PID library test"); + + isr_registry.init(); + APM_RC.Init(&isr_registry); // APM Radio initialization + + delay(1000); + //rc.trim(); + radio_trim = APM_RC.InputCh(0); + + pid.kP(1); + pid.kI(0); + pid.kD(0.5); + pid.imax(50); + pid.kP(0); + pid.kI(0); + pid.kD(0); + pid.imax(0); + Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); +} + +void loop() +{ + delay(20); + //rc.read_pwm(); + long error = APM_RC.InputCh(0) - radio_trim; + long control = pid.get_pid(error, 20, 1); + + Serial.print("control: "); + Serial.println(control,DEC); +}