TI MSPM0 MPU6050代码

if(sys_time % 20 == 0){
			//MPU6050
			NVIC_DisableIRQ(UART_0_INST_INT_IRQN);
			NVIC_DisableIRQ(GPIOA_INT_IRQn);
			if(mpu_dmp_get_data(&pitch, &roll, &yaw) == 0){
				yaw = yaw - PianYi;
				//yaw = yaw - 0.0004f*(float)sys_time - 0.49f;
				//yaw = yaw - 7;
        // 定义数据包：帧头(1) + float数据(4) + 校验位(1) + 帧尾(1)，共7字节
        uint8_t packet[7];
        uint8_t checksum = 0;
        int i = 0;
        
        // 1. 添加帧头
        packet[i++] = 0xA5;
        
        // 2. 将float拆分为4个字节并添加到数据包
        // 使用指针将float转换为字节数组（注意大小端问题）
        uint8_t* yaw_bytes = (uint8_t*)&yaw;
        for(int j = 0; j < 4; j++){
            packet[i++] = yaw_bytes[j];
            checksum += yaw_bytes[j];  // 累加数据字节计算校验和
        }
        
        // 3. 添加校验位（只取低8位）
        packet[i++] = checksum & 0xFF;
        
        // 4. 添加帧尾
        packet[i++] = 0x5A;
        
        // 发送完整数据包（共7字节）
        for(int j = 0; j < 7; j++){
            uart0_send_char(packet[j]);
        }
        } else {
            // 发送错误标记数据包
            const char err_packet[] = {0xA5, 'E', 'r', 'r', 0x03, 0x5A}; // 0x03是"Err"的校验和
            for(int j = 0; j < sizeof(err_packet); j++){
                uart0_send_char(err_packet[j]);
            }
}
        
        //uart0_send_string(buffer);
				//uart0_send_string(buffer);
				//uart0_send_char((char)yaw);
				
			NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
			NVIC_EnableIRQ(GPIOA_INT_IRQn);
}

switch(tag){
	case 0:
		TargetAngle=Target;
		go_mpu(yaw + TargetAngle);
		if( Trace_get() != 0){
delay_ms(20);
if( Trace_get() != 0){

DL_GPIO_clearPins(BEEP_PORT, BEEP_PIN_BEEP_PIN);

if(Target == Angle1){
motor(0,1000);
delay_ms(300);
}else{
motor(1000,0);
delay_ms(300);
}

DL_GPIO_setPins(BEEP_PORT, BEEP_PIN_BEEP_PIN);

tag = 1;
}
}
		break;
	case 1:
		go_xunji();
		if(Trace_get() == 0){
motor(500,500);
DL_GPIO_clearPins(BEEP_PORT, BEEP_PIN_BEEP_PIN);

delay_ms(200);
DL_GPIO_setPins(BEEP_PORT, BEEP_PIN_BEEP_PIN);



if(Trace_get() == 0 ){
	tag = 0;
	if(Target == Angle1){
Target = Angle2;
}else{
Target = Angle1;
//if(PianYi < 10.5){
//PianYi+= 1.3;
//}
}
}
}
		break;
}

