//------OPTIONS---------
#define vSCALE 5;      //increasing this will increase the resolution of the gains, effectively dividing them by this
#define vIntSCALE 10;      //The integral gain needs to be scaled further than the P and D gains
#define vMAX   100;        //maximum value to output to motor controller - 0 to 127 - this is because velocity cannot be 100% in order to trun
//#define iDEFAULT   5;     //default integral gain
//#define pDEFAULT   5;     //default proportional gain
//#define dDEFAULT   5;     //default derivitive gain
//#define desDEFAULT 0;     //default desired heading

/**
  * This struct contains all the parameters for velocity control
**/
/** MOVED TO GLOBALS
struct
{
        int1 enabled;   //flag that enables / disables velocity control
       float desired;   //desired velocity
       float error;     //current error
       float pError;    //previous error
signed int16 iVal;      //integral component
signed int16 pVal;      //proportional component
signed int16 dVal;      //derivitive component
         int iGain;     //integral gain
         int pGain;     //proportional gain
         int dGain;     //derivative gain
signed int16 PIDtot;    //total value of P+I+D components

} vCONTROL;
**/



/**
  * This function calculates the error for velocity
  *
**/
void vError()
{

   //calculate error
   vCONTROL.error = vCONTROL.desired - GPS.sog;
   //!!Perform some reality checking

}

/**
  * This function interprets the hCONTROL.PIDtot and applies scaling / conversion and sends it to roboteq
  *
**/
void apply_v_robo()
{
   //------Local Variables------
           char hexVal[3] = "";  //the hex value of PID tot in a string
           char dir[2] = "";     //a single letter for forward or reverse
           char comb[4]="";      //this is the variable used to combine hexVal of PIDTOT and Direction
           signed int16 maxPIDtot = vMAX*vSCALE; //maximum value PID total can have
   //if PIDtot is larger than max, set to max
   //a larger vscale value will increase the resolution of the Gains
   if(vCONTROL.PIDtot > maxPIDtot)
   {
      vCONTROL.PIDtot = vMAX*vSCALE;
   } else if(vCONTROL.PIDtot < -maxPIDtot)
   {
      vCONTROL.PIDtot = -vMAX*vSCALE;
   }
   //divide by scale
   vCONTROL.PIDtot /= vSCALE;
   //perform power scaling
   if(PARAMS.pScale > 0 && PARAMS.pScale < 100)
   vCONTROL.PIDtot =(int16) ((float)vCONTROL.PIDtot*((float)PARAMS.pScale / 100)) ;
   //set the direction for roboteq
   if(vCONTROL.PIDtot >=0)
   {
      //forward
      dir[0] = 'b';
   } else
   {
      //reverse
      dir[0] = 'B';
   }
   //now convert value of PIDtot to hex string
   sprintf(hexVal,"%X",abs(vCONTROL.PIDtot));
   //set combination of hex val and dir to roboteq
   //combine dir and hexVal
   strcat(comb,dir);
   strcat(comb,hexVal);
   //copy command to roboteq M2
   strcpy(ROBO.M2,comb);

}


/**
  * This function performs heading control
  *
**/
void control_velocity()
{
   //----Local Variables----
   signed int16 maxPIDtot = 127*vSCALE;   //used to test if saturation has occured

   //-----Check if hCONTROL is enabled------
   if(!vCONTROL.enabled || PARAMS.mode == 'K' || GPS.status == 'V')
   {  //we're not enabled, or GPS hasn't locked on
      //also clear integral value
      vCONTROL.iVal = 0;
      return;
   }
   
   //----Update Previous Error----
   vCONTROL.pError = vCONTROL.error;
   
   //----Calculate New Error------
   vError();
   //----if error is 0, return, a bit optimistic-----
   if(vCONTROL.error == 0)
   {
      return;
   }
   //------Proportional-----
   vCONTROL.pVal = vCONTROL.pGain*vCONTROL.error;
   //------Derivitive-------
   vCONTROL.dVal = vCONTROL.dGain*(vCONTROL.error-vCONTROL.pError);
   //------Integral---------
   //we should only perform integration if plant is not saturated
   if(abs(vCONTROL.PIDtot) < maxPIDtot)
   vCONTROL.iVal += (vCONTROL.iGain*vCONTROL.error)/vIntSCALE;
   //also, if integral gain is zero, clear any effect
   if(vCONTROL.iGain == 0)
      {
      vCONTROL.iVal = 0;
      }
   //------Total PID--------
   vCONTROL.PIDtot = vCONTROL.pVal + vCONTROL.dVal + vCONTROL.iVal;
   //------Apply PID--------
   apply_v_robo();
}