lijie/Gamble
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
lijie
2023-12-18 14:36:22 +08:00
commit 0f3cf0952c
184 changed files with 91562 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

169
MPU6050/APP/AS5600.c Normal file
View File

@ -0,0 +1,169 @@
#include "as5600.h"
#include "math.h"
#define cpr 4096
float full_rotation_offset;
long angle_data_prev;
unsigned long velocity_calc_timestamp;
float angle_prev;
/******************************************************************************/
void I2C_Init_(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//GPIOB
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
I2C_DeInit(I2C1);
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = 0; //I2C_OAR1
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = 400000; //400KHz
I2C_Init(I2C1, &I2C_InitStructure);
I2C_Cmd(I2C1, ENABLE);
}
/******************************************************************************/
/******************************************************************************/
#define AS5600_Address 0x36
#define RAW_Angle_Hi 0x0C
/******************************************************************************/
unsigned short I2C_getRawCount(I2C_TypeDef* I2Cx)
{
uint32_t Timeout;
unsigned short temp;
unsigned char dh,dl;
Timeout = 0xFFFF;
I2Cx->CR1 |= 0x0100;//CR1_START_Set;
/* Wait until SB flag is set: EV5 */
while ((I2Cx->SR1&0x0001) != 0x0001)
{
if (Timeout-- == 0)return 0;
}
/* Send the slave address, Reset the address bit0 for write*/
I2Cx->DR = AS5600_Address<<1;
Timeout = 0xFFFF;
/* Wait until ADDR is set: EV6 */
while ((I2Cx->SR1 &0x0002) != 0x0002)
{
if (Timeout-- == 0)return 0;
}
/* Clear ADDR flag by reading SR2 register */
temp = I2Cx->SR2;
/* Write the first data in DR register (EV8_1) */
I2Cx->DR = RAW_Angle_Hi;
/* EV8_2: Wait until BTF is set before programming the STOP */
while ((I2Cx->SR1 & 0x00004) != 0x000004);
/////////////////////////////////////////////////////////////////////////
/* Set POS bit */
I2Cx->CR1 |= 0x0800;//CR1_POS_Set;
Timeout = 0xFFFF;
/* Send START condition */
I2Cx->CR1 |= 0x0100;//CR1_START_Set;
/* Wait until SB flag is set: EV5 */
while ((I2Cx->SR1&0x0001) != 0x0001)
{
if (Timeout-- == 0)return 0;
}
Timeout = 0xFFFF;
/* Send slave address */
I2Cx->DR = (AS5600_Address<<1)+1;
/* Wait until ADDR is set: EV6 */
while ((I2Cx->SR1&0x0002) != 0x0002)
{
if (Timeout-- == 0)return 0;
}
/* EV6_1: The acknowledge disable should be done just after EV6,
that is after ADDR is cleared, so disable all active IRQs around ADDR clearing and
ACK clearing */
__disable_irq();
/* Clear ADDR by reading SR2 register */
temp = I2Cx->SR2;
/* Clear ACK */
I2Cx->CR1 &= 0xFBFF;//CR1_ACK_Reset;
/*Re-enable IRQs */
__enable_irq();
/* Wait until BTF is set */
while ((I2Cx->SR1 & 0x00004) != 0x000004);
/* Disable IRQs around STOP programming and data reading because of the limitation ?*/
__disable_irq();
/* Program the STOP */
I2C_GenerateSTOP(I2Cx, ENABLE);
/* Read first data */
dh = I2Cx->DR;
/* Re-enable IRQs */
__enable_irq();
/**/
/* Read second data */
dl = I2Cx->DR;
/* Make sure that the STOP bit is cleared by Hardware before CR1 write access */
while ((I2Cx->CR1&0x200) == 0x200);
/* Enable Acknowledgement to be ready for another reception */
I2Cx->CR1 |= 0x0400;//CR1_ACK_Set;
/* Clear POS bit */
I2Cx->CR1 &= 0xF7FF;//CR1_POS_Reset;
temp++; //useless,otherwise warning
return ((dh<<8)+dl);
}
/******************************************************************************/
void MagneticSensor_Init(void)
{
// angle_data_prev = I2C_getRawCount(I2C1);
full_rotation_offset = 0;
velocity_calc_timestamp=0;
}
//<2F><>
float getAngle(void)
{
float angle_data,d_angle;
angle_data = I2C_getRawCount(I2C1);
d_angle = angle_data - angle_data_prev;
if(fabs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
angle_data_prev = angle_data;
return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
}
/******************************************************************************/
float getVelocity(void)
{
unsigned long now_us;
float Ts, angle_c, vel;
now_us = SysTick->VAL;
if(now_us<velocity_calc_timestamp)Ts = (float)(velocity_calc_timestamp - now_us)/9*1e-6;
else
Ts = (float)(0xFFFFFF - now_us + velocity_calc_timestamp)/9*1e-6;
if(Ts == 0 || Ts > 0.5) Ts = 1e-3;
angle_c = getAngle();
vel = (angle_c - angle_prev)/Ts;
angle_prev = angle_c;
velocity_calc_timestamp = now_us;
return vel;
}
/******************************************************************************/

167
MPU6050/APP/MPU6050.c Normal file
View File

@ -0,0 +1,167 @@
#include "stm32f10x.h" // Device header
#include "MPU6050_Reg.h"
#include "MPU6050.h"
#include "kalman.h"
#include "alldata.h"
#define MPU6050_ADDRESS 0xD0
//<2F>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>0 <20><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
uint8_t MPU6050_WaitEvent(I2C_TypeDef* I2Cx, uint32_t I2C_EVENT)
{
uint32_t Timeout;
Timeout = 10000;
while (I2C_CheckEvent(I2Cx, I2C_EVENT) != SUCCESS)
{
Timeout --;
if (Timeout == 0)
{
return 1;
}
}
return 0;
}
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
I2C_GenerateSTART(I2C1, ENABLE);
MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT);
I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter);
MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
I2C_SendData(I2C1, RegAddress);
MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING);
I2C_SendData(I2C1, Data);
MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED);
I2C_GenerateSTOP(I2C1, ENABLE);
}
uint8_t MPU6050_ReadReg(uint8_t RegAddress,uint8_t *Data)
{
I2C_GenerateSTART(I2C1, ENABLE);
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT) != 0) return 1;
I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter);
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)!= 0) return 1;
I2C_SendData(I2C1, RegAddress);
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED)!= 0) return 1;
I2C_GenerateSTART(I2C1, ENABLE);
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT)!= 0) return 1;
I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Receiver);
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)!= 0) return 1;
I2C_AcknowledgeConfig(I2C1, DISABLE);
I2C_GenerateSTOP(I2C1, ENABLE);
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED)!= 0) return 1;
*Data = I2C_ReceiveData(I2C1);
I2C_AcknowledgeConfig(I2C1,ENABLE);
return 0;
}
void MPU6050_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//GPIOB
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
I2C_DeInit(I2C1);
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = 0; //I2C_OAR1
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = 400000; //400KHz
I2C_Init(I2C1, &I2C_InitStructure);
I2C_Cmd(I2C1, ENABLE);
MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}
uint8_t MPU6050_GetID(void)
{
uint8_t data;
MPU6050_ReadReg(MPU6050_WHO_AM_I,&data);
return data;
}
uint8_t buffer[12];
uint8_t MPU6050_GetData()
{
for(u8 i=0;i<6;i++)
{
if(MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H + i,&buffer[i]) !=0) return 1;
}
for(u8 i=0;i<6;i++)
{
if(MPU6050_ReadReg(MPU6050_GYRO_XOUT_H + i,&buffer[6+i]) !=0) return 1;;
}
return 0;
}
_st_Mpu MPU6050; //MPU6050ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
static volatile int16_t *pMpu = (int16_t *)&MPU6050;
int16_t MpuOffset[6] = {0}; //MPU6050<35><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
/* <20><>ȡMPU6050<35><30><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>˲<EFBFBD> */
//<2F><><EFBFBD><EFBFBD>ֵ:0,<2C><><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
uint8_t MpuGetData(void)
{
uint8_t i;
int res;
if( MPU6050_GetData() != 0) return 1;
for(i=0;i<6;i++)
{
pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i]; /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16bit<69><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥˮƽУ׼ֵ */
if(i < 3) /* <20>Ǽ<EFBFBD><C7BC>ٶȿ<D9B6><C8BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD> */
{
{
static struct KalmanFilter EKF[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};
kalmanfiter(&EKF[i],(float)pMpu[i]);
pMpu[i] = (int16_t)EKF[i].Out;
// printf("EKF:%f\r\n",EKF[i].Out);
}
}
if(i > 2) /* <20><><EFBFBD>ٶ<EFBFBD>һ<EFBFBD>׻<EFBFBD><D7BB><EFBFBD><EFBFBD>˲<EFBFBD> */
{
uint8_t k=i-3;
const float factor = 0.15f;
static float tBuff[3];
pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
}
}
return 0;
}