/*
 * Copyright (c) 2021, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "ti_msp_dl_config.h"
#include "board.h"
#include "tb6612.h"
#include "xunji.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include <stdio.h>
#include <string.h>

#include "oled_software_i2c.h"

// 数据包格式：共11字节
// 0xA5（1字节） + float1（4字节） + float2（4字节） + 校验位（1字节） + 0x5A（1字节）
#define PACKET_LEN 11
#define HEADER 0xA5
#define TAIL 0x5A
// 接收状态机
typedef enum {
    RX_WAIT_HEADER,    // 等待包头0xA5
    RX_RECV_FLOAT1,    // 接收第一个float（4字节）
    RX_RECV_FLOAT2,    // 接收第二个float（4字节）
    RX_RECV_CHECKSUM,  // 接收校验位
    RX_RECV_TAIL       // 接收包尾0x5A
} RxState;
// 接收缓冲区与状态变量（需加volatile，防止编译器优化）
static volatile uint8_t rx_buf[PACKET_LEN];  // 存储完整数据包
static volatile uint8_t rx_idx = 0;          // 当前接收位置索引
static volatile RxState rx_state = RX_WAIT_HEADER;  // 初始状态
static volatile uint8_t checksum_calc = 0;   // 计算的校验和
static volatile uint8_t packet_ok = 0;       // 数据包接收完成且有效标志
// 解析后的数据（第一个float赋值给P）
float P;                  // 目标变量：存储第一个float


uint8_t flag = 0;

//编码器
volatile long long _countB = 0;//临时右轮
volatile long long countB = 0;//准确
volatile long long _countA = 0;//临时左轮
volatile long long countA = 0;//准确
volatile long long countA_ = 0;//总计数
volatile long long countB_ = 0;//总计数
//3.1416 × 44mm ≈ 138.23mm 138.23mm ÷ 530 ≈ 0.2608mm

volatile int speedA=0;
volatile int speedB=0;
volatile int speed=0;

int sys_time = 0;
float pitch=0,roll=0,yaw=0;

uint8_t tag=0;//动作标签
int Angle1=40; //第一转向--
int Angle2=145;//第二转向--
int TargetAngle=40;
int Target=40;
float PianYi=7.59;//MPU6050偏移 2.5 7.59

char buffer[20];
//MPU6050转向
void go_mpu(int angle){

						
if(TargetAngle == Angle2 && yaw > 0){
angle = -180;
}
            float correction = PID_Calculate(angle);//-45向左 +45向右
            
            // 基础速度
            const int base_speed = 600;
            
            // 应用PID修正
            int left_speed = base_speed - correction;
            int right_speed = base_speed + correction;
            
            // 驱动电机
						motor(left_speed, right_speed);

}
void go_xunji(void){
				    int32_t pos = xunji();
            if(pos >= 6 || pos <= -6){
}
            // PID计算转向修正值
            float correction = PID_Calculate(pos);
            
            // 基础速度
            const int base_speed = 600;
            
            // 应用PID修正
            int left_speed = base_speed - correction;
            int right_speed = base_speed + correction;
            
            // 驱动电机
            motor(left_speed, right_speed);

}

int main(void)
{

    SYSCFG_DL_init();
    NVIC_EnableIRQ(GPIOA_INT_IRQn);
		//NVIC_EnableIRQ(TIMER_TICK_INST_INT_IRQN);
		//NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
		//NVIC_DisableIRQ(UART_0_INST_INT_IRQN);
		NVIC_EnableIRQ(UART_0_INST_INT_IRQN);

    //BEEP
		DL_GPIO_clearPins(BEEP_PORT, BEEP_PIN_BEEP_PIN);
//OLED
OLED_Init();
OLED_ShowString(10,0,(uint8_t*)"Start",16);


		PID_Init(100.0f, 0.0f, 0.0f, 30.0f); // Kp, Ki, Kd
		

		
//		MPU6050_Init();

//		while(mpu_dmp_init()){
//		delay_ms(200);
//}
//CalibrateMPU(); // 在这里调用校准函数！
//PianYi+=5;
uart0_send_string("start");


//BEEP
DL_GPIO_setPins(BEEP_PORT, BEEP_PIN_BEEP_PIN);




char buffer[10];
char oled_buffer[32];

snprintf(oled_buffer,sizeof(oled_buffer),"Flag=%d",flag);
OLED_ShowString(64,0,(uint8_t*)oled_buffer,16);
OLED_ShowChinese(112,6,0,16);


    while (1) {


if(sys_time % 20 == 0){
//OLED_Clear();


snprintf(oled_buffer,sizeof(oled_buffer),"Flag=%d",flag);
OLED_ShowString(64,0,(uint8_t*)oled_buffer,16);

snprintf(oled_buffer, sizeof(oled_buffer), "EN=%-6lldcm", (long long)((float)countA_ * 0.02608f));
OLED_ShowString(10,2,(uint8_t*)oled_buffer,16);

snprintf(oled_buffer, sizeof(oled_buffer), "EN=%-6lldcm", (long long)((float)countB_ * 0.02608f));
OLED_ShowString(10,4,(uint8_t*)oled_buffer,16);

snprintf(oled_buffer, sizeof(oled_buffer), "Angle=%-4lld", (long long)yaw);
OLED_ShowString(10,6,(uint8_t*)oled_buffer,16);
}
			sys_time++;
//			if(sys_time % 20 == 0){
//				if(countA != 0 ){
//				motor(400,0);
//}else{
//				motor(0,0);
//}

//}		

			//delay_cycles(CPUCLK_FREQ / 1000 * 2);//时间基准2ms

//操作1
if(flag == 1){
  go_xunji();
}
//操作2
if(flag == 2){
		

}
if(flag == 0){

}
        
}
}


//串口的中断服务函数
void UART_0_INST_IRQHandler(void)
{
volatile unsigned char uart_data = 0;
    //如果产生了串口中断
    switch( DL_UART_getPendingInterrupt(UART_0_INST) )
    {
        case DL_UART_IIDX_RX://如果是接收中断
            //接发送过来的数据保存在变量中
            uart_data = DL_UART_Main_receiveData(UART_0_INST);
//switch (rx_state) {
//            case RX_WAIT_HEADER:
//                // 等待包头0xA5，收到则初始化接收
//                if (uart_data == HEADER) {
//                    rx_buf[rx_idx++] = uart_data;  // 存储包头
//                    checksum_calc = 0;             // 校验和不含包头，重置
//                    rx_state = RX_RECV_FLOAT1;     // 切换到接收第一个float
//                }
//                break;

//            case RX_RECV_FLOAT1:
//                // 接收第一个float（4字节），并累加校验和
//                rx_buf[rx_idx++] = uart_data;
//                checksum_calc += uart_data;  // 校验和包含float1的所有字节
//                // 第一个float接收完成（包头1字节 + 4字节 = 5字节）
//                if (rx_idx == 5) {
//                    rx_state = RX_RECV_FLOAT2;  // 切换到接收第二个float
//                }
//                break;

//            case RX_RECV_FLOAT2:
//                // 接收第二个float（4字节），继续累加校验和
//                rx_buf[rx_idx++] = uart_data;
//                checksum_calc += uart_data;  // 校验和包含float2的所有字节
//                // 第二个float接收完成（5 + 4 = 9字节）
//                if (rx_idx == 9) {
//                    rx_state = RX_RECV_CHECKSUM;  // 切换到接收校验位
//                }
//                break;

//            case RX_RECV_CHECKSUM:
//                // 接收校验位并对比计算值
//                rx_buf[rx_idx++] = uart_data;
//                // 校验位 = （float1所有字节 + float2所有字节）的低8位
//                if (uart_data == (checksum_calc & 0xFF)) {
//                    rx_state = RX_RECV_TAIL;  // 校验通过，等待包尾
//                } else {
//                    // 校验失败，重置状态（丢弃错误包）
//                    rx_state = RX_WAIT_HEADER;
//                    rx_idx = 0;
//                }
//                break;

//            case RX_RECV_TAIL:
//                // 接收包尾0x5A，确认完整数据包
//                if (uart_data == TAIL) {
//                    rx_buf[rx_idx++] = uart_data;  // 存储包尾
//                    packet_ok = 1;                 // 标记数据包有效
//                    
//                    memcpy(&P, &rx_buf[1], 4);
////PID_Init(50, P, 10, 30.0f); // Kp, Ki, Kd
//DL_GPIO_setPins(GPIOB, TRACE_PIN_BEEP_PIN);
//delay_ms(200);
//DL_GPIO_clearPins(GPIOB, TRACE_PIN_BEEP_PIN);

//                }
//                // 无论包尾是否正确，都重置状态准备下一次接收
//                rx_state = RX_WAIT_HEADER;
//                rx_idx = 0;
//                break;

//            default:
//                // 异常状态，强制重置
//                rx_state = RX_WAIT_HEADER;
//                rx_idx = 0;
//                break;
//        }

            //将保存的数据再发送出去
            //uart0_send_char(uart_data);

//static int speed = 600;
//						if(uart_data == 'A'){
//motor(speed,speed);
//}else if(uart_data == 'Z'){
//motor(0,0);
//}else if(uart_data == 'C'){
//motor(speed,-speed);
//}else if(uart_data == 'G'){
//motor(-speed,speed);
//}else if(uart_data == 'E'){
//motor(-speed,-speed);
//}else if(uart_data == 'B'){
//motor(speed,speed - 200);
//}else if(uart_data == 'H'){
//motor(speed - 200,speed);
//}else if(uart_data == 'F'){
//motor(-speed + 200,-speed);
//}else if(uart_data == 'D'){
//motor(-speed,-speed + 200);
//}else if(uart_data == 'X'){
//speed += 100;
//}else if(uart_data == 'Y'){
//speed -= 100;
//}else if(uart_data == 'a'){
//DL_GPIO_clearPins(BEEP_PORT, BEEP_PIN_BEEP_PIN);
//delay_ms(200);
//DL_GPIO_setPins(BEEP_PORT, BEEP_PIN_BEEP_PIN);
//}


            break;

        default://其他的串口中断
            break;
    }
}



void GROUP1_IRQHandler(void)//Group1的中断服务函数
{
    //读取Group1的中断寄存器并清除中断标志位
		//DL_Interrupt_getPendingGroup(DL_INTERRUPT_GROUP_1);
		


	uint32_t gpio_status;
	
	//获取中断信号情况
	gpio_status = DL_GPIO_getEnabledInterruptStatus(ENCODE_PORT, ENCODE_EA1_PIN | ENCODE_EA2_PIN | KEY_PIN_18_PIN | ENCODE_EB1_PIN | ENCODE_EB2_PIN);
  DL_GPIO_clearInterruptStatus(ENCODE_PORT, gpio_status); // 清除已触发的中断标志
		if((gpio_status & KEY_PIN_18_PIN) == KEY_PIN_18_PIN )
		{
				flag++;
				if(flag == 3) flag = 0;
				motor(0,0);//切换状态停止电机
		}
//	//编码器A相上升沿触发
//	if((gpio_status & ENCODE_EA1_PIN) == ENCODE_EA1_PIN)
//	{
//		
//		//如果在A相上升沿下，B相为低电平
//		if(!DL_GPIO_readPins(ENCODE_PORT,ENCODE_EA2_PIN))
//		{
//			_countB--;countB_--;
//		}
//		else
//		{
//			_countB++;countB_++;
//		}
//	}//编码器B相上升沿触发
//	else if((gpio_status & ENCODE_EA2_PIN)==ENCODE_EA2_PIN)
//	{
//		//如果在B相上升沿下，A相为低电平
//		if(!DL_GPIO_readPins(ENCODE_PORT,ENCODE_EA1_PIN))
//		{
//			_countB++;countB_++;
//		}
//		else
//		{
//			_countB--;countB_--;
//		}
//	}else if((gpio_status & ENCODE_EB1_PIN)==ENCODE_EB1_PIN){//左轮EB
//        if(!DL_GPIO_readPins(ENCODE_PORT,ENCODE_EB2_PIN)){//如果B低电平
//            _countA++;countA_++;
//        }else{
//            _countA--;countA_--;
//        }
//    }else if((gpio_status & ENCODE_EB2_PIN)==ENCODE_EB2_PIN){
//        if(!DL_GPIO_readPins(ENCODE_PORT,ENCODE_EB1_PIN)){
//            _countA--;countA_--;
//        }else{
//            _countA++;countA_++;
//        }
//    }
	//清除状态/
	DL_GPIO_clearInterruptStatus(ENCODE_PORT,ENCODE_EA1_PIN|ENCODE_EA2_PIN | KEY_PIN_18_PIN | ENCODE_EB1_PIN | ENCODE_EB2_PIN);
}


void TIMER_TICK_INST_IRQHandler(void)
{
	if( DL_TimerA_getPendingInterrupt(TIMER_TICK_INST) == DL_TIMER_IIDX_ZERO )
	{
		
		countA = _countA;
        countB = _countB;
		_countA = 0;
        _countB = 0;
	}

}