void sendInt(unsigned int intToSend){
	unsigned int upperByte;
	unsigned int lowerByte;

	upperByte=intToSend & 0xFF00;//clear lower bits
	upperByte=upperByte >> 8; //shift to make it a char
	lowerByte=intToSend & 0x00FF; //clear upper bits

	sendByte(upperByte);
	sendByte(lowerByte);
} //sendInt()

/**
 * Returns 1 if the caster wheel is dropped.
 */
uint8_t cm_read_caster_wheel_drop(void)
{
	sendByte(CmdSensors);
	padCommand();
	sendByte(SEN_BUMP_DROP);
	endCommand();
	
	if (readByte() & 0x10)
		return 1;
	else
		return 0;
}

/**
 * Returns 1 if the right bumper is pressed.
 */
uint8_t cm_read_right_bumper(void)
{
	sendByte(CmdSensors);
	padCommand();
	sendByte(SEN_BUMP_DROP);
	endCommand();
	
	if (readByte() & 0x01)
		return 1;
	else
		return 0;
}

/**
 * Sets the digital outputs on the Create's cargo bay connector.
 *   @param pin*: Turns the pin on if non-zero.
 * Note 1: Not used in simulations.
 * Note 2: Only available on Create robots. Does nothing on Roomba or Scooba robots.
 */
void cm_digital_outputs(const uint8_t &pin0, const uint8_t &pin1, const uint8_t &pin2)
{
	uint8_t b = 0;
	if (pin0)	b |= 0x01;
	if (pin1)	b |= 0x02;
	if (pin2)	b |= 0x04;
	
	sendByte(CmdOutputs);
	padCommand();
	sendByte(b);
	endCommand();
}

// Send a Midi CC message to a given channel, as a given controller 0-127, with given
//  value 0-127
void Midi::sendControlChange(unsigned int channel, unsigned int controller, unsigned int value)
{
    int status = STATUS_EVENT_CONTROL_CHANGE | ((channel - 1) & 0x0f);
    
    
    if (sendFullCommands_ || (lastStatusSent_ != status)) {
        sendByte(status);
    }
    
    sendByte(controller & 0x7f);
    sendByte(value & 0x7f);
}

void iPodSerial::sendCommand(
    byte mode,
    byte cmdByte1,
    byte cmdByte2)
{
    sendHeader();
    sendLength(1 + 1 + 1);
    sendByte(mode);
    sendByte(cmdByte1);
    sendByte(cmdByte2);
    sendChecksum();
}

inline void sendFrame1(unsigned int value)
{
    unsigned int checksum=0xaa;
	serial_write(HDLC_frameFlag);
	
	sendByte(value);
	
	checksum ^= value;
	sendByte(checksum);
	
	serial_write(HDLC_frameFlag);
}

void CompactQik2s9v1::motor1Forward(uint8_t speed)
{
	if ( speed > 127 )
	{
		sendByte(MOTOR1FORWARDFASTPACKET);
		sendByte(speed-127);	
	}
	else
	{
		sendByte(MOTOR1FORWARDPACKET);
		sendByte(speed);
	}
}

/*
 Reads a value from a ADXL345 register and returns it
 */
unsigned int accelReadReg(const Accel *a, int reg){
    int I2C = a->I2C;
    sendStart(I2C);                 // start transaction
    sendByte(I2C, a->write);        // write accel device
    sendByte(I2C, reg);             // specify device register
    repeatStart(I2C);
    sendByte(I2C, a->read);         // read accel device
    unsigned int d = readByte(I2C);          // grab data
    I2CAcknowledgeByte(I2C, 0);     // Send nack
    while(!I2CAcknowledgeHasCompleted(I2C));
    I2CStop(I2C);                   // end transaction
    return d;
}

void KDC_5060R_receive(void){

  GPIO_ResetBits(CE_Port,CE_Pin);
  sendByte(CMD_OUTPUT);
  GPIO_SetBits(CE_Port,CE_Pin);

  key_data[0]=sendByte(0);
  key_data[1]=sendByte(0);
  key_data[2]=sendByte(0);
  key_data[3]=sendByte(0);

  GPIO_ResetBits(CE_Port,CE_Pin);
}

byte read_adcNS(byte ch, u16* iv)	// Read ADC, without entering SLEEP mode
	{
	byte res[2];
	if ((ch < 0) || (ch > 12))
		return INVARG;
	sendByte(READADC);
	sendByte(ch);
	*res = COMERR;
	sread(1, res);
	if (*res != 'D') return *res;
	if(sread(2, res) != 2) return COMERR;
	*iv = res[0] | (res[1] << 8);
	return 0;
	}

void sendString(void){
	
	consoleDrawText(7, 5, "sending...");
	
	int length = strlen(str);
	
	sendByte(length);
	
	int i;
	for(i=0;i<length;i++){
		sendByte(str[i]);
	}

}

uint8_t I2C::write(uint8_t address, uint8_t registerAddress, uint8_t data)
{
  returnStatus = 0;
  returnStatus = start(); 
  if(returnStatus){return(returnStatus);}
  returnStatus = sendAddress(SLA_W(address));
  if(returnStatus){return(returnStatus);}
  returnStatus = sendByte(registerAddress);
  if(returnStatus){return(returnStatus);}
  returnStatus = sendByte(data);
  if(returnStatus){return(returnStatus);}
  returnStatus = stop();
  return(returnStatus);
}

