精品露乳无马赛克视频,日本黄页网站免费大全,久久综合色8888

/***********************************************************************
// 兩輪自平衡車最終版控制程序（6軸MPU6050+互補濾波+PWM電機）
// 單片機STC12C5A60S2
// 晶振：20M
// 日期：2014.11.26 - ？
***********************************************************************/
#include <REG52.H>
#include <math.h>
#include <stdio.h>
#include <INTRINS.H>
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//******功能模塊頭文件*******
#include "DELAY.H" //延時頭文件
//--------------------------------------------
#include "STC_ISP.H" //程序燒錄頭文件
#include "SET_SERIAL.H"//串口頭文件
#include "SET_PWM.H"//PWM頭文件
#include "MOTOR.H"//電機控制頭文件
#include "MPU6050.H"//MPU6050頭文件
//-----------------這些文件---------------------------
//******角度參數************
float Gyro_y; //Y軸陀螺儀數據暫存
//--------------加速度和角度的轉化------------------
float Angle_gy; //由角速度計算的傾斜角度
//陀螺儀直接反應角度
float Accel_x; //X軸加速度值暫存
float Angle_ax; //由加速度計算的傾斜角度、
float Angle; //小車最終傾斜角度
uchar value; //角度正負極性標記
//******PWM參數*************
int speed_mr; //右電機轉速//這個的測量---------轉盤
int speed_ml; //左電機轉速
int PWM_R; //右輪PWM值計算
int PWM_L; //左輪PWM值計算
float PWM; //綜合PWM計算
float PWMI; //PWM積分值
//******電機參數*************
float speed_r_l;//電機轉速
float speed; //電機轉速濾波
float position; //位移
//******藍牙遙控參數*************
uchar remote_char;
char turn_need;
char speed_need;
//*********************************************************
//定時器100Hz數據更新中斷 T1定時器
//*********************************************************
void Init_Timer1(void)//10毫秒@20MHz,100Hz刷新頻率
{
AUXR &= 0xBF;//定時器時鐘12T模式
TMOD &= 0x0F;//設置定時器模式
TMOD |= 0x10;//設置定時器模式
TL1 = 0xE5; //設置定時初值
TH1 = 0xBE; //設置定時初值
TF1 = 0; //清除TF1標志
TR1 = 1; //定時器1開始計時
}
//*********************************************************
//中斷控制初始化
//*********************************************************
void Init_Interr(void)
{
EA = 1; //開總中斷
EX0 = 1; //開外部中斷INT0
EX1 = 1; //開外部中斷INT1
IT0 = 1; //下降沿觸發
IT1 = 1; //下降沿觸發
ET1 = 1; //開定時器1中斷
}
//******卡爾曼參數************
float code Q_angle=0.001;
float code Q_gyro=0.003;
float code R_angle=0.5;
float code dt=0.01; //dt為kalman濾波器采樣時間;
char code C_0 = 1;
float xdata Q_bias, Angle_err;
float xdata PCt_0, PCt_1, E;
float xdata K_0, K_1, t_0, t_1;
float xdata Pdot[4] ={0,0,0,0};
float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };
//*********************************************************
// 卡爾曼濾波
//*********************************************************
//Kalman濾波，20MHz的處理時間約0.77ms；
void Kalman_Filter(float Accel,float Gyro) // 濾波，輸出標準的方波驅動電機
{
Angle+=(Gyro - Q_bias) * dt; //先驗估計陀螺角度
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先驗估計誤差協方差微分的積分
PP[0][1] += Pdot[1] * dt; // =先驗估計誤差協方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;//zk-先驗估計
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[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 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后驗估計誤差協方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle+= K_0 * Angle_err; //后驗估計
Q_bias+= K_1 * Angle_err; //后驗估計
Gyro_y = Gyro - Q_bias; //輸出值(后驗估計)的微分=角速度
}
//*********************************************************
// 傾角計算（卡爾曼融合）
//*********************************************************
void Angle_Calcu(void)
{
//------加速度--------------------------
//范圍為2g時，換算關系：16384 LSB/g
//角度較小時，x=sinx得到角度（弧度）, deg = rad*180/3.14
//因為x>=sinx,故乘以1.3適當放大
Accel_x = GetData(ACCEL_XOUT_H); //讀取X軸加速度存在ACCEL_XOUT_H 寄存器
Angle_ax = (Accel_x - 1100) /16384; //去除零點偏移,計算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14; //弧度轉換為度,
//-------角速度-------------------------
//范圍為2000deg/s時，換算關系：16.4 LSB/(deg/s)
Gyro_y = GetData(GYRO_YOUT_H); //靜止時角速度Y軸輸出為【-30】左右
Gyro_y = -(Gyro_y + 30)/16.4; //去除零點偏移，計算角速度值, 【負號為方向處理】
//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度積分得到傾斜角度.
//-------卡爾曼濾波融合-----------------------
Kalman_Filter(Angle_ax,Gyro_y); //卡爾曼濾波計算傾角
/*//-------互補濾波-----------------------
//補償原理是取當前傾角和加速度獲得傾角差值進行放大，然后與
//陀螺儀角速度疊加后再積分，從而使傾角最跟蹤為加速度獲得的角度
//0.5為放大倍數，可調節補償度；0.01為系統周期10ms
Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
}
//*********************************************************
//電機轉速和位移值計算
//*********************************************************
void Psn_Calcu(void)
{
speed_r_l =(speed_mr + speed_ml)*0.5;
speed *= 0.7;//上一次的 //車輪速度濾波
speed += speed_r_l*0.3;
position += speed; //積分得到位移
position += speed_need;//加上藍牙位移，等于總的移動
if(position<-6000) position = -6000;
if(position> 6000) position = 6000;
}
static float code Kp = 9.0; //PID參數
static float code Kd = 2.6; //PID參數
static float code Kpn = 0.01; //PID參數
static float code Ksp = 2.0; //PID參數
//*********************************************************
//電機PWM值計算
//*********************************************************
void PWM_Calcu(void)
{
if(Angle<-40||Angle>40) //角度過大，關閉電機
{
CCAP0H = 0;
CCAP1H = 0;
return;
}
PWM = Kp*Angle + Kd*Gyro_y; //PID：角速度和角度
PWM += Kpn*position + Ksp*speed; //PID：速度和位置
PWM_R = PWM + turn_need;
PWM_L = PWM - turn_need; //藍牙的傾斜，
PWM_Motor(PWM_L,PWM_R);
}
//*********************************************************
//手機藍牙遙控
//*********************************************************
void Bluetooth_Remote(void)
{
remote_char = receive_char(); //接收藍牙串口數據
if(remote_char ==0x02) speed_need = -80; //前進
else if(remote_char ==0x01) speed_need = 80; //后退
else speed_need = 0; //不動
if(remote_char ==0x03) turn_need = 15; //左轉
else if(remote_char ==0x04) turn_need = -15; //右轉
else turn_need = 0; //不轉
}
/*=================================================================================*/
//*********************************************************
//main
//*********************************************************
void main()
{
delaynms(500); //上電延時
Init_PWM(); //PWM初始化
Init_Timer0(); //初始化定時器0，作為PWM時鐘源
Init_Timer1(); //初始化定時器1
Init_Interr(); //中斷初始化
Init_Motor(); //電機控制初始化
Init_BRT(); //串口初始化（獨立波特率）
InitMPU6050(); //初始化MPU6050
delaynms(500);
while(1)
{
Bluetooth_Remote();
}
}
/*=================================================================================*/
//********timer1中斷***********************
void Timer1_Update(void) interrupt 3
{
TL1 = 0xE5; //設置定時初值10MS
TH1 = 0xBE;
//STC_ISP(); //程序下載
Angle_Calcu(); //傾角計算
Psn_Calcu(); //電機位移計算
PWM_Calcu(); //計算PWM值
speed_mr = speed_ml = 0;
}
//********右電機中斷***********************
void INT_L(void) interrupt 0
{
if(SPDL == 1) { speed_ml++; } //左電機前進
else { speed_ml--; } //左電機后退
LED = ~LED;
}
//********左電機中斷***********************
void INT_R(void) interrupt 2
{
if(SPDR == 1) { speed_mr++; } //右電機前進
else { speed_mr--; } //右電機后退
LED = ~LED;
}

復制代碼

/*--------------------------------------------------------------------------
DELAY.H
延時函數頭文件
--------------------------------------------------------------------------*/
#ifndef __DELAY_H__
#define __DELAY_H__
#define uchar unsigned char
#define uint unsigned int
//------函數（聲明）--------------------------------
void Delay400Ms(void); //延時400mS函數（子程序）
void delay1ms(void); //延時1mS函數（子程序）
void delaynms(uchar n); //延時n mS函數（子程序）
//***********************************************************************************/
//延時400ms
void Delay400Ms(void)
{
uchar TempCycA = 30;//1周期MPU用30, 12周期MPU用5
uint TempCycB;
while(TempCycA--)
{
TempCycB=7269;
while(TempCycB--);
};
}
//***********************************************************************************/
//延時1ms
void delay1ms()
{
uchar i,j;
for(i=0;i<10;i++)
for(j=0;j<33;j++)
;
}
//***********************************************************************************/
//延時n*ms
void delaynms(uchar n)
{
uchar i;
for(i=0;i<n;i++) delay1ms();
}
#endif

復制代碼

/*****************************************************
普通IO模擬I2C通信
STC12C5A60S2 IT單片機 AXTL;11.0592MHz
*****************************************************/
#ifndef __I2C_H__
#define __I2C_H__
#include <REG52.H>
#include <math.h> //Keil library
#include <stdio.h> //Keil library
#include <INTRINS.H>
//********端口定義**********************************
sbit SCL=P2^1;//IIC時鐘引腳定義
sbit SDA=P2^0;//IIC數據引腳定義
#defineSlaveAddress 0xD0 //IIC寫入時的地址字節數據，+1為讀取
//********函數初始定義******************************
void Delay5us();
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(bit ack);
bit I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
void display_ACCEL_x();
void display_ACCEL_y();
void display_ACCEL_z();
uchar Single_ReadI2C(uchar REG_Address);//讀取I2C數據
void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C寫入數據
//**************************************
//延時5微秒(STC12C5A60S2@12M)
//不同的工作環境,需要調整此函數
//此延時函數是使用1T的指令周期進行計算,與傳統的12T的MCU不同
//**************************************/
void Delay5us()
{
uchar n = 4;
while (n--)
{
_nop_();
_nop_();
}
}
//**************************************
//I2C起始信號
//**************************************
void I2C_Start()
{
SDA = 1; //拉高數據線
SCL = 1; //拉高時鐘線
Delay5us(); //延時
SDA = 0; //產生下降沿
Delay5us(); //延時
SCL = 0; //拉低時鐘線
}
//**************************************
//I2C停止信號
//**************************************
void I2C_Stop()
{
SDA = 0; //拉低數據線
SCL = 1; //拉高時鐘線
Delay5us(); //延時
SDA = 1; //產生上升沿
Delay5us(); //延時
}
//**************************************
//I2C發送應答信號
//入口參數:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(bit ack)
{
SDA = ack; //寫應答信號
SCL = 1; //拉高時鐘線
Delay5us(); //延時
SCL = 0; //拉低時鐘線
Delay5us(); //延時
}
//**************************************
//I2C接收應答信號
//**************************************
bit I2C_RecvACK()
{
SCL = 1; //拉高時鐘線
Delay5us(); //延時
CY = SDA; //讀應答信號
SCL = 0; //拉低時鐘線
Delay5us(); //延時
return CY;
}
//**************************************
//向I2C總線發送一個字節數據
//**************************************
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i<8; i++) //8位計數器
{
dat <<= 1; //移出數據的最高位
SDA = CY; //送數據口
SCL = 1; //拉高時鐘線
Delay5us(); //延時
SCL = 0; //拉低時鐘線
Delay5us(); //延時
}
I2C_RecvACK();
}
//**************************************
//從I2C總線接收一個字節數據
//**************************************
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1; //使能內部上拉,準備讀取數據,
for (i=0; i<8; i++) //8位計數器
{
dat <<= 1;
SCL = 1; //拉高時鐘線
Delay5us(); //延時
dat |= SDA; //讀數據
SCL = 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;
}
#endif