16
MPU6050/APP/MPU6050.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef __MPU6050_H
#define __MPU6050_H
#include "stm32f10x.h" // Device header
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data);
//uint8_t MPU6050_ReadReg(uint8_t RegAddress);
void MPU6050_Init(void);
uint8_t MPU6050_GetID(void);
//void MPU6050_GetData();
uint8_t MPU6050_GetData();
uint8_t MPU6050_WaitEvent(I2C_TypeDef* I2Cx, uint32_t I2C_EVENT);
uint8_t MPU6050_ReadReg(uint8_t RegAddress,uint8_t *Data);
uint8_t MpuGetData(void);
#endif

36
MPU6050/APP/MPU6050_Reg.h Normal file
View File

@ -0,0 +1,36 @@
#ifndef __MPU6050_REG_H
#define __MPU6050_REG_H
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_PWR_MGMT_2 0x6C
#define MPU6050_WHO_AM_I 0x75
#endif

87
MPU6050/APP/alldata.h Normal file
View File

@ -0,0 +1,87 @@
#ifndef __ALLDATA_H
#define __ALLDATA_H
typedef signed char int8_t;
typedef signed short int int16_t;
typedef signed int int32_t;
typedef signed long long int64_t;
/* exact-width unsigned integer types */
typedef unsigned char uint8_t;
typedef unsigned short int uint16_t;
typedef unsigned int uint32_t;
typedef unsigned long long uint64_t;
//#define NULL 0
extern volatile uint32_t SysTick_count;
typedef struct{
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
}_st_Mpu;
typedef struct{
float roll;
float pitch;
float yaw;
}_st_AngE;
typedef struct
{
uint16_t roll;
uint16_t pitch;
uint16_t thr;
uint16_t yaw;
uint16_t AUX1;
uint16_t AUX2;
uint16_t AUX3;
uint16_t AUX4;
}_st_Remote;
typedef volatile struct
{
float desired; //< set point
float offset; //< offset
float prevError; //< previous error
float integ; //< integral
float kp; //< proportional gain
float ki; //< integral gain
float kd; //< derivative gain
float IntegLimitHigh; //< integral limit
float IntegLimitLow;
float measured;
float out;
float OutLimitHigh;
float OutLimitLow;
}PidObject;
typedef volatile struct
{
uint8_t unlock;
}_st_ALL_flag;
extern _st_Remote Remote;
extern _st_Mpu MPU6050;
extern _st_AngE Angle;
extern _st_ALL_flag ALL_flag;
extern PidObject pidRateX;
extern PidObject pidRateY;
extern PidObject pidRateZ;
extern PidObject pidPitch;
extern PidObject pidRoll;
extern PidObject pidYaw;
extern int16_t motor_PWM_Value[4];
#endif

