msp432搭建平衡小車(二)

顧事發表於2021-08-25

前言

　　　　上一節掌握了使用pwm驅動電機，接下來介紹如何使用msp432讀取mpu6050資料

正文

　　　首先我們得知道mpu6050通訊方式，由於mpu6050只能用i2c通訊，所以學會使用msp432的i2c,msp432的i2c驅動可以呼叫driverlib庫來使用msp432的硬體i2c，但是i2c庫方法複雜使用起來會比較麻煩，

　　　這裡我選擇偷個懶，用軟體方式模擬i2c驅動。

I2C

　　　建立一個my_i2c.h

/*
 * my_i2c.h
 *
 *  Created on: 2021年7月29日
 *      Author: Administrator
 */

#ifndef MY_I2C_H_
#define MY_I2C_H_

#include <ti/devices/msp432p4xx/driverlib/driverlib.h>
#include <delay.h>

#define SDA_IN()  GPIO_setAsInputPin(GPIO_PORT_P6,GPIO_PIN4)
#define SDA_OUT() GPIO_setAsOutputPin(GPIO_PORT_P6,GPIO_PIN4)


#define IIC_SCL_High()  GPIO_setOutputHighOnPin(GPIO_PORT_P6,GPIO_PIN5) //SCL_High
#define IIC_SCL_Low()   GPIO_setOutputLowOnPin(GPIO_PORT_P6,GPIO_PIN5) //SCL_Low
#define IIC_SDA_High()  GPIO_setOutputHighOnPin(GPIO_PORT_P6,GPIO_PIN4) //SDA_High
#define IIC_SDA_Low()   GPIO_setOutputLowOnPin(GPIO_PORT_P6,GPIO_PIN4) //SDA_Low
#define READ_SDA        GPIO_getInputPinValue(GPIO_PORT_P6,GPIO_PIN4)  //輸入SDA


void IIC_Init(void);                //初始化IIC的IO口
void IIC_Start(void);               //傳送IIC開始訊號
void IIC_Stop(void);                //傳送IIC停止訊號
void IIC_Send_Byte(uint8_t txd);         //IIC傳送一個位元組
uint8_t IIC_Read_Byte(unsigned char ack);//IIC讀取一個位元組
uint8_t IIC_Wait_Ack(void);              //IIC等待ACK訊號
void IIC_Ack(void);                 //IIC傳送ACK訊號
void IIC_NAck(void);                //IIC不傳送ACK訊號



#endif /* MY_I2C_H_ */

　　　　my_i2c.c

/*
 * my_i2c.c
 *
 *  Created on: 2021年7月29日
 *      Author: Administrator
 */

#include <my_i2c.h>

void IIC_Init (void){
    GPIO_setAsOutputPin(GPIO_PORT_P6,GPIO_PIN5 ); //CLK
    GPIO_setAsOutputPin(GPIO_PORT_P6,GPIO_PIN4);//DIN
    IIC_SCL_High();
    IIC_SDA_High();

}

void IIC_Start(void)//SDA 10 SCL 010
{
    SDA_OUT();     //sda線輸出
    IIC_SCL_High();
    IIC_SDA_High();
    delay_us(4);
    IIC_SDA_Low();//START:when CLK is high,DATA change form high to low
    delay_us(4);
    IIC_SCL_Low();//鉗住I2C匯流排，準備傳送或接收資料
}