//------------------- Modifiers for Capture ------------------------------
byte disable_actions()
	{
	// Disable all modifiers to the capture call. The capture calls will be set to 
	// do analog triggering on the first channel captured.
	byte res[1];

	sendByte(SETACTION);
	sendByte(AANATRIG);
	sendByte(0);			//Self trigger on channel zero means the first channel captured
	*res = COMERR;
	sread(1,res);
	if(*res != 'D') return *res;
	return 0;
	}

void ChainableLED::setColorRGB(byte led, byte red, byte green, byte blue)
{
    // Send data frame prefix (32x "0")
    sendByte(0x00);
    sendByte(0x00);
    sendByte(0x00);
    sendByte(0x00);
    
    // Send color data for each one of the leds
    for (byte i=0; i<_num_leds; i++)
    {
        if (i == led)
        {
            _led_state[i*3 + _CL_RED] = red;
            _led_state[i*3 + _CL_GREEN] = green;
            _led_state[i*3 + _CL_BLUE] = blue;
        }
                    
        sendColor(_led_state[i*3 + _CL_RED], 
                  _led_state[i*3 + _CL_GREEN], 
                  _led_state[i*3 + _CL_BLUE]);
    }

    // Terminate data frame (32x "0")
    sendByte(0x00);
    sendByte(0x00);
    sendByte(0x00);
    sendByte(0x00);
}

int SoftwareI2CPort::send(uint8_t address, uint32_t subaddress, uint32_t subaddressLength, const void * data, uint32_t dataLength)
{
    fillAddressBuffer(address, subaddress, subaddressLength);
    
    m_scl.setDirection(GpioPin::kInput);
    m_sda.setDirection(GpioPin::kInput);

    m_scl.clear();
    m_sda.clear();

    sendStart();

    bool ret = sendByte((address << 1), true);
    if (!ret)
    {
        sendStop();
    
        return I2C_ERROR_NO_SLAVE_ACK;
    }

    const uint8_t * subaddressData = (const uint8_t *)&m_addressBuffer[1];
    while (subaddressLength--)
    {
        ret = sendByte(*subaddressData++, true);
        if (!ret)
        {
            sendStop();
    
            return I2C_ERROR_GOT_NAK;
        }
    }

    const uint8_t * transfer_data = reinterpret_cast<const uint8_t *>(data);
    uint32_t u16Size = dataLength;
    while (u16Size--)
    {
        ret = sendByte(*transfer_data++, true);
        if (!ret)
        {
            sendStop();
    
            return I2C_ERROR_GOT_NAK;
        }
    }

    sendStop();
    
    return I2C_OK;
}

void FrSkySportSingleWireSerial::sendEmpty(uint16_t dataTypeId)
{
  if(port != NULL)
  {
    setMode(TX);
    sendByte(0x00);
    uint8_t *bytes = (uint8_t*)&dataTypeId;
    sendByte(bytes[0]);
    sendByte(bytes[1]);
    for(uint8_t i = 0; i < 4; i++) sendByte(0x00);
    sendCrc();
    port->flush();
    setMode(RX);
  }
}

/**
 * Controls the LEDs on top of the robot.
 *   @param play Turns the Play light on if non-zero.
 *   @param advance Turns the Advance light on if non-zero.
 *   @param power_color Sets the shade of the Power light between green (at 0) and orange (at 255).
 *   @param power_intensity Sets the brightness of the Power light from off (at 0) to full brightness (at 255).
 */
void cm_set_leds(const uint8_t &play, const uint8_t &advance, const uint8_t &power_color, const uint8_t &power_intensity)
{
	uint8_t b = 0;
	if (play)		b |= LEDPlay;
	if (advance)	b |= LEDAdvance;
	
	sendByte(CmdLeds);
	padCommand();
	sendByte(b);
	padCommand();
	sendByte(power_color);
	padCommand();
	sendByte(power_intensity);
	endCommand();
}

void setLeds(BYTE out[2]) {

    // We start with the lowest bit, and moving towards the higher
    sendByte(out[1]);
    sendByte(out[0]);

    
    lLatch_On();
    Nop();
    Nop();
    Nop();
    Nop();
    Nop();
    Nop();
    lLatch_Off();
}

/**************************************************************************//**
*
* main
*
* @brief      main function
*
* @param      -
*
* @return     -
*
******************************************************************************/
void main(void)
{
  uint8_t data;

  // source ACLK with internal VLO clock
  BCSCTL3 |= LFXT1S_2;

  // Set DCO register value based on the selected input clock frequency
  BCSCTL1 = BCSCTL1_VAL;
  DCOCTL = DCOCTL_VAL;

  // set GPIO as UART pins - P1.1=UCA0RXD, P1.2=UCA0TXD
  P1SEL = BIT1 + BIT2 ;
  P1SEL2 = BIT1 + BIT2;

  // setup USCI UART registers
  UCA0CTL1 |= UCSSEL_2 + UCSWRST;
  UCA0BR0 = USCI_BR0_VAL;
  UCA0BR1 = USCI_BR1_VAL;
  UCA0MCTL = USCI_BRS_VAL;
  UCA0CTL1 &= ~UCSWRST;

  // do somekind of splash screen
  splash();

  while(1)
  {
    if(rcvByte(&data) == true)
    {
      // echo back the received data
      sendByte(data);
    }
  }
}