20
MPU6050/APP/as5600.h Normal file
View File

@ -0,0 +1,20 @@
#ifndef _AS5600_H
#define _AS5600_H
#include "system.h"
#include "foc_math.h"
extern long cpr;
extern float full_rotation_offset;
extern long angle_data_prev;
extern unsigned long velocity_calc_timestamp;
extern float angle_prev;
void I2C_Init_(void);
unsigned short I2C_getRawCount(I2C_TypeDef* I2Cx);
void MagneticSensor_Init(void);
float getAngle(void);
float getVelocity(void);
#endif

75
MPU6050/APP/foc_math.c Normal file
View File

@ -0,0 +1,75 @@
#include "foc_math.h"
/***************************************************************************/
// int array instead of float array
// 4x200 points per 360 deg
// 2x storage save (int 2Byte float 4 Byte )
// sin*10000
const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000};
/***************************************************************************/
// function approximating the sine calculation by using fixed size array
// ~40us (float array)
// ~50us (int array)
// precision +-0.005
// it has to receive an angle in between 0 and 2PI
float _sin(float a){
if(a < _PI_2){
//return sine_array[(int)(199.0*( a / (_PI/2.0)))];
//return sine_array[(int)(126.6873* a)]; // float array optimized
return 0.0001*sine_array[_round(126.6873* a)]; // int array optimized
}else if(a < _PI){
// return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))];
//return sine_array[398 - (int)(126.6873*a)]; // float array optimized
return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimized
}else if(a < _3PI_2){
// return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))];
//return -sine_array[-398 + (int)(126.6873*a)]; // float array optimized
return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimized
} else {
// return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))];
//return -sine_array[796 - (int)(126.6873*a)]; // float array optimized
return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimized
}
}
/***************************************************************************/
// function approximating cosine calculation by using fixed size array
// ~55us (float array)
// ~56us (int array)
// precision +-0.005
// it has to receive an angle in between 0 and 2PI
float _cos(float a){
float a_sin = a + _PI_2;
a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin;
return _sin(a_sin);
}
/***************************************************************************/
// normalizing radian angle to [0,2PI]
float _normalizeAngle(float angle){
float a = fmod(angle, _2PI);
return a >= 0 ? a : (a + _2PI);
}
/***************************************************************************/
// Electrical angle calculation
float _electricalAngle(float shaft_angle, int pole_pairs) {
return (shaft_angle * pole_pairs);
}
/***************************************************************************/
// square root approximation function using
// https://reprap.org/forum/read.php?147,219210
// https://en.wikipedia.org/wiki/Fast_inverse_square_root
float _sqrtApprox(float number) {//low in fat
long i;
float y;
// float x;
// const float f = 1.5F; // better precision
// x = number * 0.5F;
y = number;
i = * ( long * ) &y;
i = 0x5f375a86 - ( i >> 1 );
y = * ( float * ) &i;
// y = y * ( f - ( x * y * y ) ); // better precision
return number * y;
}
/***************************************************************************/

57
MPU6050/APP/foc_math.h Normal file
View File

@ -0,0 +1,57 @@
#ifndef FOCUTILS_LIB_H
#define FOCUTILS_LIB_H
#include <math.h>
/******************************************************************************/
// sign function
#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) )
#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#define _sqrt(a) (_sqrtApprox(a))
#define _isset(a) ( (a) != (NOT_SET) )
// utility defines
#define _2_SQRT3 1.15470053838
#define _SQRT3 1.73205080757
#define _1_SQRT3 0.57735026919
#define _SQRT3_2 0.86602540378
#define _SQRT2 1.41421356237
#define _120_D2R 2.09439510239
#define _PI 3.14159265359
#define _PI_2 1.57079632679
#define _PI_3 1.0471975512
#define _2PI 6.28318530718
#define _3PI_2 4.71238898038
#define _PI_6 0.52359877559
/******************************************************************************/
// dq current structure
typedef struct
{
float d;
float q;
} DQCurrent_s;
// phase current structure
typedef struct
{
float a;
float b;
float c;
} PhaseCurrent_s;
// dq voltage structs
typedef struct
{
float d;
float q;
} DQVoltage_s;
/******************************************************************************/
float _sin(float a);
float _cos(float a);
float _normalizeAngle(float angle);
float _electricalAngle(float shaft_angle, int pole_pairs);
float _sqrtApprox(float number);
/******************************************************************************/
#endif