void IIC_Stop(void)//SDA 01 SCL 01
{
    SDA_OUT();//sda線輸出
    IIC_SCL_Low();//STOP:when CLK is high DATA change form low to high
    IIC_SDA_Low();
    delay_us(4);
    IIC_SCL_High();
    IIC_SDA_High();//傳送I2C匯流排結束訊號
    delay_us(4);
}
//等待應答訊號到來
//返回值：1，接收應答失敗
//        0，接收應答成功
uint8_t IIC_Wait_Ack(void)//
{
    uint8_t cy;
    SDA_IN();      //SDA設定為輸入
    IIC_SCL_High();delay_us(10);
    IIC_SDA_High();delay_us(10);
    if(READ_SDA)
    {
        cy=1;
        IIC_SCL_Low();
        return cy;
    }
    else
    {
        cy=0;
    }
    IIC_SCL_Low();//時鐘輸出0
    return cy;
}
//產生ACK應答
void IIC_Ack(void)
{
    IIC_SCL_Low();
    SDA_OUT();
    IIC_SDA_Low();
    delay_us(2);
    IIC_SCL_High();
    delay_us(2);
    IIC_SCL_Low();
}
//不產生ACK應答
void IIC_NAck(void)
{
    IIC_SCL_Low();
    SDA_OUT();
    IIC_SDA_High();
    delay_us(2);
    IIC_SCL_High();
    delay_us(2);
    IIC_SCL_Low();
}
//IIC傳送一個位元組
//返回從機有無應答
//1，有應答
//0，無應答
void IIC_Send_Byte(uint8_t txd)
{
    uint8_t t;
    SDA_OUT();
    IIC_SCL_Low();//拉低時鐘開始資料傳輸
    delay_us(2);
    for(t=0;t<8;t++)
    {
        if(txd&0x80)
        {
            IIC_SDA_High();delay_us(2);
        }
        else
        {
            IIC_SDA_Low();delay_us(2);
        }
        txd<<=1;
        IIC_SCL_High();
        delay_us(4);
        IIC_SCL_Low();
        delay_us(2);
    }
        delay_us(2);

}
//讀1個位元組，ack=1時，傳送ACK，ack=0，傳送nACK
uint8_t IIC_Read_Byte(unsigned char ack)
{
    unsigned char i,receive=0;
    SDA_IN();//SDA設定為輸入
    for(i=0;i<8;i++ )
    {
        IIC_SCL_Low();
        delay_us(2);
        IIC_SCL_High();
        receive<<=1;
        if(READ_SDA)
            receive++;
        delay_us(2);
    }
    if (!ack)
        IIC_NAck();//傳送nACK
    else
        IIC_Ack(); //傳送ACK
    return receive;

}

好了到這裡i2c驅動有了，就可以繼續下一步了

MPU6050

mpu6050.h

/*
 * MPU6050.H
 *
 *  Created on: 2021年7月29日
 *      Author: Administrator
 */

#ifndef MPU6050_H_
#define MPU6050_H_

#include <ti/devices/msp432p4xx/driverlib/driverlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <my_uart.h>
#include <my_i2c.h>
#include <delay.h>
#include <math.h>

#define MPU6050_ADDR         0x68
#define MPU6050_SMPLRT_DIV   0x19
#define MPU6050_CONFIG       0x1a
#define MPU6050_GYRO_CONFIG  0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_WHO_AM_I     0x75
#define MPU6050_PWR_MGMT_1   0x6b
#define MPU6050_PWR_MGMT_2   0x6c
#define MPU_ACCEL_XOUTH_REG  0x3b
#define MPU_GYRO_XOUTH_REG   0x43
#define MPU6050_TEMP_H       0x41
#define MPU6050_TEMP_L       0x42
#define MPU_DEVICE_ID_REG    0x75
typedef uint8_t u8;
typedef uint16_t u16;

#define PI 3.1415926535897932384626433832795

extern int16_t rawAccX, rawAccY, rawAccZ,rawGyroX, rawGyroY, rawGyroZ;
extern float gyroXoffset, gyroYoffset, gyroZoffset;
extern float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
extern float angleGyroX, angleGyroY, angleGyroZ,angleAccX, angleAccY, angleAccZ;
extern float angleX, angleY, angleZ;
extern uint32_t timer;
extern float accCoef;
extern float gyroCoef;

u8 MPU_Init(void);
void calcGyroOffsets(void);
void mpu_update(void);
u8 MPU_Set_Gyro_Fsr(u8 fsr);
u8 MPU_Set_Accel_Fsr(u8 fsr);
u8 MPU_Set_LPF(u16 lpf);
u8 MPU_Set_Rate(u16 rate);
short MPU_Get_Temperature();
u8 MPU_Get_Gyroscope(int16_t *gx,int16_t *gy,int16_t *gz);
u8 MPU_Get_Accelerometer(int16_t *ax,int16_t *ay,int16_t *az);
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf);
u8 MPU_Write_Byte(u8 reg,u8 data);
u8 MPU_Read_Byte(u8 reg);

