#pragma config(ProgramType, NonCompetition)
#pragma config(UART_Usage, UART1, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(Sensor, in1,    pot1,           sensorPotentiometer)
#pragma config(Sensor, in2,    pot2,           sensorPotentiometer)
#pragma config(Motor,  port1,           LeftWheel,     tmotorVex393, openLoop)
#pragma config(Motor,  port8,           RightWheel,    tmotorVex393, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*************************************************************************************
*         PIC based code for binary serial transmission
*     Should Run on a Cortex as well
*     Primarily to use the PIC as a motor controller
*     for a more capable compute platform
*     Example is provided for the VEX Robot Enthusiast community
*     Much help was provided by James Pearman and CMU
*     This is provided with the hope that improvements will be shared via the Vex Forum
*     Copyright 11/19/2012 Kevin Barrett
*     All 4 timers are used
*         T1 is used for main program timer functions Reset prior to overflow
*     T2 is used in RX char timeout checks
*     T3 is used for packet to packet timeout
*        to stop all motors remotely controlled if no input received
*         T4 is managed by the run time clock (reset every second)
*
*     It is built to control 2 drive motors with Optical Shaft Encoders
*         Added Ultrasonic (SONAR) 1/12/13
**************************************************************************************/

#define  VEXDATAOFFSET          3       // Bytes of static header data in the messages
#define  VEX_DATA_BUFFER_SIZE  20       // size of internal message buffer (older PICs have 20 byte UART buffer)
                                                         // (both tx and rx buffers must be this size or smaller)
#define  RX_MESSAGE_LENGTH 12             // 8 motors 2 bytes A/D or 2 encoders (PIC)
#define  TX_MESSAGE_LENGTH 12             // 8 motors 2 bytes A/D or 2 encoders (PIC)
#define  SYNC_1 0x56
#define  SYNC_2 0xA5
#define TIME_STEP 40                            // minimum wait millisec between transmissions

// because there is no debug stream capability in a PIC program
// these variables are being placed in Global at the top of the listing
// to allow for easy viewing in the RobotC debugger
unsigned int packetRecievedProcessed=0; // how many packets Rx and processed
short localPotentiometer1Value=0,localPotentiometer2Value=0;
short remotePotentiometer1Value=0,remotePotentiometer2Value=0;

int hours=0,minutes=0,seconds=0; // global variables for the run time clock
unsigned int goodPackets=0,badPackets=0; // keep track of good and bad packet reciept

struct {
      unsigned char  header_56;// expect a Sync
      unsigned char  header_A5;// pattern of sorts
      unsigned char  messageLength;// also need to properly set the message length
      //3 bytes (my bookkeeping)
      unsigned char  messageID;
      unsigned char  hours; // hours since robot turned on
      unsigned char  minutes; // minutes since robot turned on
      unsigned char  seconds; // seconds since robot turned on
      unsigned char  potentiometer1High; // Potentiometer readings
      unsigned char  potentiometer1Low;  // one analog port requires
                                                           // 2 bytes of data as values range from 0 to 4095?
      unsigned char  potentiometer2High; // Potentiometer readings
      unsigned char  potentiometer2Low;  // one analog port requires
                                                           // 2 bytes of data as values range from 0 to 4095?
      unsigned char  checksum;
      //12 bytes (my bookkeeping to check message length)
     } outboundData;

union  { // this definition allows the memory to be accessed in two ways
   outboundData data; // as a struct and by the struce member names
   unsigned char    buffer[VEX_DATA_BUFFER_SIZE]; // as a char array which allows
                                                                           // easy serial transmission
  } txVexData;

   txVexData myLastGoodVexDataRx; // local copy for last good received data
   txVexData myVexDataTx; // local copy for transmitted data
   txVexData myVexDataRx; // local copy for received data


void VexTxDataInit() {  // must call at least once before using serial communication
      memset(myVexDataTx.buffer, 0,VEX_DATA_BUFFER_SIZE ); // clear the whole buffer
      // Initialize packet
      myVexDataTx.data.header_56    = SYNC_1; // Set Sync Pattern one
      myVexDataTx.data.header_A5    = SYNC_2; // Set Sync Pattern two
      myVexDataTx.data.messageLength=TX_MESSAGE_LENGTH; // sets the header byte to the proper message length
}

