void lcd_data_write( unsigned char data )
{
	unsigned char TEMP = data;
	RS_H;		// Register Select High for Character

	if(TEMP & 0b10000000){D3_H;} else{D3_L;}
	if(TEMP & 0b01000000){D2_H;} else{D2_L;}
	if(TEMP & 0b00100000){D1_H;} else{D1_L;}
	if(TEMP & 0b00010000){D0_H;} else{D0_L;}
	E_H; delayMs(1,1); E_L;		// Pulse the Enable Pin

	if(TEMP & 0b00001000){D3_H;} else{D3_L;}
	if(TEMP & 0b00000100){D2_H;} else{D2_L;}
	if(TEMP & 0b00000010){D1_H;} else{D1_L;}
	if(TEMP & 0b00000001){D0_H;} else{D0_L;}
	E_H; delayMs(1,1); E_L;		// Pulse the Enable Pin
}

void lcd_command_write(unsigned char command)
{
	unsigned char TEMP = command;
	RS_L;		// Register Select Low for Command

	if(TEMP & 0b10000000){D3_H;} else{D3_L;}
	if(TEMP & 0b01000000){D2_H;} else{D2_L;}
	if(TEMP & 0b00100000){D1_H;} else{D1_L;}
	if(TEMP & 0b00010000){D0_H;} else{D0_L;}
	E_H; delayMs(1,1); E_L;		// Pulse the Enable Pin

	if(TEMP & 0b00001000){D3_H;} else{D3_L;}
	if(TEMP & 0b00000100){D2_H;} else{D2_L;}
	if(TEMP & 0b00000010){D1_H;} else{D1_L;}
	if(TEMP & 0b00000001){D0_H;} else{D0_L;}
	E_H; delayMs(1,1); E_L;		// Pulse the Enable Pin
}

void do_stop()
{
	can_interrupt = true;
	car_set_left_speed(0);
	car_set_right_speed(0);
        CALL_BACK(STOP,FINISH);
	delayMs(100);
}

/**********************************************************
 * Performs a pin reset on the target
 **********************************************************/
void hardResetTarget(void)
{
  GPIO_PinOutClear((GPIO_Port_TypeDef)RESET_PORT, RESET_PIN);
  
  delayMs(50);
  
  GPIO_PinOutSet((GPIO_Port_TypeDef)RESET_PORT, RESET_PIN);
}

void do_turn_back()
{
	car_set_left_speed(0);
	car_set_left_speed(0);
	delayMs(100);
	car_change_direction();
	CALL_BACK(TURN_BACK,FINISH);
}

void setSerialDestination(uint8_t dest) {
// Which serial port should byteTx and byteRx talk to?
// Ensure any pending bytes have been sent. Without this, the last byte sent
// before calling this might seem to disappear.
delayMs(10);
// Configure the port.
if(dest == SERIAL_CREATE) {
PORTB &= ˜0x10;
} else {
PORTB |= 0x10;
}
// Wait a bit to let things get back to normal. According to the docs, this
// should be at least 10 times the amount of time needed to send one byte.
// This is less than 1 millisecond. We are using a much longer delay to be
// super extra sure.
delayMs(10);
}