#endif /* MPU6050_H_ */

mpu6050.c

/*
 * MPU6050.C
 *
 *  Created on: 2021年7月29日
 *      Author: Administrator
 */

#include <MPU6050.H>

int16_t rawAccX, rawAccY, rawAccZ,rawGyroX, rawGyroY, rawGyroZ;
float gyroXoffset, gyroYoffset, gyroZoffset;
float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
float angleGyroX, angleGyroY, angleGyroZ,angleAccX, angleAccY, angleAccZ;
float angleX, angleY, angleZ;
float gyroCoef,accCoef;
uint32_t timer,preInterval;
float interval;

u8 MPU_Init(void)
{
    accCoef = 0.02;
    gyroCoef = 0.98;

    IIC_Init();
    MPU_Write_Byte(MPU6050_SMPLRT_DIV, 0x00);
    MPU_Write_Byte(MPU6050_CONFIG, 0x00);
    MPU_Write_Byte(MPU6050_GYRO_CONFIG, 0x08);
    MPU_Write_Byte(MPU6050_ACCEL_CONFIG, 0x00);
    MPU_Write_Byte(MPU6050_PWR_MGMT_1,0X01);
    mpu_update();
    angleGyroX = 0;
    angleGyroY = 0;
    preInterval = timer;
    return 0;
}

void calcGyroOffsets(void){
    float x = 0, y = 0, z = 0;
    int16_t rx, ry, rz;
    int i;
    for(i =0; i < 1000; i++){
    MPU_Get_Gyroscope(&rx,&ry,&rz);
    x += ((float)rx) / 65.5;
    y += ((float)ry) / 65.5;
    z += ((float)rz) / 65.5;
    }
    gyroXoffset = x / 1000;
    gyroYoffset = y / 1000;
    gyroZoffset = z / 1000;
}
void mpu_update(void){
    MPU_Get_Accelerometer(&rawAccX, &rawAccY, &rawAccZ);
    temp = MPU_Get_Temperature();
    MPU_Get_Gyroscope(&rawGyroX,&rawGyroY,&rawGyroZ);

    accX = ((float)rawAccX) / 16384.0;
    accY = ((float)rawAccY) / 16384.0;
    accZ = ((float)rawAccZ) / 16384.0;

    angleAccX = atan2(accY, accZ + abs(accX)) * 360 / 2.0 / PI;
    angleAccY = atan2(accX, accZ + abs(accY)) * 360 / -2.0 / PI;

    gyroX = ((float)rawGyroX) / 65.5;
    gyroY = ((float)rawGyroY) / 65.5;
    gyroZ = ((float)rawGyroZ) / 65.5;

    gyroX -= gyroXoffset;
    gyroY -= gyroYoffset;
    gyroZ -= gyroZoffset;

    angleGyroX += gyroX * interval;
    angleGyroY += gyroY * interval;
    angleGyroZ += gyroZ * interval;

    interval = (timer - preInterval) * 0.001;

    angleX = (gyroCoef * (angleX + gyroX * interval)) + (accCoef * angleAccX);
    angleY = (gyroCoef * (angleY + gyroY * interval)) + (accCoef * angleAccY);
    angleZ = angleGyroZ;
    preInterval = timer ;

}
//設定MPU6050陀螺儀感測器滿量程範圍
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,設定成功
//    其他,設定失敗
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
    return MPU_Write_Byte(MPU6050_GYRO_CONFIG,fsr<<3);//設定陀螺儀滿量程範圍
}
//設定MPU6050加速度感測器滿量程範圍
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,設定成功
//    其他,設定失敗
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
    return MPU_Write_Byte(MPU6050_ACCEL_CONFIG,fsr<<3);//設定加速度感測器滿量程範圍
}
//設定MPU6050的數字低通濾波器
//lpf:數字低通濾波頻率(Hz)
//返回值:0,設定成功
//    其他,設定失敗
u8 MPU_Set_LPF(u16 lpf)
{
    u8 data=0;
    if(lpf>=188)data=1;
    else if(lpf>=98)data=2;
    else if(lpf>=42)data=3;
    else if(lpf>=20)data=4;
    else if(lpf>=10)data=5;
    else data=6;
    return MPU_Write_Byte(MPU6050_CONFIG,data);//設定數字低通濾波器
}
//設定MPU6050的取樣率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,設定成功
//    其他,設定失敗
u8 MPU_Set_Rate(u16 rate)
{
    u8 data;
    if(rate>1000)rate=1000;
    if(rate<4)rate=4;
    data=1000/rate-1;
    data=MPU_Write_Byte(MPU6050_SMPLRT_DIV,data);  //設定數字低通濾波器
    return MPU_Set_LPF(rate/2); //自動設定LPF為取樣率的一半
}

