通過STM32單片機實時采集MPU6050二軸數數據,然后通過
三角函數計算出偏移角度,從而用單片機IO進行給水、排水控制。
以下代碼由宇天網絡科技編寫
#include "stm32f10x.h"
#include "platform_config.h"
#include "stm32f10x_usart.h"
#include "misc.h"
#include "stdarg.h"
#include "math.h"
USART_InitTypeDef USART_InitStructure;
#define PI=3.1415926;
#define IIC_READ_SDA() GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_15); /* 讀SDA口線狀態 */
#define GetKey() GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0); /* 讀SDA口線狀態 */
#define GetKey2() GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_2); /* 讀SDA口線狀態 */
#define SDA_1 GPIO_SetBits(GPIOB,GPIO_Pin_15); /*字庫IC接口定義:Rom_IN就是字庫IC的SI*/
#define SDA_0 GPIO_ResetBits(GPIOB,GPIO_Pin_15);
#define SCK_1 GPIO_SetBits(GPIOB,GPIO_Pin_13); /*字庫IC接口定義:Rom_SCK就是字庫IC的SCK*/
#define SCK_0 GPIO_ResetBits(GPIOB,GPIO_Pin_13);
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
// 定義MPU6050內部地址
//****************************************
#define SMPLRT_DIV 0x19 //陀螺儀采樣率,典型值:0x07(125Hz)
#define CONFIG 0x1A //低通濾波頻率,典型值:0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺儀自檢及測量范圍,典型值:0x18(不自檢,2000deg/s)
#define ACCEL_CONFIG 0x1C //加速計自檢、測量范圍及高通濾波頻率,典型值:0x01(不自檢,2G,5Hz)
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B //電源管理,典型值:0x00(正常啟用)
#define WHO_AM_I 0x75 //IIC地址寄存器(默認數值0x68,只讀)
#define SlaveAddress 0xD0 //IIC寫入時的地址字節數據,+1為讀取
//****************************************
int CY=0;
//函數聲明
//****************************************
void delay(unsigned int k); //延時
//MPU6050操作函數
void InitMPU6050(); //初始化MPU6050
void Delay5us();
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(int ack);
int I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
uchar Single_ReadI2C(uchar REG_Address); //讀取I2C數據
void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C寫入數據
/* Private function prototypes -----------------------------------------------*/
void RCC_Configuration(void);
void GPIO_Configuration(void);
void NVIC_Configuration(void);
void Delay_us(unsigned int time);
void Delay(__IO uint32_t nCount);
void USART_OUT(USART_TypeDef* USARTx, uint8_t *Data,...);
char *itoa(int value, char *string, int radix);
void USART_Config(USART_TypeDef* USARTx);
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStruct;
USART_ClockInitTypeDef USART_ClockInitStruct;
void USART_Config(USART_TypeDef* USARTx){
USART_InitStructure.USART_BaudRate = 115200; //速率115200bps
USART_InitStructure.USART_WordLength = USART_WordLength_8b; //數據位8位
USART_InitStructure.USART_StopBits = USART_StopBits_1; //停止位1位
USART_InitStructure.USART_Parity = USART_Parity_No; //無校驗位
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //無硬件流控
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收發模式
/* Configure USART1 */
USART_Init(USARTx, &USART_InitStructure); //配置串口參數函數
/* Enable USART1 Receive and Transmit interrupts */
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); //使能接收中斷
USART_ITConfig(USART1, USART_IT_TXE, ENABLE); //使能發送緩沖空中斷
//USART_ITConfig(USART1, USART_IT_TC, ENABLE); //使能發送完成中斷
/* Enable the USART1 */
USART_Cmd(USART1, ENABLE);
}
//**************************************
//延時5微秒(STC90C52RC@12M)
//不同的工作環境,需要調整此函數
//當改用1T的MCU時,請調整此延時函數
//**************************************
void Delay5us()
{
unsigned int i;
unsigned int j;
for(i=0;i<1;i++)< span="">
{
for(j=0;j<5;j++);< span="">
}
}
//**************************************
//I2C起始信號
//**************************************
void I2C_Start()
{
SDA_1; //拉高數據線
SCK_1; //拉高時鐘線
Delay5us(); //延時
SDA_0; //產生下降沿
Delay5us(); //延時
SCK_0; //拉低時鐘線
}
//**************************************
//I2C停止信號
//**************************************
void I2C_Stop()
{
SDA_0; //拉低數據線
SCK_1; //拉高時鐘線
Delay5us(); //延時
SCK_1; //產生上升沿
Delay5us(); //延時
}
//**************************************
//I2C發送應答信號
//入口參數:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(int ack)
{
if(ack==1){SDA_1;}else{SDA_0;} //寫應答信號
SCK_1; //拉高時鐘線
Delay5us(); //延時
SCK_0; //拉低時鐘線
Delay5us(); //延時
}
//**************************************
//I2C接收應答信號
//**************************************
int I2C_RecvACK()
{
SCK_1; //拉高時鐘線
Delay5us(); //延時
CY = IIC_READ_SDA(); //讀應答信號
SCK_0; //拉低時鐘線
Delay5us(); //延時
return CY;
}
//**************************************
//向I2C總線發送一個字節數據
//**************************************
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i<8; i++) //8位計數器
{
if(dat&0x80){SDA_1;}
else {SDA_0;} //送數據口
SCK_1; //拉高時鐘線
Delay5us(); //延時
SCK_0; //拉低時鐘線
Delay5us(); //延時
dat=dat<<=1;< p="">
}
I2C_RecvACK();
}
//**************************************
//從I2C總線接收一個字節數據
//**************************************
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA_1; //使能內部上拉,準備讀取數據,
for (i=0; i<8; i++) //8位計數器
{
dat <<= 1;
SCK_1; //拉高時鐘線
Delay5us(); //延時
dat |= IIC_READ_SDA(); //讀數據
SCK_0; //拉低時鐘線
Delay5us(); //延時
}
return dat;
}
//**************************************
//向I2C設備寫入一個字節數據
//**************************************
void Single_WriteI2C(uchar REG_Address,uchar REG_data)
{
I2C_Start(); //起始信號
I2C_SendByte(SlaveAddress); //發送設備地址+寫信號
I2C_SendByte(REG_Address); //內部寄存器地址,
I2C_SendByte(REG_data); //內部寄存器數據,
I2C_Stop(); //發送停止信號
}
//**************************************
//從I2C設備讀取一個字節數據
//**************************************
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start(); //起始信號
I2C_SendByte(SlaveAddress); //發送設備地址+寫信號
I2C_SendByte(REG_Address); //發送存儲單元地址,從0開始
I2C_Start(); //起始信號
I2C_SendByte(SlaveAddress+1); //發送設備地址+讀信號
REG_data=I2C_RecvByte(); //讀出寄存器數據
I2C_SendACK(1); //接收應答信號
I2C_Stop(); //停止信號
return REG_data;
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠狀態
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
//**************************************
//合成數據
//**************************************
int GetData(uchar REG_Address)
{
uchar H,L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H<<8)+L; //合成數據
}
void RCC_Configuration(void)
{
SystemInit();
}
void GPIO_Configuration(void)
{
RCC_APB2PeriphClockCmd( RCC_APB2Periph_USART1 |RCC_APB2Periph_GPIOA |RCC_APB2Periph_GPIOC |RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOB |RCC_APB2Periph_AFIO , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_13|GPIO_Pin_15|GPIO_Pin_1; //LED1
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //USART1 TX
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //復用推挽輸出
GPIO_Init(GPIOA, &GPIO_InitStructure); //A端口
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; //USART1 RX
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //復用開漏輸入
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1| GPIO_Pin_2; //LED1
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_2; //LED1
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
void NVIC_Configuration(void)
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; //設置串口1中斷
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; //搶占優先級 0
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; //子優先級為0
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能
NVIC_Init(&NVIC_InitStructure);
}
int low=60;
int hgt=10;
short a=0;
short b=0;
float c=0;
char buf[8];
int bt=1;
int bt2=1;
int ct=0;
bool led=1;
int main(void)
{
RCC_Configuration();
NVIC_Configuration();
GPIO_Configuration();
USART_Config(USART1);
InitMPU6050();
GPIO_SetBits(GPIOA,GPIO_Pin_1);
GPIO_SetBits(GPIOB,GPIO_Pin_1);
while(1)
{
ct++;
a=GetData(ACCEL_ZOUT_H);
b=GetData(ACCEL_XOUT_H);
c=atan(abs(b)*1.0/abs(a));
c=c*180/3.14159;
sprintf(buf, "%.2f",c);
USART_OUT(USART1,buf);
USART_OUT(USART1,"\r\n");
if(ct==500)
{
if(led==1){GPIO_ResetBits(GPIOB,GPIO_Pin_6);}
else{GPIO_SetBits(GPIOB,GPIO_Pin_6);}
led=!led;
ct=0;
}
if(c
{
GPIO_ResetBits(GPIOB,GPIO_Pin_1);
}
if(c>low)
{
GPIO_SetBits(GPIOB,GPIO_Pin_1);
}
bt=GetKey();
if(0==bt)
{
Delay_us(40000);
bt=GetKey();
if(0==bt)
{GPIO_ResetBits(GPIOA,GPIO_Pin_1);low=c;}
}
bt2=GetKey2();
if(0==bt2)
{
Delay_us(40000);
bt2=GetKey2();
if(0==bt2)
{GPIO_SetBits(GPIOA,GPIO_Pin_1);hgt=c;}
}
// Delay_us(50000);
}
}
void Delay_us(unsigned int time)
{
unsigned int i=0;
while(time--)
{
i=100; //自己定義
while(i--) ;
}
}
/******************************************************
格式化串口輸出函數
"\r" 回車符 USART_OUT(USART1, "abcdefg\r")
"\n" 換行符 USART_OUT(USART1, "abcdefg\r\n")
"%s" 字符串 USART_OUT(USART1, "字符串是:%s","abcdefg")
"%d" 十進制 USART_OUT(USART1, "a=%d",10)
**********************************************************/
void USART_OUT(USART_TypeDef* USARTx, uint8_t *Data,...){
const char *s;
int d;
char buf[16];
va_list ap;
va_start(ap, Data);
while(*Data!=0){ //判斷是否到達字符串結束符
if(*Data==0x5c){ //'\'
switch (*++Data){
case 'r': //回車符
USART_SendData(USARTx, 0x0d);
Data++;
break;
case 'n': //換行符
USART_SendData(USARTx, 0x0a);
Data++;
break;
default:
Data++;
break;
}
}
else if(*Data=='%'){ //
switch (*++Data){
case 's': //字符串
s = va_arg(ap, const char *);
for ( ; *s; s++) {
USART_SendData(USARTx,*s);
while(USART_GetFlagStatus(USARTx, USART_FLAG_TC)==RESET);
}
Data++;
break;
case 'd': //十進制
d = va_arg(ap, int);
itoa(d, buf, 10);
for (s = buf; *s; s++) {
USART_SendData(USARTx,*s);
while(USART_GetFlagStatus(USARTx, USART_FLAG_TC)==RESET);
}
Data++;
break;
default:
Data++;
break;
}
}
else USART_SendData(USARTx, *Data++);
while(USART_GetFlagStatus(USARTx, USART_FLAG_TC)==RESET);
}
}
/******************************************************
整形數據轉字符串函數
char *itoa(int value, char *string, int radix)
radix=10 標示是10進制 非十進制,轉換結果為0;
例:d=-379;
執行 itoa(d, buf, 10); 后
buf="-379"
**********************************************************/
char *itoa(int value, char *string, int radix)
{
int i, d;
int flag = 0;
char *ptr = string;
/* This implementation only works for decimal numbers. */
if (radix != 10)
{
*ptr = 0;
return string;
}
if (!value)
{
*ptr++ = 0x30;
*ptr = 0;
return string;
}
/* if this is a negative value insert the minus sign. */
if (value < 0)
{
*ptr++ = '-';
/* Make the value positive. */
value *= -1;
}
for (i = 10000; i > 0; i /= 10)
{
d = value / i;
if (d || flag)
{
*ptr++ = (char)(d + 0x30);
value -= (d * i);
flag = 1;
}
}
/* Null terminate the string. */
*ptr = 0;
return string;
} /* NCL_Itoa */