復制代碼

/*********************************************************
STC_12C5A60S2免斷電燒錄程序
*********************************************************/
#ifndef _STC_ISP_H_
#define _STC_ISP_H_
#include<REG52.h>
sfr IAP_CONTR = 0xC7; //IAP Control Register
sbit IN_OFF=P3^0;//串口接收端
//**************************************************
void STC_ISP()
{
IN_OFF=1; //――2
if(!IN_OFF){
IAP_CONTR=0x60;
} //判斷串口是否有數據過來
}
#endif

復制代碼

/**********************************************************************
串口收發程序（頭文件）
波特率：9600Hz
可選定時器1或獨立波特率作為串口時鐘
**********************************************************************/
#ifndef __SET_SERIAL_H__
#define __SET_SERIAL_H__
sfr AUXR = 0x8E;
sfr BRT = 0x9C;
//***********************************************************************************/
//定時器1初始化
/*void Init_Timer1()
{
// set timer1 mode
TMOD |= 0x20; //設置timer1為8位自動重載模式
SCON = 0x50; // 設定串行口工作方式
PCON &= 0xef; // 波特率不倍增
TH1=TL1=0xFD; //9600 Hz
TR1 = 1; //開啟Timer1
}*/
//***********************************************************************************/
//獨立波特率初始化
void Init_BRT()////9600bps@20MHz
{
PCON &= 0x7F;//波特率不倍速
SCON = 0x50; //8位數據,可變波特率
AUXR |= 0x04;//獨立波特率發生器時鐘為Fosc,即1T
BRT = 0xBF; //設定獨立波特率發生器重裝值
AUXR |= 0x01;//串口1選擇獨立波特率發生器為波特率發生器
AUXR |= 0x10;//啟動獨立波特率發生器
}
//***********************************************************************************/
//串口接收程序
receive_char() //receive data
{
unsigned char rxd;
if(RI)
{
RI = 0;
rxd = SBUF;
return(rxd);
}
}
//***********************************************************************************/
//串口發送子程序
void send_char(unsigned char txd) //sent out data
{
SBUF = txd;
while(!TI);
TI = 0;
}
#endif