24
MPU6050/APP/i2c.c Normal file
View File

@ -0,0 +1,24 @@
#include "i2c.h"
void I2C_Init_(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//GPIOB
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
I2C_DeInit(I2C1);
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = 0; //I2C_OAR1
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = 400000; //400KHz
I2C_Init(I2C1, &I2C_InitStructure);
I2C_Cmd(I2C1, ENABLE);
}

9
MPU6050/APP/i2c.h Normal file
View File

@ -0,0 +1,9 @@
#ifndef __I2C_H
#define __I2C_H
#include "stm32f10x.h" // Device header
void I2C_Init_(void);
#endif

124
MPU6050/APP/imu.c Normal file
View File

@ -0,0 +1,124 @@
#include "imu.h"
#include "mpu6050.h"
#include <math.h>
const float M_PI = 3.1415926535;
const float RtA = 57.2957795f;
const float AtR = 0.0174532925f;
const float Gyro_G = 0.03051756f*2; //<2F><><EFBFBD><EFBFBD><EFBFBD>dz<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-2000<30><30>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>1 / (65536 / 4000) = 0.03051756*2
const float Gyro_Gr = 0.0005326f*2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>,ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD> 2*0.03051756 * 0.0174533f = 0.0005326*2
static float NormAcc;
/* <20><>Ԫ<EFBFBD><D4AA>ϵ<EFBFBD><CFB5> */
typedef volatile struct {
float q0;
float q1;
float q2;
float q3;
} Quaternion;
Quaternion NumQ = {1, 0, 0, 0};
/* <20><><EFBFBD><EFBFBD><EFBFBD>ǻ<EFBFBD><C7BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
struct V{
float x;
float y;
float z;
};
volatile struct V GyroIntegError = {0};
_st_AngE Angle; //<2F><>ǰ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>ֵ̬
/* <20><>Ԫ<EFBFBD><D4AA><EFBFBD>ⷨ<EFBFBD><E2B7A8>ʼ<EFBFBD><CABC> */
void imu_rest(void)
{
NumQ.q0 =1;
NumQ.q1 = 0;
NumQ.q2 = 0;
NumQ.q3 = 0;
GyroIntegError.x = 0;
GyroIntegError.y = 0;
GyroIntegError.z = 0;
Angle.pitch = 0;
Angle.roll = 0;
}
void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt)
{
volatile struct V Gravity,Acc,Gyro,AccGravity;
static float KpDef = 0.5f ;
static float KiDef = 0.0001f;
//static float KiDef = 0.00001f;
float q0_t,q1_t,q2_t,q3_t;
//float NormAcc;
float NormQuat;
float HalfTime = dt * 0.5f;
//<2F><>ȡ<EFBFBD><C8A1>Ч<EFBFBD><D0A7>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);
// <20><><EFBFBD>ٶȹ<D9B6>һ<EFBFBD><D2BB>
//printf("accX:%d\r\n",MPU6050.accX);
NormAcc = 1/sqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
//printf("NorAcc%f\r\n",NormAcc);
// NormAcc = Q_rsqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
Acc.x = pMpu->accX * NormAcc;
Acc.y = pMpu->accY * NormAcc;
Acc.z = pMpu->accZ * NormAcc;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵ó<CBB5><C3B3><EFBFBD>ֵ
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȻ<D9B6><C8BB>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȵIJ<C8B5><C4B2><EFBFBD>ֵ
GyroIntegError.x += AccGravity.x * KiDef;
GyroIntegError.y += AccGravity.y * KiDef;
GyroIntegError.z += AccGravity.z * KiDef;
//<2F><><EFBFBD>ٶ<EFBFBD><D9B6>ںϼ<DABA><CFBC>ٶȻ<D9B6><C8BB>ֲ<EFBFBD><D6B2><EFBFBD>ֵ
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;
// һ<><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;
NumQ.q0 += q0_t;
NumQ.q1 += q1_t;
NumQ.q2 += q2_t;
NumQ.q3 += q3_t;
// <20><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
NormQuat = 1/sqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
NumQ.q0 *= NormQuat;
NumQ.q1 *= NormQuat;
NumQ.q2 *= NormQuat;
NumQ.q3 *= NormQuat;
// <20><>Ԫ<EFBFBD><D4AA>תŷ<D7AA><C5B7><EFBFBD><EFBFBD>
{
#ifdef YAW_GYRO
*(
float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA; //yaw
#else
float yaw_G = pMpu->gyroZ * Gyro_G;
if((yaw_G > 1.0f) || (yaw_G < -1.0f)) //<2F><><EFBFBD><EFBFBD>̫С<CCAB><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>Ǹ<EFBFBD><C7B8>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pAngE->yaw += yaw_G * dt;
// printf("Yaw:%f\r\n",pAngE->yaw);
}
#endif
pAngE->pitch = asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;
pAngE->roll = atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA; //PITCH
// printf("Pitch:%f;\r\n",pAngE->pitch);
// printf("Roll:%f;\r\n",pAngE->roll);
}
}