/*******************************************************************************
* Description : 
* Syntax      : 
* Parameters I: 
* Parameters O: 
* return      : 
*******************************************************************************/
void CC1101_debug(void)
{
#ifdef CC1101_RX_Debug

    //..
    
    
#endif
    
#define CC1101_TX_Debug    
#ifdef CC1101_TX_Debug   

    //..
//    memset(RF.TxBuff, 0xAA, RF_payloadSize);
//    RF.TxBuff[0] = 0xFE;
//    RF.TxBuff[1] = 0xFE;
//    RF.TxBuff[2] = 0x68;
//    RF.TxBuff[4] = 0x00;
//    RF.TxBuff[5] = 0x00;
//    RF.TxBuff[6] = 0x00;
//    RF.TxBuff[7] = 0x00;
//    RF.TxBuff[8] = 0x00;
//    RF.TxBuff[9] = 0x22;
//    RF.TxBuff[10] = 0x68;
//    RF.TxBuff[11] = 0x11;
//    RF.TxBuff[12] = 0x04;
//    RF.TxBuff[13] = 0x3F;
//    RF.TxBuff[14] = 0x34;
//    RF.TxBuff[15] = 0x33;
//    RF.TxBuff[16] = 0x37;
//    RF.TxBuff[17] = 0xE4;
//    RF.TxBuff[18] = 0x16;    
    
    while (1)
    {
        CC1101_Send(RF.TxBuff, RF_shortPayloadSize);
        delayMs(20);
    }    
//    FE FE 68 00 00 00 00 00 22 68 11 04 3F 34 33 37 E4 16 //时间
    
//    TX_buff[0] = 0x66;             // test 测试数据帧  显示表地址
//    TX_buff[1] = 0xCC;
//    TX_buff[2] = 0xCC;
//    TX_buff[3] = 0xCC;
//    TX_buff[4] = 0xCC;
//    TX_buff[5] = 0xAF;
//    TX_buff[6] = 0x33;
//    TX_buff[7] = 0x33;
//    TX_buff[8] = 0x33;
//    TX_buff[9] = 0x33;
//    TX_buff[10] = 0x33;
//    TX_buff[11] = 0x33;
//    TX_buff[12] = 0x77;
    
#endif
    

}

void pumpLEDOffCommand()
{
    delayMs(200);

    fillHeaderBuffer(IR_HDR_LED);
    fillCommandBuffer(IR_CMD_LED_OFF);
    sendIRCommandBlocking();
    fillHeaderBuffer(IR_HDR_HANSUN);
}

s8 RC522_reset()
{
    RC522_RST = 1;
    delayMs(10);
    RC522_RST = 0;
    delayMs(10);
    RC522_RST = 1;
    delayMs(100);
    RC522_SPI_read(0x02);
    RC522_SPI_write(CommandReg, Cmd_SoftReset);
    RC522_SPI_write(ModeReg, 0x3D);//和Mifare卡通讯，CRC初始值0x6363
    RC522_SPI_write(TReloadReg_L, 30);
    RC522_SPI_write(TReloadReg_H, 0);
    RC522_SPI_write(TModeReg, 0x8D);
    RC522_SPI_write(TPrescalerReg, 0x3E);
    RC522_SPI_write(TxASKReg, 0x40);
    return MI_OK;
}

/*
 *  Data  2016.5.22  Night
 *  Log   取消速度加减控制
 *        速度环变积分控制
 *        方向换取消不完全微分控制 加入分段控制
 *        直立环限幅了
 *        测试fabs（）会改变内部的值吗
 */
void main (void)
 {
   DisableInterrupts;
    
    System_Init();
    //----------蜂鸣器-----------------------------------------------------------------------------------------------
    //  PTA9_O = 1;
    //  delayMs(10);
    //  PTA9_O = 0;     
    
    delayMs(30);
    SetNeutral();
    
    
    EnableInterrupts;
    
    //---------------------------等待车子到达直立---------------------------------------------------------------------
    while( g_nCarControlFlag==0 )
     {
        WaitCarStand();
        LED_FLOAT_PRINT(0,  0,"ANGLE: ",CAR_ANGLE_NOW, 4);
     }
    g_nDirectionPrior=0;// 1为方向控制优先 0为速度控制优先
    
    /*
    *  1：启动 
    *  0：关闭
    */
    g_nAngleControlFlag = 1;	/*****ANGLE_CONTROL_START*******/
    g_nSpeedControlFlag = 1;	/*****SPEED_CONTROL_START*******/
    g_nDirectionControlFlag = 1;  /*****DIRECTION_CONTROL_START***/
    
    while(1)
     { 
        //-----------------------数据发送选择区-----------------------------------
        //    switch(4)
        //    {
        //    case 1: SendData1();break;//1.MMA8451_Value   2.Angle_dot   3.Angle
        //    case 2: SendData2();break;//1.g_fAngleControlOut  2.g_fSpeedControlOut 3.g_fDirectionControlOut;
        //    case 3: SendData3();break;//1.g_nLeftMotorPulse 2.g_nRightMotorPulse
        //    case 4: SendData4();break;//1.fAngleAndSpeed  2.g_fCarSpeed
        //    case 5: SendData5();break;//1.VOLTAGE_LEFT    2.VOLTAGE_RIGHT
        //    case 6: SendData6();break;
        //    default:break;
        //    }
        //    LPLD_LPTMR_DelayMs(15);
        
        //-----------------------OLED显示区----------------------------------
        LED_FLOAT_PRINT(0,  0,"ANGLE: ",CAR_ANGLE_NOW, 4);
        LED_FLOAT_PRINT(0,  2,"L: ", VOLTAGE_LEFT, 4);
        LED_FLOAT_PRINT(0,  3,"R: ", VOLTAGE_RIGHT, 4);
        LED_FLOAT_PRINT(0,  5,"D:", g_fDirection, 4);
        
        
        
     }
 }

