/** pid.h
 *
 * This file defines two structures, PID and DCVelocityCtrl, and
 * functions to operate on these structures.
 * 
 */

#ifndef PID_H
#define PID_H

#define DEFAULT_KP 3
#define DEFAULT_KI 1
#define DEFAULT_KD 5
#define DEFAULT_STALL 0xFFFF
#define DEFAULT_UPPER_LIMIT  100
#define DEFAULT_LOWER_LIMIT -100

#include "eyebot.h"


/**********************************************************************
 * PID CONTROLLER
 *********************************************************************/


typedef struct pid_controller
{
  int setpoint;    // reference value
  int stallpoint;  // stall limit
  int limits[2];   // output limits

  float Kp;        // proportional gain
  float Ki;        // integral gain
  float Kd;        // derivative gain
  
  int error_last;
  int error_sum;

} PID;


/**
 * PIDInit
 *
 * INPUT: *pid - pointer to PID structure
 *
 * OUTPUT: none
 *
 * USAGE: Call to initialise the PID structure
 */
void PIDInit( PID *pid );


/**
 * PIDSetGains
 *
 * INPUT: *pid - pointer to PID structure
 *        Kp   - value of proportional gain
 *        Ki   - value of integral gain
 *        Kd   - value of derivative gain
 *
 * OUTPUT: none
 *
 * USAGE: Set the gains for the PID structure
 */
void PIDSetGains( PID *pid, double Kp, double Ki, double Kd );


/**
 * PIDSetPoint
 *
 * INPUT: *pid - pointer to PID structure
 *        setpoint - the reference value for the controller
 *
 * OUTPUT: none
 *
 * USAGE: Set the reference value for the controller
 */
void PIDSetPoint( PID *pid, int setpoint);


/**
 * PIDSetLimits
 *
 * INPUT: *pid - pointer to PID structure
 *        LL - lower limit
 *        UL - upper limit
 *
 * OUTPUT: none
 *
 * USAGE: Set the output signal limits, NOTE: the upper limit must be 
 *        greater than or equal to the lower limit.
 */
void PIDSetLimits( PID *pid, int LL, int UL);


/**
 * PIDCalculate
 *
 * INPUT: *pid - pointer to PID structure
 *        y    - the input to the controller (the output of the system) 
 *        
 * OUTPUT: The modified control signal
 *
 * USAGE: The PID control law
 */
int PIDCalculate( PID *pid, int y );


/**
 * PIDStalled
 *
 * INPUT: *pid - pointer to PID structure
 *
 * OUTPUT: 0 if not stalled, 1 if stalled
 *
 * USAGE: Checks that the integral error is below the stall point.
 */
int PIDStalled( PID *pid );


/**
 * PIDStallPoint
 *
 * INPUT: *pid  - pointer to PID structure
 *        stall - value of stall point
 *
 * OUTPUT: none
 *
 * USAGE: Set the stall point.
 */
void PIDStallPoint( PID *pid, int stall );


/**
 * PIDReset
 *
 * INPUT: *pid - pointer to PID structure
 *
 * OUTPUT: none
 *
 * USAGE: Reset the error values.  Useful when the integral error 
 *        becomes too large (i.e. reached the stall point).
 */
void PIDReset( PID *pid );



/**********************************************************************
 * DC Motor controller
 *********************************************************************/

typedef struct
{
  int position;        // most recent position
  int velocity;        // most recent velocity
  int velocity_limit;  // a velocity rate limiter

  MotorHandle motor;
  QuadHandle quad;
  PID pid;

} DCVelocityCtrl;


/**
 * DCVInit
 *
 * INPUT: *ctrl - pointer to a DCVelocityCtrl structure
 *        motor - DeviceSemantics for the motor 
 *                (i.e. MOTOR_LEFT, MOTOR_RIGHT)
 *        quad  - DeviceSemantics for the quadrature encoder 
 *                (i.e. QUAD_LEFT, QUAD_RIGHT)
 *
 * OUTPUT: none
 *
 * USAGE:Set the desired value of the controller
 */
void DCVInit( DCVelocityCtrl *ctrl,
  	      DeviceSemantics motor,
	      DeviceSemantics quad);

/**
 * DCVSetpoint
 *
 * INPUT: *ctrl    - pointer to a DCVelocityCtrl structure
 *        setpoint - the desired value
 *
 * OUTPUT: none
 *
 * USAGE:Set the desired value of the controller
 */
void DCVSetpoint(DCVelocityCtrl *ctrl, int setpoint);


/**
 * DCVCalculate
 *
 * INPUT: *ctrl - pointer to a DCVelocityCtrl structure
 *
 * OUTPUT: none
 *
 * USAGE: The motor control law, call on regular intervals.   
 *
 */  
void DCVCalculate(DCVelocityCtrl *ctrl);


/**
 * DCVPosition
 *
 * INPUT: *ctrl - pointer to a DCVelocityCtrl structure
 *
 * OUTPUT: current position of motor
 *
 * USAGE: Call to get position of motor   
 */  
int DCVPosition(DCVelocityCtrl *ctrl);


/**
 * DCVVelocity
 *
 * INPUT: *ctrl - pointer to a DCVelocityCtrl structure
 *
 * OUTPUT: current velocity of motor
 *
 * USAGE: Call to get velocity of motor   
 */  
int DCVVelocity(DCVelocityCtrl *ctrl);


/**
 * DCVRelease
 *
 * INPUT: *ctrl - pointer to a DCVelocityCtrl structure
 *
 * OUTPUT: none
 *
 * USAGE: Release the motor and quadrature.
 */
void DCVRelease(DCVelocityCtrl *ctrl);


#endif