Using timer interrupts in a library

I know that the FIFO library compiles fine. I've worked out many general errors within the RCXIR library, but the library is new (certainly far from finished), and most likely some errors still exist (won't know until it compiles without error for once).

RCXIR_Test.ino

#include <RCXIR.h>

void setup()
{
  
  Serial.begin(2400);
  Serial.println("");
  
  // This code will only run once, after each powerup or reset of the board

}

void loop()
{
  // This code will loops consecutively

}

RCXIR.h

/*
*  Matthew Richardson
*  matthewrichardson37<at>gmail.com
*  http://mattallen37.wordpress.com/
*  Initial date: Sep. 26, 2012
*  Last updated: Sep. 26, 2012
*
*  You may use this code as you wish, provided you give credit where it's due.
*
*  This is a library for the Arduino to send and receive IR messages with a Lego RCX.
*/

#ifndef RCXIR_h
#define RCXIR_h

#include "Arduino.h"
#include <FIFO.h>

// Variables...
// Use static for .cpp file specific variables.
// define variables in .h as well, using extern for global variables.

static volatile byte ISRloc = 0;                 // A pointer into the byte currently being sent. 0 is inactive, 1-11 for active position (based on start bit, data bits, parity, and stop bit).
static volatile byte ISRlocPul = 0;              // A pointer into the pulse currently being sent. 0 is inactive, 1-32 for active position.
static byte BIT[9];                              // An array that will contain the bit values of the byte being sent (and the parity bit).

static byte Pin_tx;
static byte Pin_rx;

FIFOClass FIFO;

byte RCXIRsetup(byte pin_tx, byte pin_rx);
byte RCXIRflush();
void RCXIRwrite(byte ByteCount, byte * OutArray);
int  RCXIRread();

void __Trigger(void);
void __SetTimer(int);
void __SetTimerUS(unsigned int);
void __SetTimerMS(unsigned int);

#endif

RCXIR.cpp

/*
*  Matthew Richardson
*  matthewrichardson37<at>gmail.com
*  http://mattallen37.wordpress.com/
*  Initial date: Sep. 26, 2012
*  Last updated: Sep. 26, 2012
*
*  You may use this code as you wish, provided you give credit where it's due.
*
*  This is a library for the Arduino to send and receive IR messages with a Lego RCX.
*/

#include "RCXIR.h"



byte RCXIRsetup(byte pin_tx, byte pin_rx){
  Pin_tx = pin_tx;
  Pin_rx = pin_rx;
//  return FIFO.CreateBuffer(64);
}

byte RCXIRflush(){
 
}

void RCXIRwrite(byte ByteCount, byte * OutArray){

}

int RCXIRread(){

}

void __Trigger(){
  __SetTimerUS(5);
}

void __SetTimerUS(unsigned int Time){    // US to wait. Up tp 32767.5 us (32.7675 ms).
  cli();
  TCCR1A = 0;
  TCCR1B = 0;
  OCR1A = Time * 2;
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS11);
  TIMSK1 |= (1 << OCIE1A);
  sei();
}

void __SetTimerMS(unsigned int Time){    // MS to wait. Up to 1048.56 ms.
  long L_Time = Time * 125;
  L_Time = L_Time / 2;
  
  cli();
  TCCR1A = 0;
  TCCR1B = 0;
  OCR1A = L_Time;
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS12);
  TIMSK1 |= (1 << OCIE1A);
  sei();
}

void __SetTimer(int Length){
  switch(Length){
    case 0:
      Length = 13;  // should be 13.157894736842105263157894736842 uS based on 38kHz carrier. Should be 13.020833333333333333333333333333 based on 2400 baud.
    break;
    case 1:
      Length = 417; // should be 416.66666666666666666666666666666 uS based on 2400 baud.
    break;
  }
  __SetTimerUS(Length);
}

/*

  ISRloc controls which bit is being sent
  ISRlocPul controls which pulse of a 0 bit is being sent

*/

