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

msp432 Build the balance trolley ( Two ) More articles about

  1. Two wheel self balancing trolley double closed loop PID Control design

                                                                                            Research significance of two wheel self balancing trolley ...

  2. build Extjs frame ( Two )

    build Extjs frame Two . Write entry file app.js, To configure extjs Components \ View file path And will app.js introduce index.html       stay app.js Specify the path of some files in ,Extjs The beginning of the page ...

  3. AIX Next RAC build Oracle10G( Two ) Host configuration

    AIX Next RAC Build a series AIX Next RAC build Oracle10G( Two ) Host configuration Environmental Science node node 1 node 2 Small model IBM P-series 630 IBM P-series 630 Host name AIX2 ...

  4. CentOS7 build Kafka( Two )kafka piece

    CentOS7 build Kafka( Two )kafka piece We said that before zookeeper Build ,zookeeper After running, you can start to build kafka 了 . Must see If you like official documents, please move :[http://kafka.a ...

  5. hadoop HA+Federation( High availability Federation ) Build configuration ( Two )

    hadoop HA+Federation( High availability Federation ) Build configuration ( Two ) label ( The blank space to separate ): hadoop core-site.xml <?xml version="1.0" e ...

  6. Use pytorch Quickly build a neural network to achieve two classification tasks ( Include examples )

    Use pytorch Quickly build a neural network to achieve two classification tasks ( Include examples ) Introduce The last study note introduced not to use pytorch Packaged neural network framework implementation logistic The regression model , And according to autograd Realization ...

  7. Highly balanced binary search tree (AVL Trees )

    AVL The basic concept of tree AVL Trees are highly balanced (height balanced) Binary search tree : For each node x,x The height difference between the left subtree and the right subtree ( Balance factor ) At most 1. Some may ask : Why would there be AVL Trees ? What does it have ...

  8. be based on SpringBoot Build an application development framework ( Two ) —— Login authentication

    zero . Preface This article is based on < be based on SpringBoot Build an application development framework ( One )—— Infrastructure >, Through this article , I'm familiar SpringBoot Usage of , Completed the construction of the application framework . Before starting this article , There's a lot of space in the bottom ...

  9. Deep learning (TensorFlow) Environment building :( Two )Ubuntu16.04+1080Ti Driver.

    I just got it a few days ago 2 platform GPU The machine is assembled , Also wrote a hardware configuration list article ——< Deep learning (TensorFlow) Environment building :( One ) Hardware purchase and host assembly >. These two are also being installed Ubuntu 16.04 and 108 ...

  10. SpringBoot Take off series - Use idea Set up the environment ( Two )

    One . Environment configuration install idea I won't talk about my tutorial , I believe you must have installed it , in addition maven The environment must be installed , So we'll start using idea Develop tools to create a springboot Of web project , Here's a idea ...

Random recommendation

  1. The picture scrolls left and right Jquery Special effects

    The picture scrolls left and right Jquery Special effects , Support auto play on and off , At the same time, it supports right and left arrow click play , The specific procedures are as follows <!DOCTYPE html > <html> <head> ...

  2. SpringMVC Learning Series (6) And data validation

    In the series (4).(5) We show you how to bind data , After binding the data, how to ensure the correctness of the data we get ? That's what we're going to talk about in this article —> data validation . Here we use Hibernate-validator To test ...

  3. Apache Spark Read the source code 12 -- Hive on Spark Operation environment setup

    Welcome to reprint , Reprint please indicate the source , Huihu Ichiro . Wedge Hive Is based on Hadoop Open source data warehouse tools , Similar to SQL Of HiveQL Language , So that the upper level of data analysts do not need to know too much MapReduce Knowledge stored in H ...

  4. Use js Realize mobile device access jump to the specified directory

    The most recent projects are always done at the same time pc Sites and mobile sites , When the mobile phone visits, the default is to see pc Site , Need to be in url Add up /mobile For normal access , This code was shared with me by my colleagues , It's quite practical . CODE function ...

  5. About select Some basic knowledge of elements

    by select Several methods of binding values to elements : One . Through string splicing , Append to select Under the element , Two . adopt DOM establish option Elements , Tie it up value Values and text : function loadProvinve( ...

  6. ViewPager.getChildCount() meaning

    viewpager.getChildCount() very easy Misunderstood as viewpager Of subpages size. It and getCount There is still a difference getChildCount() Is the currently visible page size Than ...

  7. AlertDialog Writing

    public void onItemClick(AdapterView<?> parent, View view, int position,long id) { AlertDialog. ...

  8. FCC(ES6 How to write it ) Exact Change

    Design a cash register  checkCashRegister() , It puts the purchase price (price) As the first parameter , The payment amount (cash) As the second parameter , And the change in the cash register (cid) As the third parameter . cid  ...

  9. Record once JavaWeb Development of garbled code to solve

    POST Submitted in Chinese , The test can correctly receive , And print out Chinese on the console But the code stored in the database is garbled Checked the database , The settings are utf-8, Finally, I found that... Should be set where the database is connected : jdbc:mysql://localhost: ...

  10. 《 Wechat applet component 》 collect

    https://github.com/liuqian0413/wxappUI https://github.com/liujians/Wa-UI