/******************** (C) COPYRIGHT 2023 GeekRebot *****************************
* File Name          : main.c
* Current Version    : V1.0  & ST 3.5.0
* Author             : zhanli 719901725@qq.com
* Date of Issued     : 2023.04.06 zhanli: Create
* Comments           : GeekRebot STM32
********************************************************************************/
#include "bsp_sys.h"
#include "stdio.h"
#include "bluetooth.h"  
#include "MPU6050.h"
unsigned char BLE_RX[64];

extern float Pitch;
										  
/**----------------------------------------------------------------------
* Function    : main
* Description : GeekRebot主函数
* Author      : zhanli&719901725@qq.com
* Date        : 2023/04/22 zhanli
*---------------------------------------------------------------------**/ 
int main(void)
{	
	System_Init();
	Bluetooth_Init(9600);  
	PB_USART_printf(USART2, (u8*)"Init Blue tooth..\r\n");
	Car_Go();

	Delay_ms(200);
	PB_USART_printf(USART2, (u8*)"IIC Stop..\r\n");
	
	uint8_t data = i2c_CheckDevice(0xD0);
	PB_USART_printf(USART2, (u8*)"IIC Init Res = %d\r\n", (int)data);
	//	PB_USART_printf(USART2, (u8*)"MPU6050_initialize..\r\n");
	MPU6050_initialize();
	DMP_Init();
	
	Timer6_Init();          
	Delay_ms(100); 
	while (1)
	{
		Delay_ms(20);
		Read_DMP();
		USART2_Receive(BLE_RX, 6);
		
		if((BLE_RX[3] == 0xB1)&&(BLE_RX[4]==0xB5))
		{
			BLE_RX[3] = 0x00;
			USART2_Send(BLE_RX, 6);
		}
		
		if((BLE_RX[3] == 0xB3)&&(BLE_RX[4]==0xB7))
		{
			BLE_RX[3] = 0x00;
			USART2_Send(BLE_RX, 6);
		}
		
		if((BLE_RX[3] == 0xB2)&&(BLE_RX[4]==0xB6))
		{
			Car_Go();
			USART2_Send(BLE_RX, 6);
		}
		if((BLE_RX[3] == 0xB4)&&(BLE_RX[4]==0xB8))
		{
		
			Car_TurnLeft();
			Delay_ms(500);
			Car_Go();
			BLE_RX[3] = 0x00;
			USART2_Send(BLE_RX, 6);
		}
		if((BLE_RX[3] == 0xB5)&&(BLE_RX[4]==0xB9))
		{
			Car_Stop();
			USART2_Send(BLE_RX, 6);
		}
		if((BLE_RX[3] == 0xB6)&&(BLE_RX[4]==0xBA))
		{

			Car_TurnRight();
			Delay_ms(500);
			Car_Go();
			BLE_RX[3] = 0x00;
			USART2_Send(BLE_RX, 6);
		}
		if((BLE_RX[3] == 0xB8)&&(BLE_RX[4]==0xBC))
		{
			Car_Back();
			USART2_Send(BLE_RX, 6);
		} 
	}
}