//得到溫度值
//返回值:溫度值(擴大了100倍)
short MPU_Get_Temperature(void)
{
    u8 buf[2];
    short raw;
    float temp;
    MPU_Read_Len(MPU6050_ADDR,MPU6050_TEMP_H,2,buf);
    raw=((u16)buf[0]<<8)|buf[1];
    temp=36.53+((double)raw)/340;
    return temp*100;;
}
//得到陀螺儀值(原始值)
//gx,gy,gz:陀螺儀x,y,z軸的原始讀數(帶符號)
//返回值:0,成功
//    其他,錯誤程式碼
u8 MPU_Get_Gyroscope(int16_t *gx,int16_t *gy,int16_t *gz)
{
    u8 buf[6],res;
    res=MPU_Read_Len(MPU6050_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
    if(res==0)
    {
        *gx=((u16)buf[0]<<8)|buf[1];
        *gy=((u16)buf[2]<<8)|buf[3];
        *gz=((u16)buf[4]<<8)|buf[5];
    }
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺儀x,y,z軸的原始讀數(帶符號)
//返回值:0,成功
//    其他,錯誤程式碼
u8 MPU_Get_Accelerometer(int16_t *ax,int16_t *ay,int16_t *az)
{
    u8 buf[6],res;
    res=MPU_Read_Len(MPU6050_ADDR ,MPU_ACCEL_XOUTH_REG,6,buf);
    if(res==0)
    {
        *ax=((u16)buf[0]<<8)|buf[1];
        *ay=((u16)buf[2]<<8)|buf[3];
        *az=((u16)buf[4]<<8)|buf[5];
    }
    return res;;
}
//IIC連續寫
//addr:器件地址
//reg:暫存器地址
//len:寫入長度
//buf:資料區
//返回值:0,正常
//    其他,錯誤程式碼
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    u8 i;
    IIC_Start();
    IIC_Send_Byte((addr<<1)|0);//傳送器件地址+寫命令
    if(IIC_Wait_Ack())  //等待應答
    {
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(reg); //寫暫存器地址
    IIC_Wait_Ack();     //等待應答
    for(i=0;i<len;i++)
    {
        IIC_Send_Byte(buf[i]);  //傳送資料
        if(IIC_Wait_Ack())      //等待ACK
        {
            IIC_Stop();
            return 1;
        }
    }
    IIC_Stop();
    return 0;
}
//IIC連續讀
//addr:器件地址
//reg:要讀取的暫存器地址
//len:要讀取的長度
//buf:讀取到的資料儲存區
//返回值:0,正常
//    其他,錯誤程式碼
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    IIC_Start();
    IIC_Send_Byte((addr<<1)|0 );//傳送器件地址+寫命令
    if(IIC_Wait_Ack())  //等待應答
    {
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(reg); //寫暫存器地址
    IIC_Wait_Ack();     //等待應答
    IIC_Start();
    IIC_Send_Byte((addr<<1)|1 );//傳送器件地址+讀命令
    IIC_Wait_Ack();     //等待應答
    while(len)
    {
        if(len==1)*buf=IIC_Read_Byte(0);//讀資料,傳送nACK
        else *buf=IIC_Read_Byte(1);     //讀資料,傳送ACK
        len--;
        buf++;
    }
    IIC_Stop(); //產生一個停止條件
    return 0;
}
//IIC寫一個位元組
//reg:暫存器地址
//data:資料
//返回值:0,正常
//    其他,錯誤程式碼
u8 MPU_Write_Byte(u8 reg,u8 data)
{
    IIC_Start();
    IIC_Send_Byte((MPU6050_ADDR << 1) | 0);//傳送器件地址+寫命令
    if(IIC_Wait_Ack())  //等待應答
    {
        IIC_Stop();
        printf("%s\r\n","error");
        delay_ms(100);
        return 1;
    }
    IIC_Send_Byte(reg); //寫暫存器地址
    IIC_Wait_Ack();     //等待應答
    IIC_Send_Byte(data);//傳送資料
    if(IIC_Wait_Ack())  //等待ACK
    {
        IIC_Stop();
        return 1;
    }
    IIC_Stop();
    return 0;
}
//IIC讀一個位元組
//reg:暫存器地址
//返回值:讀到的資料
u8 MPU_Read_Byte(u8 reg)
{
    u8 res;
    IIC_Start();
    IIC_Send_Byte((MPU6050_ADDR << 1) | 0 );//傳送器件地址+寫命令
    IIC_Wait_Ack();
    IIC_Send_Byte(reg); //寫暫存器地址
    IIC_Wait_Ack();
    IIC_Start();
    IIC_Send_Byte((MPU6050_ADDR << 1) | 1 );//傳送器件地址+讀命令
    IIC_Wait_Ack();
    res=IIC_Read_Byte(0);//讀取資料,傳送nACK
    IIC_Stop();         //產生一個停止條件
    return res;
}

定時器

由於姿態換算會用上積分，所以需要一個持續的時間值，我使用了一個增計數定時器

先配置定時器機構體

#define TIMER_PERIOD    2000

const Timer_A_UpModeConfig upConfig =
{
        TIMER_A_CLOCKSOURCE_SMCLK,              // SMCLK Clock Source
        TIMER_A_CLOCKSOURCE_DIVIDER_6,          // SMCLK/1 = 3MHz
        TIMER_PERIOD,                           // 5000 tick period
        TIMER_A_TAIE_INTERRUPT_DISABLE,         // Disable Timer interrupt
        TIMER_A_CCIE_CCR0_INTERRUPT_ENABLE ,    // Enable CCR0 interrupt
        TIMER_A_DO_CLEAR                        // Clear value
};

定時器初始化

void Timer_init(void){

    MAP_Timer_A_configureUpMode(TIMER_A1_BASE, &upConfig);
    MAP_Interrupt_enableInterrupt(INT_TA1_0);
    MAP_Timer_A_startCounter(TIMER_A1_BASE, TIMER_A_UP_MODE);
    Interrupt_enableMaster();
}

定時器中斷設定一個時間變數

void TA1_0_IRQHandler(void)
{
    MAP_Timer_A_clearCaptureCompareInterrupt(TIMER_A1_BASE,
            TIMER_A_CAPTURECOMPARE_REGISTER_0);
    timer++;
}

中斷頻率為1ms,時間變數是32位，可以持續49天不會出錯

串列埠

為了檢查能否正常的讀取資料，使用串列埠列印到終端顯示

關於串列埠的使用，首先串列埠配置結構體

const eUSCI_UART_ConfigV1 uartConfig =
{
        EUSCI_A_UART_CLOCKSOURCE_SMCLK,          // SMCLK Clock Source
        78,                                     // BRDIV = 78
        2,                                       // UCxBRF = 2
        0,                                       // UCxBRS = 0
        EUSCI_A_UART_NO_PARITY,                  // No Parity
        EUSCI_A_UART_LSB_FIRST,                  // LSB First
        EUSCI_A_UART_ONE_STOP_BIT,               // One stop bit
        EUSCI_A_UART_MODE,                       // UART mode
        EUSCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION,  // Oversampling
        EUSCI_A_UART_8_BIT_LEN                  // 8 bit data length
};

第一項時鐘源可以預設，但是實際沒有關係，因為一般使用dco時鐘作為串列埠時鐘，

第二項至第四項影響波特率關於其引數可以參考官方工具 http://software-dl.ti.com/msp430/msp430_public_sw/mcu/msp430/MSP430BaudRateConverter/index.html

在其後預設就好，串列埠初始化

void usart_init(void){
    /* Selecting P1.2 and P1.3 in UART mode */
    MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P1,
            GPIO_PIN2 | GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);

    /* Setting DCO to 12MHz */
    CS_setDCOCenteredFrequency(CS_DCO_FREQUENCY_12);

    //![Simple UART Example]
    /* Configuring UART Module */
    MAP_UART_initModule(EUSCI_A0_BASE, &uartConfig);

    /* Enable UART module */
    MAP_UART_enableModule(EUSCI_A0_BASE);

    /* Enabling interrupts */
    MAP_UART_enableInterrupt(EUSCI_A0_BASE, EUSCI_A_UART_RECEIVE_INTERRUPT);
    MAP_Interrupt_enableInterrupt(INT_EUSCIA0);
    MAP_Interrupt_enableMaster();
}

為方便使用重定義printf方法

int  fputc(int _c, register FILE *_fp)
{
        MAP_UART_transmitData( EUSCI_A0_BASE , (uint8_t) _c);
        return _c;
}
int  fputs(const char *_ptr, register FILE *_fp)
{
        uint16_t i, len;
        len = sizeof(_ptr);
        for(i=0; i<len; i++)
        {
        MAP_UART_transmitData( EUSCI_A0_BASE , (unsigned char)_ptr[i] );
        }
    return len;
}

my_uart.h

/*
 * my_uart.h
 *
 *  Created on: 2021年7月29日
 *      Author: Administrator
 */

#ifndef MY_UART_H_
#define MY_UART_H_
#include <ti/devices/msp432p4xx/driverlib/driverlib.h>
#include <stdio.h>
#include <delay.h>

void usart_init();

#endif /* MY_UART_H_ */

my_uart.c

/*
 * my_uart.c
 *
 *  Created on: 2021年7月29日
 *      Author: Administrator
 */
#include <my_uart.h>

int  fputc(int _c, register FILE *_fp)
{
        MAP_UART_transmitData( EUSCI_A0_BASE , (uint8_t) _c);
        return _c;
}
int  fputs(const char *_ptr, register FILE *_fp)
{
        uint16_t i, len;
        len = sizeof(_ptr);
        for(i=0; i<len; i++)
        {
        MAP_UART_transmitData( EUSCI_A0_BASE , (unsigned char)_ptr[i] );
        }
    return len;
}

const eUSCI_UART_ConfigV1 uartConfig =
{
        EUSCI_A_UART_CLOCKSOURCE_SMCLK,          // SMCLK Clock Source
        78,                                     // BRDIV = 78
        2,                                       // UCxBRF = 2
        0,                                       // UCxBRS = 0
        EUSCI_A_UART_NO_PARITY,                  // No Parity
        EUSCI_A_UART_LSB_FIRST,                  // LSB First
        EUSCI_A_UART_ONE_STOP_BIT,               // One stop bit
        EUSCI_A_UART_MODE,                       // UART mode
        EUSCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION,  // Oversampling
        EUSCI_A_UART_8_BIT_LEN                  // 8 bit data length
};
void usart_init(void){
    /* Selecting P1.2 and P1.3 in UART mode */
    MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P1,
            GPIO_PIN2 | GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);

    /* Setting DCO to 12MHz */
    CS_setDCOCenteredFrequency(CS_DCO_FREQUENCY_12);

    //![Simple UART Example]
    /* Configuring UART Module */
    MAP_UART_initModule(EUSCI_A0_BASE, &uartConfig);

    /* Enable UART module */
    MAP_UART_enableModule(EUSCI_A0_BASE);

    /* Enabling interrupts */
    MAP_UART_enableInterrupt(EUSCI_A0_BASE, EUSCI_A_UART_RECEIVE_INTERRUPT);
    MAP_Interrupt_enableInterrupt(INT_EUSCIA0);
    MAP_Interrupt_enableMaster();
}