復制代碼

/*********************************************************************************************
兩路PWM輸出控制設置
/*********************************************************************************************/
#ifndef __SET_PWM_H__
#define __SET_PWM_H__
//Declare SFR associated with the PCA
sfr CCON = 0xD8; //PCA control register
sbit CCF0 = CCON^0; //PCA module-0 interrupt flag
sbit CCF1 = CCON^1; //PCA module-1 interrupt flag
sbit CR = CCON^6; //PCA timer run control bit
sbit CF = CCON^7; //PCA timer overflow flag
sfr CMOD = 0xD9; //PCA mode register
sfr CL = 0xE9; //PCA base timer LOW
sfr CH = 0xF9; //PCA base timer HIGH
sfr CCAPM0 = 0xDA; //PCA module-0 mode register
sfr CCAP0L = 0xEA; //PCA module-0 capture register LOW
sfr CCAP0H = 0xFA; //PCA module-0 capture register HIGH
sfr CCAPM1 = 0xDB; //PCA module-1 mode register
sfr CCAP1L = 0xEB; //PCA module-1 capture register LOW
sfr CCAP1H = 0xFB; //PCA module-1 capture register HIGH
sfr PCAPWM0 = 0xf2;
sfr PCAPWM1 = 0xf3;
//***********************************************************************************/
//PWM模式設置
void Init_PWM()
{
// set PWM mode
CCON = 0; //Initial PCA control register(PCA timer stop,Clear CF flag,Clear all module interrupt flag)
CL = 0; //Reset PCA base timer
CH = 0;
CMOD = 0x04; //Set PCA timer clock source as timer0 overflow,Disable PCA timer overflow interrupt
CCAP0H = CCAP0L = 255; //PWM0 port output X% duty cycle square wave
CCAPM0 = 0x42; //PCA module-0 work in 8-bit PWM mode and no PCA interrupt
CCAP1H = CCAP1L = 255; //PWM1 port output X% duty cycle square wave
CCAPM1 = 0x42; //PCA module-1 work in 8-bit PWM mode and no PCA interrupt
CR = 1; //PCA timer start run
}
//***********************************************************************************/
//設置Timer0為8位自動重載模式，作為PWM時鐘源
void Init_Timer0()
{
// set timer0 mode
AUXR = 0x00; //timer0 work in 12T mode
TMOD|= 0x02; //set timer0 counter mode2 (8-bit auto-reload)
TH0=TL0=130; //PWM 50Hz
TR0 = 1; //timer0 start running(as PWM clk)
}
#endif

