/*
   Notes:
      Sign Convention - Clockwise is positive, Counterclockwise is negative
      COMPASS.heading should be a float
*/


//------OPTIONS---------
#define hSCALE 10;      //increasing this will increase the resolution of the gains, effectively dividing them by this
#define hIntSCALE 10;      //The integral gain needs to be scaled further than the P and D gains
#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 heading control
**/
/*MOVED TO GLOBALS
struct
{
        int1 enabled;   //flag that enables / disables heading control
       float desired;   //desired heading
       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

} hCONTROL;
*/ //MOVED TO GLOBALS


/**
  * This function calculates the error for heading
  *
**/
void hError()
{
   //---local variables---
   signed int16 error = 0;
   //calculate error
   error = hCONTROL.desired - COMPASS.heading;
   
   //A compass returns to 0* after 360, so this needs to be taken into account
   //also, error can't be more than 180*
   if(abs(error) > 180)
   {
      //we need to switch direction then
      //error *= -1;
      //test if error is positive or negative
      if(error<0)
      {
         //we need to add 180*
         error +=360;
      } else
      {
         //we need to subract 180*
         error -= 360;
      }   
   }
   //we now have our properly signed error
   hCONTROL.error = error;
}

/**
  * This function interprets the hCONTROL.PIDtot and applies scaling / conversion and sends it to roboteq
  *
**/
void apply_h_robo()
{
   //------Local Variables------
           char hexVal[3] = "";
           char dir[2] = "";
           char comb[4]="";
           signed int16 maxPIDtot = 127*hSCALE;
   //if PIDtot is larger than max, set to max
   //a larger max value will increase the resolution of the Gains
   if(hCONTROL.PIDtot > maxPIDtot)
   {
      hCONTROL.PIDtot = 127*hSCALE;
   } else if(hCONTROL.PIDtot < -maxPIDtot)
   {
      hCONTROL.PIDtot = -127*hSCALE;
   }
   //divide by scale
   hCONTROL.PIDtot /= hSCALE;
   //perform power scaling
   if(PARAMS.pScale > 0 && PARAMS.pScale < 100)
   hCONTROL.PIDtot =(int16) ((float)hCONTROL.PIDtot*((float)PARAMS.pScale / 100)) ;
   //set the direction for roboteq
   if(hCONTROL.PIDtot >=0)
   {
      //clockwise turn
      dir[0] = 'a';
   } else
   {
      //counterclockwise turn
      dir[0] = 'A';
   }
   //now convert value of PIDtot to hex string
   sprintf(hexVal,"%X",abs(hCONTROL.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_heading()
{
   //----Local Variables----
   signed int16 maxPIDtot = 127*hSCALE;   //used to test if saturation has occured

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