STM32倒立擺程序源碼

ID:590365 · 發(fā)表于 2019-9-9 18:51

這個(gè)是已經(jīng)調(diào)好的倒立擺的程序、有需要的趕緊下載哦！

單片機(jī)源程序如下:

#include "MiniBalance.h"
#include "math.h"
#include "led.h"
#include "mpu6050.h"
#define PI 3.14159265
/**************************************************************************
函數(shù)功能：5MS定時(shí)中斷函數(shù) 5MS控制周期
入口參數(shù)：無(wú)
返回值：無(wú)
作者：Mini Balance
**************************************************************************/
int zhongzhi=1750;
int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
void TIM1_UP_TIM16_IRQHandler(void)
{
if(TIM1->SR&0X0001)//5ms定時(shí)中斷
{
TIM1->SR&=~(1<<0); //===清除定時(shí)器1中斷標(biāo)志位
readEncoder(); //===讀取編碼器的值
adc=Get_Adc(0);
Led_Flash(400); //===LED閃爍;
Get_battery_volt(); //===獲取電池電壓
key(100); //===掃描按鍵狀態(tài)
Balance_Pwm=balance(adc,zhongzhi);
Velocity_Pwm=velocity(Encoder_Left);
Moto1=Balance_Pwm-Velocity_Pwm; //===計(jì)算左輪電機(jī)最終PWM
Xianfu_Pwm(); //===PWM限幅
if(Turn_Off(adc,Voltage)==0) //===如果不存在異常
Set_Pwm(Moto1); //===賦值給PWM寄存器
}
}
/**************************************************************************
函數(shù)功能：直立PID控制
入口參數(shù)：角度、角速度
返回值：直立控制PWM
作者：Mini Balance
**************************************************************************/
int balance(int adc, int target)
{
static int Last_Bias;
int balance,Bias;
Bias=adc-target; //===求出平衡的角度中值和機(jī)械相關(guān)
balance=15*Bias+50*(Bias-Last_Bias); //===計(jì)算平衡控制的電機(jī)PWM
Last_Bias=Bias;
return balance;
}
/**************************************************************************
函數(shù)功能：速度PI控制
入口參數(shù)：左輪編碼器、右輪編碼器
返回值：速度控制PWM
作者：Mini Balance
**************************************************************************/
int velocity(int encoder_left)
{
static int Velocity,Encoder_Least,Encoder,Movement;
static long Encoder_Integral;
//=============遙控前進(jìn)后退部分=======================//
if(1==Flag_Qian) Movement=-900; //===如果前進(jìn)標(biāo)志位置1 位移為負(fù)
else if(1==Flag_Hou) Movement=900; //===如果后退標(biāo)志位置1 位移為正
else Movement=0;
//=============速度PI控制器=======================//
Encoder_Least =Encoder_Left; //===獲取最新速度偏差
Encoder *= 0.4; //===一階低通濾波器
Encoder += Encoder_Least*0.6; //===一階低通濾波器
Encoder_Integral +=Encoder; //===積分出位移積分時(shí)間：5ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遙控器數(shù)據(jù)，控制前進(jìn)后退
if(Encoder_Integral>500) Encoder_Integral=500; //===積分限幅
if(Encoder_Integral<-500) Encoder_Integral=-500; //===積分限幅
Velocity=Encoder*300+Encoder_Integral*10; //===速度控制
if(Turn_Off(Angle_Balance,Voltage)==1) Encoder_Integral=0; //===電機(jī)關(guān)閉后清除積分
return Velocity;
}
/**************************************************************************
函數(shù)功能：賦值給PWM寄存器
入口參數(shù)：左輪PWM、右輪PWM
返回值：無(wú)
作者：Mini Balance
**************************************************************************/
void Set_Pwm(int moto1)
{
PBout(3)=1;//===電機(jī)使能打開(kāi)
if(moto1<0) PBout(5)=1, PBout(4)=0;
else PBout(5)=0, PBout(4)=1;
TIM4->CCR2=myabs(moto1);
}
/**************************************************************************
函數(shù)功能：讀取編碼器的數(shù)據(jù)并進(jìn)行數(shù)據(jù)類型轉(zhuǎn)換
入口參數(shù)：無(wú)
返回值：無(wú)
作者：Mini Balance
**************************************************************************/
void readEncoder(void)
{
u16 Encoder_L; //===左右編碼器的脈沖計(jì)數(shù)
Encoder_L= TIM3 -> CNT; //===獲取正交解碼2數(shù)據(jù)
TIM3 -> CNT=0; //===計(jì)數(shù)器清零
if(Encoder_L>32768) Encoder_Left=Encoder_L-65000; else Encoder_Left=Encoder_L; //===數(shù)據(jù)類型轉(zhuǎn)換
}
/**************************************************************************
函數(shù)功能：限制PWM賦值
入口參數(shù)：無(wú)
返回值：無(wú)
作者：Mini Balance
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=7100; //===PWM滿幅是7200 限制在7100
if(Moto1<-Amplitude) Moto1=-Amplitude;
if(Moto1>Amplitude) Moto1=Amplitude;
if(Moto2<-Amplitude) Moto2=-Amplitude;
if(Moto2>Amplitude) Moto2=Amplitude;
}
/**************************************************************************
函數(shù)功能：異常關(guān)閉電機(jī)
入口參數(shù)：傾角和電壓
返回值：1：異常 0：正常
作者：Mini Balance
**************************************************************************/
u8 Turn_Off(int adc, int voltage)
{
u8 temp;
if(adc<(zhongzhi-500)||adc>(zhongzhi+500)||1==Flag_Stop||Voltage<1110)//===電壓低于11.1V 關(guān)閉電機(jī)
{
temp=1;
PBout(3)=0;
PBout(12)=0;
PAout(15)=0;
PBout(4)=0;
PBout(5)=0;
}
else
temp=0;
return temp;
}

復(fù)制代碼

所有資料51hei提供下載:

倒立擺2.3.7z (354.95 KB, 下載次數(shù): 35)

ID:267330 · 發(fā)表于 2019-9-10 10:23

有圖片嗎？

ID:238482 · 發(fā)表于 2019-9-12 08:58

贊一個(gè)

ID:261330 · 發(fā)表于 2019-9-12 15:44

有沒(méi)有接線圖？光有程序沒(méi)原理圖人家怎么看，簡(jiǎn)單說(shuō)明一下也好啊。

帳號(hào)		自動(dòng)登錄	找回密碼
密碼			立即注冊(cè)

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

STM32倒立擺程序源碼