復制代碼

/****************************************
平衡車兩路直流減速電機PWM控制
****************************************/
#ifndef__MOTOR_H__
#define __MOTOR_H__
#include <REG52.H>
#include "SET_PWM.H"//PWM頭文件
//電機PWM控制端口定義
sbit LED = P0^0; //LED
sbit EN12 = P1^3; //L298電機（左）驅動使能
sbit EN34 = P1^4; //L298電機（右）驅動使能
sbit M1= P1^1; //電機驅動(M1=1，M2=0，右電機后退)
sbit M2= P1^2; //電機驅動(M1=0，M2=1，右電機前進)
sbit M3= P1^5; //電機驅動(M3=1，M4=0，左電機前進)
sbit M4= P1^0; //電機驅動(M3=0，M4=1，左電機后退)
sbit SPDR = P3^4; //右電機B相測速，用來判斷電機轉向
sbit SPDL = P3^5; //左電機B相測速，用來判斷電機轉向
//變量定義
bit flg_direc; //電機運動方向標志
/*********函數區************************/
//**************************************
//電機初始化
//**************************************
void Init_Motor()
{
CCAP0H = 255; //關閉PWM0輸出
CCAP1H = 255; //關閉PWM1輸出
EN12 = 0; //關閉左電機
EN34 = 0; //關閉右電機
}
//**************************************
//電機控制
//**************************************
void PWM_Motor(int pwm_l,int pwm_r)
{
if(pwm_l<0)
{
M1 = 1; //右電機后退
M2 = 0;
pwm_l = -pwm_l;
}
else
{
M1 = 0; //右電機前進
M2 = 1;
}
if(pwm_r<0)
{
M3 = 0; //左電機后退
M4 = 1;
pwm_r = -pwm_r;
}
else
{
M3 = 1; //左電機前進
M4 = 0;
}
if(pwm_l>255) pwm_l = 255 ; //防止PWM值超過255
if(pwm_r>255) pwm_r = 255 ; //防止PWM值超過255
CCAP0H = 0 + pwm_l; //設定PWM0占空比(CCAP0H=255，速度最大)
CCAP1H = 0 + pwm_r; //設定PWM1占空比(CCAP0H=255，速度最大)
//這里50可調，作用是跳過電機PWM死區，在PWM值為0-40時，電機不轉動，即死區（**錯誤的注釋，解釋在下句*）
//*此處曾經完全被誤導，曾經在0的地方加的是50，目的是為了消除電機PWM死區，但是導致小車嚴重震蕩，現在該為零才對
}
#endif