/* EUSCI A0 UART ISR - Echoes data back to PC host */
void EUSCIA0_IRQHandler(void)
{
    uint32_t status = MAP_UART_getEnabledInterruptStatus(EUSCI_A0_BASE);

    MAP_UART_clearInterruptFlag(EUSCI_A0_BASE, status);

    if(status & EUSCI_A_UART_RECEIVE_INTERRUPT_FLAG)
    {
        MAP_UART_transmitData(EUSCI_A0_BASE, MAP_UART_receiveData(EUSCI_A0_BASE));
    }

}

資料檢測

　　mian.c

 * Author: 
*******************************************************************************/
/* DriverLib Includes */
#include <ti/devices/msp432p4xx/driverlib/driverlib.h>

/* Standard Includes */
#include <MPU6050.H>
#include <my_PWM.H>
#include <my_uart.h>
#include <delay.h>
#include <stdint.h>
#include <stdbool.h>

int16_t rawAccX, rawAccY, rawAccZ,rawGyroX, rawGyroY, rawGyroZ;
float gyroXoffset, gyroYoffset, gyroZoffset;
float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
float angleGyroX, angleGyroY, angleGyroZ,angleAccX, angleAccY, angleAccZ;
float angleX, angleY, angleZ;
uint32_t timer;

double P[2][2] = {{ 1, 0 },{ 0, 1 }};
double Pdot[4] ={ 0,0,0,0};
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.005,C_0 = 1;
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;