ISR(TIMER1_COMPA_vect)            // Timer interrupt triggered.
{
  if(ISRloc){                     // Was I in the middle of a byte?
__Re_Evaluate_Pulse:
    
    if(ISRlocPul){                // Was I in the middle of sending a bit?
      if(ISRlocPul%2){            // If odd, send a HIGH
        digitalWrite(Pin_tx, HIGH);
      }
      else{                       // else send a LOW
        digitalWrite(Pin_tx, LOW);         
      }
      
      if(ISRlocPul==32){          // Was I at the end?
        __SetTimer(0);            // Come back in a short time
        ISRlocPul = 0;            // Reset the pulse pointer
        ISRloc ++;                // Advance to the next of the n instructions per byte.
      }
      else{
        __SetTimer(0);            // Come back in a short time
        ISRlocPul++;              // Add to the pulse pointer
      }
    }
    
    if(!ISRlocPul){               // If I am not sending a bit, or it just finished.
      if(ISRloc == 1){            // Start bit
        ISRlocPul = 1;            // Set ISRlocPul to 1, so that it will initialize and send.
        goto __Re_Evaluate_Pulse; // Go back and re evaluate, so that it sends the first of the pulse.      
      }
      else if(ISRloc == 11){      // Stop bit
        __SetTimer(1);            // Don't send anything during this time period
        ISRloc = 0;               // Reset the ISRloc, so that at the end of the time period, it will get a new byte.                                                    
      }
      else{                       // I am sending a byte, and on bits 0-7 or parity
        if(BIT[ISRloc-2]){        // If the bit is 1
          __SetTimer(1);          // Don't send anything during this time period
          ISRloc++;
        }
        else{                       // else if the bit is 0
          ISRlocPul = 1;            // Set ISRlocPul to 1, so that it will initialize and send.
          goto __Re_Evaluate_Pulse; // Go back and re evaluate, so that it sends the first of the pulse.
        }        
      }
    }
  }
  else{                           // I was not in the middle of a message.

  }
}

FIFO.h

#ifndef FIFO_h
#define FIFO_h

#include "Arduino.h"

class FIFOClass
{    
  public:

    FIFOClass(void);

    byte CreateBuffer(unsigned int length);      // True if it was successful, otherwise false.

    byte BufferEmpty();                          // True if the buffer is empty.
    byte BufferFull();                           // True if the buffer is full.
    int BytesAvailableToRead();                  // How many bytes are available to read.
    int FreeBytesSpace();                        // How much room is available.
    byte AddElement(byte input);                 // Add an element.
    byte AddArray(byte length, byte * input);    // Add an array to the buffer.
    int GetElement(byte GoOn = true);            // Read an element, and optionally set to go on.
    void FlushBuffer();                          // Clear the buffer.

  private:
  
    uint8_t * _FB;
//    byte FB[BufferSize];     // Full (entire) Buffer
    int _EP;                  // End Pointer
    int _SP;                  // Start Pointer
    unsigned int _BufferSize;

}; 

#endif

FIFO.cpp

#include "FIFO.h"

FIFOClass::FIFOClass()
{

}

byte FIFOClass::CreateBuffer(unsigned int length){
  length += 1;
  free(_FB);
  _FB = (uint8_t*)malloc(length);
  if (_FB == NULL)
		return 0;
  _BufferSize = length;
  _SP = 0;
  _EP = 0;
  return 1;
}

byte FIFOClass::BufferEmpty(){     // True if the buffer is empty.
  return _SP==_EP;
}

byte FIFOClass::BufferFull(){      // True if the buffer is full.
  return (_SP-1==_EP || ((_SP==0)&&(_EP == _BufferSize-1)));    // If EP precedes the ST by 1, with wrap-around.
}

int FIFOClass::BytesAvailableToRead(){
  if(BufferEmpty())return 0;
  int result;
  if(_SP + 1 < _EP){
    result = _EP - _SP - 1;
  }
  else if(_SP > _EP){
    result = (_EP + _BufferSize) - _SP - 1;
  }
  return result;
}

int FIFOClass::FreeBytesSpace(){
  return _BufferSize - 1 - BytesAvailableToRead();
}

byte FIFOClass::AddElement(byte input){
  byte result = false;
  if(BufferFull()){    // The buffer is full, so that means we are trying to over-write.
    _SP++;               // Move my start pointer ahead one.
    result = true;      // Set the return to true, bebause we had to over-write data.
  }
  _FB[_EP] = input;
  _EP++;
  if(_EP == _BufferSize)_EP = 0;
  if(_SP == _BufferSize)_SP = 0;
  return result;
}

byte FIFOClass::AddArray(byte length, byte * input){
  byte result = false;
  for(int i = 0; i < length; i++){
    if(BufferFull()){    // The buffer is full, so that means we are trying to over-write.
      _SP++;               // Move my start pointer ahead one.
      if(_SP == _BufferSize)_SP = 0;
      result = true;
    }
    _FB[_EP] = input[i];
    _EP++;
    if(_EP == _BufferSize)_EP = 0;
  }
  return result;
}

int FIFOClass::GetElement(byte GoOn){
  int result;
  if(BufferEmpty()){
    result = -1;
  }
  else{
    result = _FB[_SP];
    if(GoOn){
      _FB[_SP] = 0;
      _SP++;
      if(_SP == _BufferSize)_SP = 0;
    }
  }
  return result;
}

void FIFOClass::FlushBuffer(){

//  byte trash;
//  while(_BytesAvailableToRead()){     // Could probably just ArrayInit the buffer to 0s with _BufferSize elements, and set SP and EP to 0.
//    trash = GetElement();
//  }

  _SP = 0;
  _EP = 0;
  for(int i = 0; i < _BufferSize; i++){
    _FB[i] = 0;
  }
}