Msp432 building balance trolley (II)

Attend to affairs 2021-08-31 18:49:45 阅读数:449

msp432 msp building balance trolley

Preface

The previous section mastered the use of pwm Drive motor , Next, how to use msp432 Read mpu6050 data

Text

First of all, we need to know mpu6050 communication mode , because mpu6050 Only use i2c signal communication , So learn to use msp432 Of i2c,msp432 Of i2c Driver can call driverlib Library to use msp432 The hardware i2c, however i2c The library method is complex and troublesome to use ,

Here I choose to steal a lazy , Simulate with software i2c drive .

I2C

Build a my_i2c.h

/*
* my_i2c.h
*
* Created on: 2021 year 7 month 29 Japan
* 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) // Input SDA
void IIC_Init(void); // initialization IIC Of IO mouth 
void IIC_Start(void); // send out IIC Start signal 
void IIC_Stop(void); // send out IIC Stop signal 
void IIC_Send_Byte(uint8_t txd); //IIC Send a byte 
uint8_t IIC_Read_Byte(unsigned char ack);//IIC Read a byte 
uint8_t IIC_Wait_Ack(void); //IIC wait for ACK The signal 
void IIC_Ack(void); //IIC send out ACK The signal 
void IIC_NAck(void); //IIC Do not send ACK The signal 
#endif /* MY_I2C_H_ */

my_i2c.c

