00001 /* 00002 00003 The contents of this file are subject to the Mozilla Public License 00004 Version 1.1 (the "License"); you may not use this file except in 00005 compliance with the License. You may obtain a copy of the License at 00006 http://www.mozilla.org/MPL/ 00007 00008 Software distributed under the License is distributed on an "AS IS" 00009 basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the 00010 License for the specific language governing rights and limitations 00011 under the License. 00012 00013 The Original Code is the PID trajectory playback library. 00014 00015 The Initial Developers of the Original Code are Andrew Crossan (ac@dcs.gla.ac.uk) 00016 and John Williamson (jhw@dcs.gla.ac.uk) from University of Glasgow. 00017 00018 Portions created by Andrew Crossan and John Williamson are Copyright (C) 2006. 00019 All Rights Reserved. 00020 00021 Contributor(s): ______________________________________. 00022 00023 */ 00024 00025 00026 /* Header file for a PID Controller 00027 * 00028 * Developed by John Williamson (jhw@dcs.gla.ac.uk) at University of Glasgow 00029 * 00030 */ 00031 00032 /* Structure for PID controller */ 00033 typedef struct control_state{ 00034 double x, target_x, last_action, offset; 00035 double p, i , d, filter, out_filter, gain, sat_low, sat_high; 00036 double cn_p, cn_i, cn_d, cn_f; 00037 char *input, *output; 00038 float *float_in, *float_out; 00039 int saturate; 00040 double dead_zone; 00041 int soft_saturate; // Use soft saturation 00042 double soft_saturate_scale; // x-scale of saturation; higher values mean more step like response 00043 double integrator_decay; // Integrator exp. decay values 00044 double integrator_max_decay; // Integrator exp. decay value after reaching max_integrator 00045 double max_integrator; // Threshold at which integrator_max_decay takes effect. 00046 double integrator_kill; // Zone at which integrator deactivated 00047 double max_change; // Max step value in input 00048 double input_filter; // Input filtering 00049 double i_cap; // Cap on the integrator term to avoid wind-up 00050 } control_state; 00051 00052 /* Limits the integrator. 00053 I is multiplied by decay each time step 00054 When integrator reaches max_decay, 00055 it is multiplied by max_decay instead. 00056 By default, integrator protection is not enabled. 00057 */ 00058 void set_integrator_decay(control_state *state, double max, double decay, double max_decay); 00059 void set_rate_limiter(control_state *state, double max_change); 00060 void set_integrator_kill(control_state *state, double kill); 00061 control_state *init_state(double p, double i, double d, double f, double out_filter, double gain, double dead_zone, 00062 double sat_low, double sat_high); 00063 double control(control_state *state, double x, double target); 00064 void set_soft_saturation(control_state *state, double scale); 00065 void reset_state(control_state *state); 00066 void set_input_filter(control_state *state, double f); 00067 /* Set PID controller DC offset */ 00068 void set_dc(control_state *state, double dc); 00069 void set_integrator_cap(control_state *state, double cap);
1.4.7