/******************************************************************************/
/* Copyright 1992 MBARI                                                       */
/******************************************************************************/
/* Summary  : Servo Control Module Definitions for Tiburon Control System     */
/* Filename : servo.h                                                         */
/* Author   : Janice Tarrant                                                  */
/* Project  : Tiburon                                                         */
/* Version  : Version 1.0                                                     */
/* Created  : 10/12/92                                                        */
/* Modified : 06/10/94                                                        */
/* Archived :                                                                 */
/******************************************************************************/
/* Modification History :                                                     */
/* $Header: servo.h,v 1.1 97/12/04 15:28:17 oreilly Exp $
 * $Log:        servo.h,v $
 * Revision 1.1  97/12/04  15:28:17  15:28:17  oreilly (Thomas C. O'Reilly)
 * Initial revision
 *
 *
 */
/******************************************************************************/

#ifndef ROV_CONTROL_SERVO_H
#define ROV_CONTROL_SERVO_H

#define X_PROPORTIONAL_GAIN         950 /* x proportional gain                */
#define Y_PROPORTIONAL_GAIN         950 /* y proportional gain                */
#define DEPTH_PROPORTIONAL_GAIN     410 /* depth proportional gain            */
#define ALTITUDE_PROPORTIONAL_GAIN  180 /* altitude proportional gain         */
#define HEADING_PROPORTIONAL_GAIN  8400 /* heading proportional gain          */

#define X_INTEGRAL_GAIN               0 /* x integral gain                    */
#define Y_INTEGRAL_GAIN               0 /* y integral gain                    */
#define DEPTH_INTEGRAL_GAIN           0 /* depth integral gain                */
#define ALTITUDE_INTEGRAL_GAIN        0 /* altitude integral gain             */
#define HEADING_INTEGRAL_GAIN         0 /* heading integral gain              */

#define X_DERIVATIVE_GAIN          3300 /* x derivative gain                  */
#define Y_DERIVATIVE_GAIN          3300 /* y derivative gain                  */
#define DEPTH_DERIVATIVE_GAIN         0 /* depth derivative gain              */
#define ALTITUDE_DERIVATIVE_GAIN   1280 /* altitude derivative gain           */
#define HEADING_DERIVATIVE_GAIN    5880 /* heading derivative gain            */

/*#define CONTROL_DT    0.05*/          /* sample period (20 Hz rate)         */
#define CONTROL_DT    0.06667           /* sample period (15 Hz rate)         */
#define EPSILON       0.00001           /* floating point zero                */
#define ACCEL_G       9.8               /* gravitational acceleration (m/s)   */
#define ADAPTIVE_GAIN 0.8               /* adaptive buoyancy gain             */
#define LAMBDA  DEPTH_PROPORTIONAL_GAIN / DEPTH_DERIVATIVE_GAIN

#endif