//DEFINE THE PINS USED FOR THE ROBOTEQ CONTROLLER UART
//MUST BE 9600 bause 7 Bit Even Parity
#use rs232(baud=9600,xmit=PIN_B5,rcv=PIN_B4,bits=7,PARITY=E,stream=ROBOSTREAM,ERRORS,DISABLE_INTS)

//-------Options------
#define ROBOTEQ_AX3500 1         //Set to 1 if AX3500 controller is being used
#define ROBO_TIMEOUT 3500         //Time in microseconds to wait for incoming characters

//--------FLAGS------
int1 robo_timeout_error=FALSE;   //raised when timeout occurs waiting for serial characters

/**
  * This function will wait for a incoming character a certain amount of time
  * until it times out and raises the timeout_error flag
**/
char robo_timed_getc() {
   int32 timeout=0;
   robo_timeout_error=FALSE;
   while(!kbhit(ROBOSTREAM)&&(timeout<ROBO_TIMEOUT)) // wait up to .5ms for character
     {
          delay_us(1);
          timeout++;
     }
   if(kbhit(ROBOSTREAM))
          return(fgetc(ROBOSTREAM));
   else {
          robo_timeout_error=TRUE;
          return(0);
   }
}
/**
  * This function sends the motor commands to the roboteq
  *
**/
void update_robo()
{

//------Check for Kill mode!-----
if(PARAMS.mode == 'K')
{
//set motors to a00 and b00
strcpy(ROBO.M1,"A00");
strcpy(ROBO.M2,"B00");
}

   //output header - !
   fputc('!',ROBOSTREAM);
   //output motor 1 command
   fputs(ROBO.M1,ROBOSTREAM);
   //output carriage return
   fputc('\r',ROBOSTREAM);
   //output header - !
   fputc('!',ROBOSTREAM);
   //output motor 2 command
   fputs(ROBO.M2,ROBOSTREAM);
   //output carriage return
   fputc('\r',ROBOSTREAM);
}

/**
  * This Function queries the RoboTeq for the amps the motors are consuming and the voltage
  * of the batteries
**/
void query_robo()
{
   //------Local Variables------
  int i=0;              //used for looping / character count
  char rxchar;       //current character recieved from roboteq
  char rxvalues[5]="";     //recieved value of request
  char value[5]="";      //used to construct values to assign to global variables
  //-------Initialize local variables
  strcpy(value,"0x"); //value recieved will be concatenated on this, then atoi'ed
   i=0;                    //reset counter to after '0x'
   robo_timeout_error = 0; //reset timeout flag
  //-----query amps of motor 1-----
  fputs("?a\r",ROBOSTREAM);
  //------Recieve Response-------
   while(!robo_timeout_error)
   {
      rxchar = robo_timed_getc(); //gets character from roboteq
      //roboteq loops back all character recieved, then a '+' for command understood
      //filter these out if character is not'0-9' or 'A-F'
      if( (rxchar >= 0x30 && rxchar <= 0x39) || (rxchar >=41 && rxchar <=46) )
      {
         //accept character
         rxvalues[i] = rxchar;
         i++;
         //we only need four characters
         //if we have accepted four good characters, break out of while
         if(i>=4)
         {
//!DEBUG
//fprintf(BASESTREAM,"BROKEOUT\r\n");
            break;
         }
      }
   }
   //------Validity Check----------
   //if we didn't recieve four characters, we timed out, and should ignore what was returned
   if(strlen(rxvalues) < 4)
   {
      return;
   }
   //------Parse Value Recieved-----
//!DEBUG
//fprintf(BASESTREAM,"AMPS1:%X,%X,%X,%X\r\n",rxvalues[0],rxvalues[1],rxvalues[2],rxvalues[3]);
   //construct value for motor 1
   value[2] = rxvalues[0];
   value[3] = rxvalues[1];
   //set value for motor 1
   ROBO.amps1 = atoi(value);
   //construct value for motor2
   value[2] = rxvalues[2];
   value[3] = rxvalues[3];
   //set value for motor 2
   ROBO.amps2 = atoi(value);
   //divide by 2 if AX3500 - see datasheet
   if(ROBOTEQ_AX3500)
   {
      ROBO.amps1 /= 2;
      ROBO.amps2 /= 2;
   }

}