void VexRxDataInit() {  // must call at least once before using serial communication
      memset(myVexDataRx.buffer, 0,VEX_DATA_BUFFER_SIZE ); // clear the whole buffer
      memset(myLastGoodVexDataRx.buffer, 0,VEX_DATA_BUFFER_SIZE ); // clear the whole buffer     
}

void initSerialPort () { // must call at least once before using serial communication
  // There is only one port for general use on a PIC so set it's baud rate
  configureSerialPort(UART1, uartUserControl);
  setBaudRate(UART1, baudRate115200);
  return;
  }


void timeTag() { // provides HH:MM:SS based run time clock
   static short secondsTimeTic=0;
    secondsTimeTic = time1[T4];
    if (secondsTimeTic> 1000) {
       seconds++;
       ClearTimer(T4);// used to reset Timer 4 every 1000 ms to avoid overflow
      }
      if (seconds ==60) {
         seconds = 0;
         minutes++;
      }
      if (minutes == 60) {
         minutes =0;
         hours++;
      }
    return;
  }

void sendVexData() // transmit buffer
   {
  for (int i=0; i <= TX_MESSAGE_LENGTH-1; i++)
     {
    sendChar(UART1, myVexDataTx.buffer[i]);
    while(!bXmitComplete(UART1)) {abortTimeslice();}// test the boolean Xmit Complete flag
    }
   }

   
   void copyVexData() // make a copy of the last good data received
   {
  for (int i=0; i <= RX_MESSAGE_LENGTH-1; i++)
     {
    myLastGoodVexDataRx.buffer[i] = myVexDataRx.buffer[i];
    }
   }

////////////////////////////////////////////////

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  Receive data timeout code                                                               */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
static  int timeout     =0; // global variable for persisting timeout duration (0 = timeout not active)

void StartTimeout( )
{
    // Start timeout
      ClearTimer(T2);
    timeout = time1[T2];
}

void StopTimeout( )
{
    // Stop timeout
    timeout = 0;
}

int CheckTimeout( )
{
    // Is timeout running ?
    if( timeout > 0 )
        {
        if((time1[T2] - timeout) < 10)
            return(0);
        else
            return(1);
        }
    return(0);
}
/********************************************************************
Motor Timeout code
*********************************************************************/
static int motorTimeout=0; // global variable for persisting motor drive timeout duration (0 = timeout not active)

void StartMotorTimeout( )
{
    // Start timeout
      ClearTimer(T3);
      motorTimeout=1;
}


int CheckMotorTimeout( )
{
    // Is timeout running ?
    if( motorTimeout > 0 )
        {
        if(time1[T3]  > 80) // 80 millisec ~ 4 packets worth of messages
            return(1);
        else
            return(0);
        }
    return(0);
}


void VexDataChecksumTx(  ) // calculate checksum the same way as the XBEE
{
    int  i;
    int  cs = 0;

    for(i=VEXDATAOFFSET;i<(myVexDataTx.data.messageLength-1);i++)
      cs = cs + myVexDataTx.buffer[i];
    myVexDataTx.data.checksum = 0xFF - (cs & 0xFF);
    return;
}

void VexDataChecksumRx(  ) // calculate checksum the same way as the XBEE
{
    int  i;
    int  cs = 0;

    for(i=VEXDATAOFFSET;i<abs(myVexDataRx.data.messageLength)-1;i++)
      cs = cs + myVexDataRx.buffer[i];
    myVexDataRx.data.checksum = 0xFF - (cs & 0xFF);
    return;
}