void goRightNoSuper() {
	moveXAdjusted(30);
	moveY(0);
	while(analog(0) < 127) {
		delayMs(50);
	}
	moveY(0);
	moveX(0);
}

void moveRight() {

	while(1) {
		moveY(0);
		moveX(30);
		while(digitalInput(0)) {
			delayMs(50);
			if(analog(0) >127) {
				return;
			}
		}
		moveX(0);
		moveY3(25);
		while(!digitalInput(0)) {
			delayMs(50);
		}
	}
}

void goForwards()
{
	moveX(10);
	moveY(30);
	while(!digitalInput(0)) {
		delayMs(50);
	}
	moveY(0);
}

void motorTest() 
{
	clearScreen();
	printString("motor test");
	buttonWait();
	clearScreen();
	printString("forwards");
	moveY(10);
	delayMs(2000);
	moveY(0);
	buttonWait();
	clearScreen();
	printString("right");
	moveX(10);
	delayMs(2000);
	moveX(0);
	clearScreen();
	printString("done");
}

u8 MPU_getTemp(float *temp)
{
	u8 tmp[2];
	short data;
	MPU_read(2, MPU_TEMP_OUT_H, tmp);
	data = ((s16)(tmp[0]) << 8) | tmp[1];
	*temp = 36.53 + ((double)data) / 340;
	delayMs(100);
	return 0;
}

void MPU_I2C_stop()
{
	MPU_SDA_OUT();
	MPU_I2C_SDA_W = 0;
	MPU_I2C_DELAY();
	MPU_I2C_SCL = 1;
	MPU_I2C_DELAY();
	MPU_I2C_SDA_W = 1;
	delayMs(10);
}

/** Use this to verify that putchar() is working. printf() requires a working putchar() method. 
You should see a steady stream of 'U' output to your terminal.
This test is also useful if you are implementing a software UART and need to verify bit timing */
int main( void )
{
    halInit();
    while (1)
    {
        toggleLed(0);
        printf("U");
        delayMs(1000);
    }
}

int main(void)
{
    TRISBbits.TRISB15 = 0;
    LATBbits.LATB15 = 1;

    while(1){
        delayMs(DEBOUNCE_DELAY);
        LATBbits.LATB15 = !LATBbits.LATB15;
    }
}

/** Use this to verify that printf() is working.
@pre working putchar() method.
 */
int main( void )
{
    halInit();
    int num = 0;
    while (1)
    {
        printf("Hello World %u\r\n", num++);
        delayMs(1000);
    }
}

/** Resets the ZNP using hardware and retrieves the SYS_RESET_IND message. 
This method is used to restart the ZNP's internal state machine and apply changes to startup options, zigbee device type, etc.
@post znpResult contains the error code, or ZNP_SUCCESS if success.
@return a pointer to the beginning of the version structure, or a pointer to indeterminate data if error.
@see Interface Specification for order of fields
*/
unsigned char* znpReset()
{
    RADIO_OFF();
    DEBUG_OFF();
    delayMs(1);
    RADIO_ON(); 
    DEBUG_ON();
    znpResult = spiPoll();              //Note: this will be different if UART
    return (znpBuf+SRSP_PAYLOAD_START);  //the beginning of the reset indication string
}