17
MPU6050/APP/imu.h Normal file
View File

@ -0,0 +1,17 @@
#ifndef __IMU_H
#define __IMU_H
#include "alldata.h"
#define squa( Sq ) (((float)Sq)*((float)Sq))
extern _st_AngE Angle;//<2F><>ǰ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>ֵ̬
extern void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt);
extern void imu_rest(void);
#endif

10
MPU6050/APP/kalman.c Normal file
View File

@ -0,0 +1,10 @@
#include "kalman.h"
//һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>
void kalmanfiter(struct KalmanFilter *EKF,float input)
{
EKF->NewP = EKF->LastP + EKF->Q;
EKF->Kg = EKF->NewP / (EKF->NewP + EKF->R);
EKF->Out = EKF->Out + EKF->Kg * (input - EKF->Out);
EKF->LastP = (1 - EKF->Kg) * EKF->NewP;
}

18
MPU6050/APP/kalman.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef __KALMAN_H
#define __KALMAN_H
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
struct KalmanFilter{
float LastP; //<2F><>һ<EFBFBD><D2BB>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float NewP; //<2F><><EFBFBD>µ<EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float Out; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Kg; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Q; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float R; //<2F>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
};
extern void kalmanfiter(struct KalmanFilter *EKF,float input); //һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>
#endif

15
MPU6050/APP/led.c Normal file
View File

@ -0,0 +1,15 @@
#include "led.h"
void led_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC,ENABLE);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC,&GPIO_InitStructure);
}

12
MPU6050/APP/led.h Normal file
View File

@ -0,0 +1,12 @@
#ifndef _LED_H
#define _LED_H
#define led PCout(13)
#include "stm32f10x.h"
#include "system.h"
void led_init(void);
#endif

80
MPU6050/APP/pwm.c Normal file
View File

@ -0,0 +1,80 @@
#include "pwm.h"
void TIM2_PWM_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //ʹ<><CAB9>TIM2ʱ<32><CAB1>
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //ʹ<><CAB9>GPIOAʱ<41><CAB1>
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); //ʹ<><CAB9>GPIOAʱ<41><CAB1>
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_14;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
//<2F><><EFBFBD><EFBFBD>PA1<41><31>PA2Ϊ<32><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Ϊ50MHZ
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//<2F><><EFBFBD><EFBFBD>TIM2<4D>ڲ<EFBFBD>ʱ<EFBFBD><CAB1>
TIM_InternalClockConfig(TIM2);
//<2F><><EFBFBD><EFBFBD>TIM2
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; //ʱ<>ӷ<EFBFBD>Ƶ
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD>ϼ<EFBFBD><CFBC><EFBFBD>
TIM_TimeBaseInitStructure.TIM_Period = 1440 - 1; //ARR <20>Զ<EFBFBD><D4B6><EFBFBD>װ<EFBFBD><D7B0>ֵ
TIM_TimeBaseInitStructure.TIM_Prescaler = 1 - 1; //PSC Ԥ<><D4A4>Ƶ<EFBFBD><C6B5>
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0; //
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);
//<2F><><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;//<2F><><EFBFBD><EFBFBD>PWMģʽ1
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;//<2F><>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>Ϊ<EFBFBD>ߵ<EFBFBD>ƽ
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0; //CCR
TIM_OC1Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>1 PA0
TIM_OC2Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>2 PA1
TIM_OC3Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>3 PA2
TIM_Cmd(TIM2, ENABLE); //ʹ<>ܶ<EFBFBD>ʱ<EFBFBD><CAB1>2
}
void TIM4_1ms_Init(void) //(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_TimeBaseInitStructure.TIM_Period = 1000-1; //1ms
TIM_TimeBaseInitStructure.TIM_Prescaler= 72-1; //72<37><32>Ƶ=1MHz
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
TIM_Cmd(TIM4,ENABLE);
}
uint32_t time1_cntr;
uint32_t time2_cntr;
//<2F><>ʱ<EFBFBD><CAB1><EFBFBD>жϷ<D0B6><CFB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void TIM4_IRQHandler(void)
{
if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
{
time1_cntr++;
time2_cntr++;
}
TIM_ClearITPendingBit(TIM4,TIM_IT_Update); //<2F><><EFBFBD><EFBFBD><EFBFBD>жϱ<D0B6>־λ
}

13
MPU6050/APP/pwm.h Normal file
View File

@ -0,0 +1,13 @@
#ifndef _PWM_H
#define _PWM_H
#include "system.h"
#define M_Enable GPIO_SetBits(GPIOC,GPIO_Pin_14);
#define M_Disable GPIO_ResetBits(GPIOC,GPIO_Pin_14);
extern uint32_t time1_cntr;
extern uint32_t time2_cntr;
void TIM2_PWM_Init(void);
void TIM4_1ms_Init(void);
#endif

100
MPU6050/APP/servo.c Normal file
View File