task serialRxTask()
{
    static  int serialRxState=0;
    static  short serialDataReceived = 0;
    short c;

    while (true) {
       // check for a received character
       c = getChar(UART1);
       if( c == -1 )
         {
         // -1 implies no received char
         // Check for inter char timeout
         if( CheckTimeout() == 1 ){
           serialRxState = 0;
           StopTimeout();
           }
         abortTimeslice(); // ends thread execution
         continue;
         }
       // A new character has been received so process it.

       // Start inter char timeout used to detect Partial Packet Rx
       StartTimeout();
         // Rx Data state machine
       // Where are we in the message parsing process ?
       switch( serialRxState )
         {
         case  0:
             // look for first header byte
             if( (unsigned char)c == SYNC_1 ) {
              serialRxState++;
             }
             else {
               StopTimeout();
             }
             break;

         case  1:
             // look for second header byte
             if( (unsigned char)c == SYNC_2 ) {
               serialRxState++;
             }
             else
               {
               // Bad message
               StopTimeout();
               serialRxState = 0;
               badPackets++;
               }
             break;


         case  2:
             // next is message length
             myVexDataRx.data.messageLength=c;
             // check to ensure RX datagram is within the size we can accept
             // if it is too large it would cause a memory exception so reject it
             if (myVexDataRx.data.messageLength > VEX_DATA_BUFFER_SIZE) {
               // Bad message
               StopTimeout();
               serialRxState = 0;
               badPackets++;
                     } else { // within bounds so we can accept it although it still might not be good
             serialRxState++;
             serialDataReceived =serialRxState;
              }
             break;

         case  3:
             // receive the data packet
               
             myVexDataRx.buffer[serialDataReceived] = c;
             serialDataReceived++;
             // repeat this step until message length data bytes are received
             if( serialDataReceived >= myVexDataRx.data.messageLength-1){
               serialRxState++;
             }
             // check to avoid buffer overflow conditions
             if( serialDataReceived >= (VEX_DATA_BUFFER_SIZE-1) )
               {
               // Error
               StopTimeout();
               serialRxState = 0;
               badPackets++;
               }
             break;

         case  4:
             // Received checksum byte
             // stop timeout
             StopTimeout();
             // calculate received checksum
             VexDataChecksumRx(  );
             // comapare checksums

             if( (unsigned char)c == myVexDataRx.data.checksum)
               { // Good data
               goodPackets++;
               // Copy Good Data Received to last good data structure
                     // probably should add semaphore code to avoid read access while
                     // writing the copy but thats another day's task
                     copyVexData();
               }
             else
               {
                     badPackets++;
               }
             // done
             serialRxState = 0;
             break;

         default:
         } // end case
       } // end while
} // end task

/*
Main Program


*/
task main()
{

// do these steps once per run
initSerialPort ();
VexTxDataInit();  // initialize data structures
VexRxDataInit();

// Start up the receive serial data task
StartTask(serialRxTask);

while (true) { // begin main program loop
   // Update Sensor Values at the start of the loop
   localPotentiometer1Value  = SensorValue[pot1];
   localPotentiometer2Value  = SensorValue[pot2];
  timeTag(); // update runtime in Hours, Minutes, and Seconds


  if (time1[T1]> TIME_STEP) {
     ClearTimer(T1);
     // Send Data every time step
         // store readings in outboundData struct
       myVexDataTx.data.hours                       = hours; // these are 1 byte copies
       myVexDataTx.data.minutes                    = minutes;
       myVexDataTx.data.seconds                    = seconds;
       myVexDataTx.data.potentiometer1High  = (localPotentiometer1Value >> 8) & 0xFF; // data larger than a byte
       myVexDataTx.data.potentiometer1Low   = localPotentiometer1Value & 0xFF; // must be 'bit banged' into byte sized chunks
       myVexDataTx.data.potentiometer2High  = (localPotentiometer2Value >> 8) & 0xFF;
       myVexDataTx.data.potentiometer2Low   = localPotentiometer2Value & 0xFF;
       VexDataChecksumTx(  );
       sendVexData();
     } // end if Time step  */
     
  // Check if good packet received
     if ( goodPackets > packetRecievedProcessed) {
        packetRecievedProcessed = goodPackets;
        remotePotentiometer1Value = myLastGoodVexDataRx.data.potentiometer1High << 8;
        remotePotentiometer1Value = remotePotentiometer1Value + myLastGoodVexDataRx.data.potentiometer1Low;
        remotePotentiometer2Value = myLastGoodVexDataRx.data.potentiometer2High << 8;
        remotePotentiometer2Value = remotePotentiometer2Value + myLastGoodVexDataRx.data.potentiometer2Low;
     }
     
   }// end main program while loop

} // end task main