#define TIMER_PERIOD    2000

const Timer_A_UpModeConfig upConfig =
{
        TIMER_A_CLOCKSOURCE_SMCLK,              // SMCLK Clock Source
        TIMER_A_CLOCKSOURCE_DIVIDER_6,          // SMCLK/1 = 3MHz
        TIMER_PERIOD,                           // 5000 tick period
        TIMER_A_TAIE_INTERRUPT_DISABLE,         // Disable Timer interrupt
        TIMER_A_CCIE_CCR0_INTERRUPT_ENABLE ,    // Enable CCR0 interrupt
        TIMER_A_DO_CLEAR                        // Clear value
};
void Kalman_Filter(double angle_m,double gyro_m)
{
    angleX+=(gyro_m-q_bias) * dtt;
    Pdot[0]=Q_angle - P[0][1] - P[1][0];
    Pdot[1]=- P[1][1];
    Pdot[2]=- P[1][1];
    Pdot[3]=Q_gyro;
    P[0][0] += Pdot[0] * dtt;
    P[0][1] += Pdot[1] * dtt;
    P[1][0] += Pdot[2] * dtt;
    P[1][1] += Pdot[3] * dtt;
    angle_err = angle_m - angleX;
    PCt_0 = C_0 * P[0][0];
    PCt_1 = C_0 * P[1][0];
    E = R_angle + C_0 * PCt_0;
    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;
    t_0 = PCt_0;
    t_1 = C_0 * P[0][1];
    P[0][0] -= K_0 * t_0;
    P[0][1] -= K_0 * t_1;
    P[1][0] -= K_1 * t_0;
    P[1][1] -= K_1 * t_1;
    angleX+= K_0 * angle_err;
    q_bias += K_1 * angle_err;
}
void Timer_init(void){

    MAP_Timer_A_configureUpMode(TIMER_A1_BASE, &upConfig);
    MAP_Interrupt_enableInterrupt(INT_TA1_0);
    MAP_Timer_A_startCounter(TIMER_A1_BASE, TIMER_A_UP_MODE);
    Interrupt_enableMaster();
}
void LED_init(void){
    MAP_GPIO_setAsOutputPin(GPIO_PORT_P1, GPIO_PIN0);
    MAP_GPIO_setOutputLowOnPin(GPIO_PORT_P1, GPIO_PIN0);
}
int main(void)
{
    MAP_WDT_A_holdTimer();
    MAP_FPU_enableModule();
    MAP_FPU_enableLazyStacking();
    delay_init(12);
    usart_init();
    MPU_Init();
    Timer_init();
    LED_init();
    calcGyroOffsets();
    GPIO_Motor_Init();
    PWM_Init();
    while(1){
      mpu_update();
      Kalman_Filter(angleX,gyroX );
      printf(" %f  %f  %f  %d  %d \r\n  ",angleX,mySetpoint,pwm,left_pulse_number,right_pulse_number);
     }
}
void TA1_0_IRQHandler(void)
{
    MAP_Timer_A_clearCaptureCompareInterrupt(TIMER_A1_BASE,
            TIMER_A_CAPTURECOMPARE_REGISTER_0);
    timer++;
    timer1++;
    if(timer1 == 1000){
        timer1 = 0;
        GPIO_toggleOutputOnPin(GPIO_PORT_P1, GPIO_PIN0);
    }
}