@ -0,0 +1,100 @@
#include "servo.h"
#include "Delay.h"
void Servo_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //ʹ<><CAB9>TIM2ʱ<32><CAB1>
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //ʹ<><CAB9>GPIOAʱ<41><CAB1>
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_4;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//<2F><><EFBFBD><EFBFBD>PA1<41><31>PA2Ϊ<32><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>Ϊ50MHZ
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//<2F><><EFBFBD><EFBFBD>TIM2<4D>ڲ<EFBFBD>ʱ<EFBFBD><CAB1>
TIM_InternalClockConfig(TIM2);
//<2F><><EFBFBD><EFBFBD>TIM2
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; //ʱ<>ӷ<EFBFBD>Ƶ
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD>ϼ<EFBFBD><CFBC><EFBFBD>
TIM_TimeBaseInitStructure.TIM_Period = 100 - 1; //ARR <20>Զ<EFBFBD><D4B6><EFBFBD>װ<EFBFBD><D7B0>ֵ
TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1; //PSC Ԥ<><D4A4>Ƶ<EFBFBD><C6B5>
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0; //
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);
//<2F><><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;//<2F><><EFBFBD><EFBFBD>PWMģʽ1
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;//<2F><>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>Ϊ<EFBFBD>ߵ<EFBFBD>ƽ
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0; //CCR
TIM_OC1Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>1 PA0
TIM_OC2Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>2 PA1
TIM_OC3Init(TIM2, &TIM_OCInitStructure); //<2F><>ʼ<EFBFBD><CABC>ͨ<EFBFBD><CDA8>3 PA2
TIM_Cmd(TIM2, ENABLE); //ʹ<>ܶ<EFBFBD>ʱ<EFBFBD><CAB1>2
}
void Rotate(u8 a,u8 b,u8 c)
{
TIM_SetCompare1(TIM2,a);
TIM_SetCompare2(TIM2,b);
TIM_SetCompare3(TIM2,c);
}
//<2F>޸<EFBFBD>ͨ<EFBFBD><CDA8>2<EFBFBD>Ƚ<EFBFBD>ֵ
// TIM_SetCompare2(TIM2, Compare);
//<2F><>ת
void Rotate_clockwise(void)
{
Rotate(10,0,0);
delay_ms(10);
Rotate(10,10,0);
delay_ms(10);
Rotate(0,10,0);
delay_ms(10);
Rotate(0,10,10);
delay_ms(10);
Rotate(0,0,10);
delay_ms(10);
Rotate(10,0,10);
}
//<2F><>ת
void Rotate_counterclockwise(void)
{
Rotate(0,10,0);
delay_ms(10);
Rotate(10,10,0);
delay_ms(10);
Rotate(10,0,0);
delay_ms(10);
Rotate(10,0,10);
delay_ms(10);
Rotate(0,0,10);
delay_ms(10);
Rotate(0,10,10);
}
//ֹͣ
void Rotate_stop(void)
{
Rotate(0,0,0);
}
//ɲ<><C9B2>
void Rotate_shache(void)
{
Rotate(3,0,0);
}

16
MPU6050/APP/servo.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef _PWM_H
#define _PWM_H
#include "system.h"
#define PWM_EN PAout(4);
void Servo_Init(void);
void PWM_SetCompare2(uint16_t Compare);
void PWM_SetCompare3(uint16_t Compare);
void Rotate(u8 a,u8 b,u8 c);
void Rotate_clockwise(void);
void Rotate_counterclockwise(void);
void Rotate_stop(void);
void Rotate_shache(void);
#endif

99
MPU6050/APP/stm32_flash.c Normal file
View File