復制代碼

/*********************************************
MPU-6050數據處理
*********************************************/
#ifndef _MPU6050_H_
#define _MPU6050_H_
#include <REG52.H>
#include <INTRINS.H>
#include "I2C.H"
//****************************************
// 定義MPU6050內部地址
//****************************************
#defineSMPLRT_DIV0x19//陀螺儀采樣率，典型值：0x07(125Hz)
#defineCONFIG0x1A//低通濾波頻率，典型值：0x06(5Hz)
#defineGYRO_CONFIG0x1B//陀螺儀自檢及測量范圍，典型值：0x18(不自檢，2000deg/s)
#defineACCEL_CONFIG0x1C//加速計自檢、測量范圍及高通濾波頻率，典型值：0x01(不自檢，2G，5Hz)
#defineACCEL_XOUT_H0x3B
#defineACCEL_XOUT_L0x3C
#defineACCEL_YOUT_H0x3D
#defineACCEL_YOUT_L0x3E
#defineACCEL_ZOUT_H0x3F
#defineACCEL_ZOUT_L0x40
#defineTEMP_OUT_H0x41
#defineTEMP_OUT_L0x42
#defineGYRO_XOUT_H0x43
#defineGYRO_XOUT_L0x44
#defineGYRO_YOUT_H0x45
#defineGYRO_YOUT_L0x46
#defineGYRO_ZOUT_H0x47
#defineGYRO_ZOUT_L0x48
#definePWR_MGMT_10x6B//電源管理，典型值：0x00(正常啟用)
#defineWHO_AM_I0x75//IIC地址寄存器(默認數值0x68，只讀)
//#defineSlaveAddress 0xD0 //IIC寫入時的地址字節數據，+1為讀取
//MPU6050操作函數聲明
void InitMPU6050(); //初始化MPU6050
int GetData(uchar REG_Address); //16位數據合成
//**************************************
//初始化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)
{
uint H;
uchar L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H<<8)+L; //合成數據
}
#endif

復制代碼

欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136