平衡二叉樹
2021-04-21
二叉樹
平衡二叉樹（AVL）
2018-12-13
二叉樹
平衡二叉樹AVL
2017-07-16
二叉樹
【筆記】平衡二叉樹
2017-11-20
筆記二叉樹
玩轉OpenHarmony PID：教你打造兩輪平衡車
2022-12-16
排序二叉樹和平衡二叉樹
2020-09-29
排序二叉樹
YVT二線網約車平臺逆襲？合規擴張和運營平衡是難點
2022-02-28
JZ-039-平衡二叉樹
2021-12-28
二叉樹
資料結構-平衡二叉樹
2020-02-07
資料結構二叉樹
110. 平衡二叉樹
2020-12-04
二叉樹
每日一練（28）：平衡二叉樹
2022-03-05
二叉樹
資料結構——平衡二叉樹
2017-03-19
資料結構二叉樹
小米九號平衡車正式釋出：1999元
2015-10-19
平衡二叉樹（AVL樹）和二叉排序樹轉化為平衡二叉樹及C語言實現
2020-12-09
二叉樹排序C語言
python實現非平衡二叉樹
2021-08-06
Python二叉樹
平衡二叉樹（AVL樹），原來如此！！！
2021-08-25
二叉樹
十三、Mysql之平衡二叉樹(AVL樹)
2018-12-10
MySql二叉樹
平衡二叉查詢樹：紅黑樹
2020-12-05
夜刷：平衡二叉樹的基本操作
2020-12-17
二叉樹
二叉平衡樹 python 列表遞迴
2021-01-03
Python遞迴
平衡二叉樹，B樹，B+樹
2020-12-24
二叉樹
平衡樹和二叉樹的區別
2020-09-25
二叉樹
【劍指offer】判斷二叉樹平衡
2014-05-28
二叉樹
美國銷售的平衡車都不安全？亞馬遜全盤下架
2016-02-25
亞馬遜
Java實現紅黑樹(平衡二叉樹)
2021-10-13
Java二叉樹
[CareerCup] 4.1 Balanced Binary Tree 平衡二叉樹
2015-07-29
二叉樹
小程式驗證車牌號（含新能源車牌）
2019-03-28
24. 平衡二叉樹，及其程式碼實現
2020-10-21
二叉樹
LeetCode-110-平衡二叉樹
2021-11-24
LeetCode二叉樹
自動平衡二叉樹的構建-AVL樹
2021-01-30
二叉樹
紅黑樹和平衡二叉樹的區別
2014-03-30
二叉樹
JZ79 判斷是不是平衡二叉樹
2024-04-22
二叉樹
MSP432安裝與環境配置(ccs) 學習筆記
2020-10-11
筆記
Oracle RAC搭建(二)
2014-05-12
Oracle
二叉樹的深度、寬度遍歷及平衡樹
2018-06-11
二叉樹
[java]二叉樹構建、遍歷、深度、平衡性
2017-08-27
Java二叉樹
小區地下車庫怎麼進出，停車場尋車功能怎麼用
2023-03-30
MVVM框架的搭建（二）——專案搭建
2018-10-10
MVVM框架

msp432搭建平衡小車(二)

前言

正文

I2C

MPU6050

定時器

串列埠

資料檢測

相關文章