@ -0,0 +1,99 @@
#include "stm32_flash.h"
//<2F><>ȡָ<C8A1><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD>İ<EFBFBD><C4B0><EFBFBD>(16λ<36><CEBB><EFBFBD><EFBFBD>)
//faddr:<3A><><EFBFBD><EFBFBD>ַ(<28>˵<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>Ϊ2<CEAA>ı<EFBFBD><C4B1><EFBFBD>!!)
//<2F><><EFBFBD><EFBFBD>ֵ:<3A><>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD>.
vu16 STM32_FLASH_ReadHalfWord(u32 faddr)
{
return *(vu16*)faddr;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4>
//WriteAddr:<3A><>ʼ<EFBFBD><CABC>ַ
//pBuffer:<3A><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
//NumToWrite:<3A><><EFBFBD><EFBFBD>(16λ)<29><>
void STM32_FLASH_Write_NoCheck(u32 WriteAddr,u16 *pBuffer,u16 NumToWrite)
{
u16 i;
for(i=0;i<NumToWrite;i++)
{
FLASH_ProgramHalfWord(WriteAddr,pBuffer[i]);
WriteAddr+=2;//<2F><>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>2.
}
}
//<2F><>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ʼд<CABC><D0B4>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD>
//WriteAddr:<3A><>ʼ<EFBFBD><CABC>ַ(<28>˵<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>Ϊ2<CEAA>ı<EFBFBD><C4B1><EFBFBD>)
//pBuffer:<3A><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
//NumToWrite:<3A><><EFBFBD><EFBFBD>(16λ)<29><>(<28><><EFBFBD><EFBFBD>Ҫд<D2AA><D0B4><EFBFBD><EFBFBD>16λ<36><CEBB><EFBFBD>ݵĸ<DDB5><C4B8><EFBFBD>.)
#if STM32_FLASH_SIZE<256
#define STM32_SECTOR_SIZE 1024 //<2F>ֽ<EFBFBD>
#else
#define STM32_SECTOR_SIZE 2048
#endif
u16 STM32_FLASH_BUF[STM32_SECTOR_SIZE/2];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2K<32>ֽ<EFBFBD>
void STM32_FLASH_Write(u32 WriteAddr,u16 *pBuffer,u16 NumToWrite)
{
u32 secpos; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ
u16 secoff; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>Ƶ<EFBFBD>ַ(16λ<36>ּ<EFBFBD><D6BC><EFBFBD>)
u16 secremain; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>ַ(16λ<36>ּ<EFBFBD><D6BC><EFBFBD>)
u16 i;
u32 offaddr; //ȥ<><C8A5>0X08000000<30><30><EFBFBD>ĵ<EFBFBD>ַ
if(WriteAddr<STM32_FLASH_BASE||(WriteAddr>=(STM32_FLASH_BASE+1024*STM32_FLASH_SIZE)))return;//<2F>Ƿ<EFBFBD><C7B7><EFBFBD>ַ
FLASH_Unlock(); //<2F><><EFBFBD><EFBFBD>
offaddr=WriteAddr-STM32_FLASH_BASE; //ʵ<><CAB5>ƫ<EFBFBD>Ƶ<EFBFBD>ַ.
secpos=offaddr/STM32_SECTOR_SIZE; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ
secoff=(offaddr%STM32_SECTOR_SIZE)/2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ƫ<EFBFBD><C6AB>(2<><32><EFBFBD>ֽ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ.)
secremain=STM32_SECTOR_SIZE/2-secoff; //<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD>ռ<EFBFBD><D5BC><EFBFBD>С
if(NumToWrite<=secremain)secremain=NumToWrite;//<2F><><EFBFBD><EFBFBD><EFBFBD>ڸ<EFBFBD><DAB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Χ
while(1)
{
STM32_FLASH_Read(secpos*STM32_SECTOR_SIZE+STM32_FLASH_BASE,STM32_FLASH_BUF,STM32_SECTOR_SIZE/2);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<secremain;i++)//У<><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
if(STM32_FLASH_BUF[secoff+i]!=0XFFFF)
break;//<2F><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>
}
if(i<secremain)//<2F><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>
{
FLASH_ErasePage(secpos*STM32_SECTOR_SIZE+STM32_FLASH_BASE);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for(i=0;i<secremain;i++)//<2F><><EFBFBD><EFBFBD>
{
STM32_FLASH_BUF[i+secoff]=pBuffer[i];
}
STM32_FLASH_Write_NoCheck(secpos*STM32_SECTOR_SIZE+STM32_FLASH_BASE,STM32_FLASH_BUF,STM32_SECTOR_SIZE/2);//д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else
STM32_FLASH_Write_NoCheck(WriteAddr,pBuffer,secremain);//д<>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD>,ֱ<><D6B1>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
if(NumToWrite==secremain)
break;//д<><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else//д<><D0B4>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
{
secpos++; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>1
secoff=0; //ƫ<><C6AB>λ<EFBFBD><CEBB>Ϊ0
pBuffer+=secremain; //ָ<><D6B8>ƫ<EFBFBD><C6AB>
WriteAddr+=secremain; //д<><D0B4>ַƫ<D6B7><C6AB>
NumToWrite-=secremain; //<2F>ֽ<EFBFBD>(16λ)<29><><EFBFBD>ݼ<EFBFBD>
if(NumToWrite>(STM32_SECTOR_SIZE/2))
secremain=STM32_SECTOR_SIZE/2;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
else
secremain=NumToWrite;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
}
}
FLASH_Lock();//<2F><><EFBFBD><EFBFBD>
}
//<2F><>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD>
//ReadAddr:<3A><>ʼ<EFBFBD><CABC>ַ
//pBuffer:<3A><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
//NumToWrite:<3A><><EFBFBD><EFBFBD>(16λ)<29><>
void STM32_FLASH_Read(u32 ReadAddr,u16 *pBuffer,u16 NumToRead)
{
u16 i;
for(i=0;i<NumToRead;i++)
{
pBuffer[i]=STM32_FLASH_ReadHalfWord(ReadAddr);//<2F><>ȡ2<C8A1><32><EFBFBD>ֽ<EFBFBD>.
ReadAddr+=2;//ƫ<><C6AB>2<EFBFBD><32><EFBFBD>ֽ<EFBFBD>.
}
}

19
MPU6050/APP/stm32_flash.h Normal file
View File

@ -0,0 +1,19 @@
#ifndef _stm32_flash_H
#define _stm32_flash_H
#include "system.h"
//<2F>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>
#define STM32_FLASH_SIZE 64 //<2F><>ѡSTM32<33><32>FLASH<53><48><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С(<28><>λΪK)
//FLASH<53><48>ʼ<EFBFBD><CABC>ַ
#define STM32_FLASH_BASE 0x08000000 //STM32 FLASH<53><48><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>ַ
vu16 STM32_FLASH_ReadHalfWord(u32 faddr);
void STM32_FLASH_Write(u32 WriteAddr,u16 *pBuffer,u16 NumToWrite); //<2F><>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ʼд<CABC><D0B4>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD>
void STM32_FLASH_Read(u32 ReadAddr,u16 *pBuffer,u16 NumToRead); //<2F><>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD><C8B5><EFBFBD><EFBFBD><EFBFBD>
#endif