/*
* my_i2c.c
*
* Created on: 2021 year 7 month 29 Japan
* 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 Line out 
 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();// Hold on I2C Bus , Ready to send or receive data 
}
void IIC_Stop(void)//SDA 01 SCL 01
{
SDA_OUT();//sda Line out 
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();// send out I2C Bus end signal 
delay_us(4);
}
// Waiting for the answer signal to arrive
// Return value :1, Failed to receive response
// 0, Received response successfully 
uint8_t IIC_Wait_Ack(void)//
{
uint8_t cy;
SDA_IN(); //SDA Set to input 
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();// Clock output 0
return cy;
}
// produce ACK The reply 
void IIC_Ack(void)
{
IIC_SCL_Low();
SDA_OUT();
IIC_SDA_Low();
delay_us(2);
IIC_SCL_High();
delay_us(2);
IIC_SCL_Low();
}
// Do not produce ACK The reply 
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 Send a byte
// Return whether the slave has a response
//1, There's a response
//0, No response 
void IIC_Send_Byte(uint8_t txd)
{
uint8_t t;
SDA_OUT();
IIC_SCL_Low();// Pull down the clock and start data transmission 
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);
}
// read 1 Bytes ,ack=1 when , send out ACK,ack=0, send out nACK
uint8_t IIC_Read_Byte(unsigned char ack)
{
unsigned char i,receive=0;
SDA_IN();//SDA Set to input 
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();// send out nACK
else
IIC_Ack(); // send out ACK
return receive;
}

All right, come here i2c There's a driver , You can go on to the next step

MPU6050

mpu6050.h

/*
* MPU6050.H
*
* Created on: 2021 year 7 month 29 Japan
* 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 year 7 month 29 Japan
* 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 ;
}
// Set up MPU6050 Gyro sensor full scale range
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
// Return value :0, Set up the success
// other , Setup failed 
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU6050_GYRO_CONFIG,fsr<<3);// Set the gyro full scale range 
}
// Set up MPU6050 Full scale range of acceleration sensor
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
// Return value :0, Set up the success
// other , Setup failed 
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU6050_ACCEL_CONFIG,fsr<<3);// Set the full-scale range of the acceleration sensor 
}
// Set up MPU6050 Digital low pass filter
//lpf: Digital low-pass filter frequency (Hz)
// Return value :0, Set up the success
// other , Setup failed 
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);// Set the digital low-pass filter 
}
// Set up MPU6050 Sampling rate ( Assume Fs=1KHz)
//rate:4~1000(Hz)
// Return value :0, Set up the success
// other , Setup failed 
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); // Set the digital low-pass filter 
return MPU_Set_LPF(rate/2); // Automatic setting LPF Half the sampling rate 
}
// Get the temperature value
// Return value : Temperature value ( Expanded 100 times )
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;;
}
// Get the gyro value ( Original value )
//gx,gy,gz: gyroscope x,y,z The original reading of the shaft ( Signed )
// Return value :0, success
// other , Error code 
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;;
}
// Get the acceleration value ( Original value )
//gx,gy,gz: gyroscope x,y,z The original reading of the shaft ( Signed )
// Return value :0, success
// other , Error code 
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 Write in succession
//addr: Device address
//reg: Register address
//len: Write length
//buf: Data area
// Return value :0, normal
// other , Error code 
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
IIC_Start();
IIC_Send_Byte((addr<<1)|0);// Send device address + Write orders 
if(IIC_Wait_Ack()) // Waiting for an answer 
 {
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg); // Write register address 
IIC_Wait_Ack(); // Waiting for an answer 
for(i=0;i<len;i++)
{
IIC_Send_Byte(buf[i]); // send data 
if(IIC_Wait_Ack()) // wait for ACK
 {
IIC_Stop();
return 1;
}
}
IIC_Stop();
return 0;
}
//IIC Read continuously
//addr: Device address
//reg: Register address to read
//len: Length to read
//buf: Read data store
// Return value :0, normal
// other , Error code 
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
IIC_Start();
IIC_Send_Byte((addr<<1)|0 );// Send device address + Write orders 
if(IIC_Wait_Ack()) // Waiting for an answer 
 {
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg); // Write register address 
IIC_Wait_Ack(); // Waiting for an answer 
 IIC_Start();
IIC_Send_Byte((addr<<1)|1 );// Send device address + Read command 
IIC_Wait_Ack(); // Waiting for an answer 
while(len)
{
if(len==1)*buf=IIC_Read_Byte(0);// Reading data , send out nACK
else *buf=IIC_Read_Byte(1); // Reading data , send out ACK
len--;
buf++;
}
IIC_Stop(); // Create a stop condition 
return 0;
}
//IIC Write a byte
//reg: Register address
//data: data
// Return value :0, normal
// other , Error code 
u8 MPU_Write_Byte(u8 reg,u8 data)
{
IIC_Start();
IIC_Send_Byte((MPU6050_ADDR << 1) | 0);// Send device address + Write orders 
if(IIC_Wait_Ack()) // Waiting for an answer 
 {
IIC_Stop();
printf("%s\r\n","error");
delay_ms(100);
return 1;
}
IIC_Send_Byte(reg); // Write register address 
IIC_Wait_Ack(); // Waiting for an answer 
IIC_Send_Byte(data);// send data 
if(IIC_Wait_Ack()) // wait for ACK
 {
IIC_Stop();
return 1;
}
IIC_Stop();
return 0;
}
//IIC Read a byte
//reg: Register address
// Return value : Data read 
u8 MPU_Read_Byte(u8 reg)
{
u8 res;
IIC_Start();
IIC_Send_Byte((MPU6050_ADDR << 1) | 0 );// Send device address + Write orders 
 IIC_Wait_Ack();
IIC_Send_Byte(reg); // Write register address 
 IIC_Wait_Ack();
IIC_Start();
IIC_Send_Byte((MPU6050_ADDR << 1) | 1 );// Send device address + Read command 
 IIC_Wait_Ack();
res=IIC_Read_Byte(0);// Reading data , send out nACK
IIC_Stop(); // Create a stop condition 
return res;
}

Timer

Because the attitude conversion will use integral , So you need a duration value , I used an incrementing timer

First configure the timer mechanism

#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
};

Timer initialization

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();
}

Timer interrupt sets a time variable

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

The interrupt frequency is 1ms, The time variable is 32 position , It can last 49 God doesn't make mistakes

A serial port

In order to check whether the data can be read normally , Use the serial port to print to the terminal display

About the use of serial port , First, configure the serial port structure

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
};

The first clock source can default to , But it doesn't really matter , Because of the general use of dco The clock is used as the serial port clock ,

Items 2 to 4 affect the baud rate. For its parameters, please refer to the official tools   http://software-dl.ti.com/msp430/msp430_public_sw/mcu/msp430/MSP430BaudRateConverter/index.html 

After that, just default , Serial initialization

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();
}

Redefine... For ease of use printf Method

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 year 7 month 29 Japan
* 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 year 7 month 29 Japan
* 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));
}
}

Data detection

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);
}
}

 

版权声明:本文为[Attend to affairs]所创,转载请带上原文链接,感谢。 https://car.inotgo.com/2021/08/20210825223128154i.html