118
MPU6050/APP/usart1.c Normal file
View File

@ -0,0 +1,118 @@
#include "usart1.h"
//#include "math.h"
#include <stdlib.h>
u8 USART_RX_BUF[64];
//<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>
unsigned short USART_RX_STA=0; //<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>־
float error;
void USART1_IRQHandler(void) //<2F><><EFBFBD><EFBFBD>1<EFBFBD>жϳ<D0B6><CFB3><EFBFBD>
{
unsigned char Res;
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //
{
Res =USART_ReceiveData(USART1); //<2F><>ȡ<EFBFBD><C8A1><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD>ֽ<EFBFBD>
if((USART_RX_STA&0x8000)==0) //<2F><><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
{
if(USART_RX_STA&0x4000) //<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD>0x0d
{
if(Res!=0x0a)USART_RX_STA=0; //<2F><><EFBFBD>մ<EFBFBD><D5B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¿<EFBFBD>ʼ
else
{
USART_RX_STA|=0x8000; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
USART_RX_BUF[USART_RX_STA&0X3FFF]='\0'; //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ֽڷ<D6BD>'0<><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
}
}
else //<2F><>û<EFBFBD>յ<EFBFBD>0x0D
{
if(Res==0x0d)USART_RX_STA|=0x4000;
else
{
USART_RX_BUF[USART_RX_STA&0X3FFF]=Res;
USART_RX_STA++;
if(USART_RX_STA>(64-1))USART_RX_STA=0; //<2F><><EFBFBD>մ<EFBFBD><D5B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¿<EFBFBD>ʼ
}
}
}
if((USART_RX_STA&0x8000)!=0)
{
TX_485;
switch(USART_RX_BUF[0])
{
case 'y': //D
error = atof((const char *)(USART_RX_BUF+1));
// error = fabs(error) > 0.02 ? error : 0 ;
break;
}
RX_485;
USART_RX_STA=0;
}
}
}
int fputc(int ch,FILE *p) //<2F><><EFBFBD><EFBFBD>Ĭ<EFBFBD>ϵģ<CFB5><C4A3><EFBFBD>ʹ<EFBFBD><CAB9>printf<74><66><EFBFBD><EFBFBD>ʱ<EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>
{
USART_SendData(USART1,(u8)ch);
while(USART_GetFlagStatus(USART1,USART_FLAG_TXE)==RESET);
return ch;
}
//<2F><>ʼ<EFBFBD><CABC>IO <20><><EFBFBD><EFBFBD>1
//pclk1:PCLK1ʱ<31><CAB1>Ƶ<EFBFBD><C6B5>(Mhz)
//bound:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void usart1_Init(u32 bound)
{
//GPIO<49>˿<EFBFBD><CBBF><EFBFBD><EFBFBD><EFBFBD>
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_8;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_9;//TX //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PA9
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GPIO_Init(GPIOA,&GPIO_InitStructure); /* <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IO */
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_10;//RX //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PA10
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IN_FLOATING; //ģ<><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
GPIO_Init(GPIOA,&GPIO_InitStructure); /* <20><>ʼ<EFBFBD><CABC>GPIO */
//USART1 <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
USART_InitStructure.USART_BaudRate = bound;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
USART_InitStructure.USART_WordLength = USART_WordLength_8b;//<2F>ֳ<EFBFBD>Ϊ8λ<38><CEBB><EFBFBD>ݸ<EFBFBD>ʽ
USART_InitStructure.USART_StopBits = USART_StopBits_1;//һ<><D2BB>ֹͣλ
USART_InitStructure.USART_Parity = USART_Parity_No;//<2F><><EFBFBD><EFBFBD>żУ<C5BC><D0A3>λ
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//<2F><>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //<2F>շ<EFBFBD>ģʽ
USART_Init(USART1, &USART_InitStructure); //<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
USART_Cmd(USART1, ENABLE); //ʹ<>ܴ<EFBFBD><DCB4><EFBFBD>1
USART_ClearFlag(USART1, USART_FLAG_TC);
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
//Usart1 NVIC <20><><EFBFBD><EFBFBD>
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//<2F><><EFBFBD><EFBFBD>1<EFBFBD>ж<EFBFBD>ͨ<EFBFBD><CDA8>
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2;//<2F><>ռ<EFBFBD><D5BC><EFBFBD>ȼ<EFBFBD>2
NVIC_InitStructure.NVIC_IRQChannelSubPriority =2; //<2F><><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD>1
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQͨ<51><CDA8>ʹ<EFBFBD><CAB9>
NVIC_Init(&NVIC_InitStructure); //<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>VIC<49>Ĵ<EFBFBD><C4B4><EFBFBD>
RX_485;
}

19
MPU6050/APP/usart1.h Normal file
View File

@ -0,0 +1,19 @@
#ifndef _usart1_H
#define _usart1_H
#include "system.h"
#include "stdio.h"
#define RX_485 GPIO_ResetBits(GPIOA,GPIO_Pin_8);
#define TX_485 GPIO_SetBits(GPIOA,GPIO_Pin_8);
extern float error;
extern u8 USART_RX_BUF[64];
//<2F><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>
extern unsigned short USART_RX_STA; //<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>־
void usart1_Init(u32 bound);
#endif