基于MSP430單片機的四旋翼 PID Control種群遺傳算法通過卡爾曼濾波實現自穩

ID:415178 · 發表于 2018-10-25 13:22

基于MSP430單片機的四旋翼僅供大家參考

PID Control種群遺傳算法：

#include "io430x14x.h"
#include "stdlib.h"
#include "sys.h"
#include "Control.h"
#define col_MAX 50 //群體空間大小
#define var_p 65 //變異概率：65 對應的變異概率約等于0.001，655 為0.01 rand():0-65535
#define epoch_MAX 200 //進化代數
void inherit(void) //遺傳進化PID
{
unsigned int colony[col_MAX]={
62267,15148,39769,31849,58411,49944,29915,58375 ,53831
,29144,40332,51900,60411,48378,11552,26588,61306,60089
,26887,58565, 3794,23125,53291, 646, 9102,13288,13023
,39570,17838,13029, 1001,48941,29169,61066,30539,27436
,55457,34416,13280,44049,54926, 1287,44647,24869,54512
,32952,46495,28107,19963,12429
};
unsigned int health[col_MAX]; //對應colony[] ，每個個體的適應值，
unsigned int dad,mum,baby1,baby2;
unsigned int mini_health,mini_id;
unsigned int temp_health;
unsigned char i,epoch; //epoch 遺傳代數
mini_health = found(&colony[0],&health[0]); //評估初始個體，并作適應值縮放，找出最小適應值
for(epoch=0; epoch < epoch_MAX; epoch++) //開始進化
{
i = roulette(&health[0]);//輪盤賭轉,返回數組下標
dad = colony[i];
i = roulette(&health[0]);//輪盤賭轉,返回數組下標
mum = colony[i];
baby1 = dad&0xff;baby1 |= mum&0xff00;
baby2 = mum&0xff;baby2 |= dad&0xff00;
baby1 = variation(baby1); //變異？
baby2 = variation(baby2);
temp_health = evaluating_unit(baby1); //評估個體適應值
if(temp_health > mini_health)
{
mini_id = evaluating(&health[0]); //取得最差適應值的個體id(數組下標)
colony[mini_id] = baby1;
health[mini_id] = temp_health - mini_health;
}
temp_health = evaluating_unit(baby2); //評估個體適應值
if(temp_health > mini_health)
{
mini_id = evaluating(&health[0]); //取得最差個體的適應值，及其id
colony[mini_id] = baby2;
health[mini_id] = temp_health - mini_health;
}
}
while(1); //結束進化
}
#define ev_N 25
#define aim_value 300
uint evaluating_unit(uint unit)//評估個體適應值順便PWM輸出
{
uint ret=0,temp=0,max=0;
uchar i=0;
int uk=0; //PID增量
redressal(pid, unit); //根據個體，對基因進行解碼修改PID三個參數
while(i<ev_N) //PID調整 ev_N.PID調整次數
{
if(measured > desired)
{
temp = measured - desired;
if(temp > max)
max=temp; //最大超調量
}
else
temp = desired - measured;
ret+=temp;
uk = (int)(PID_Posi(pid, measured, desired)); //PWM輸出
M1 = speed[0] - uk; //y軸
M3 = speed[0] + uk; //y軸
i++;
}
}
ret=65535/(ret/ev_N+max);
return ret; //返回適應值
}
void redressal(float* pid,uint colon)
{
pid[0]=(float)((colon&0xFC00)>>10)*5.0/63; //[0:0.079:5]
pid[1]=(float)((colon&0x3E0)>>5)*0.1/31.0; //[0:0.0032:0.1]
pid[2]=(float)(colon&0x1F)/31.0; //[0:0.032:1]
}
uint found(uint colony,uint health) //計算初始群體適應值，并找出最小值
{
uchar i;
uint mini=0xff;
for(i=0;i<col_MAX;i++)
{
*(health+i) = evaluating_unit(*(colony+i)); //評估個體適應值
if(*(health+i)<mini)
mini=*(health+i);
}
for(i=0;i<col_MAX;i++) //適應值縮放
*(health+i)-=mini;
return mini;
}
uchar roulette(uint health)
{
uchar i;
uint temp=0;
i=(uchar)(rand()%col_MAX); //0~(col_MAX-1) 隨機選擇起點
while(1)
{
if((col_MAX-1)==i) //實現首尾相接
i=0;
temp+=*(health+i); //累加適應值
if (temp>1200)
return i;
i++;
}
}
uchar evaluating(uint health) //取得最小適應值的個體id(數組下標)
{
uchar i,id;
uint mini=0xffff;
for(i=0;i<col_MAX;i++)
{
if(*(health+i)<mini)
{
mini=*(health+i);
id=i;
}
}
return id;
}
uint variation(uint baby) //對基因進行變異
{
uchar i;
for(i=0;i<16;i++)
{
if(rand()<var_p) //rand()0-65535，變異概率：65 對應的變異概率約等于0.001，655 為0.01
{
if(0==(baby&(1<<i)))
baby|=(1<<i);
else
baby &= ~(1<<i);
}
}
return baby;
}

復制代碼

單片機源程序如下:

#include "io430x14x.h"
#include "sys.h"
#include "24L01.h"
#include "MPU6050.h"
#include "HMC5883L.h"
#include "EEPROM.h"
#include "Control.h"
#include "math.h"
uchar RxBuf[32]={0};
uchar TxBuf[32]={0};
uchar EEPROMBuffer[SIZE]={0};
int speed[5] = {1300,0,0,0,0};//M0,M1-M4
int desire[4]={0,0,0,0};//HMC_YAW,ROLL,PITCH,YAW
float PID_Roll[6]={0}, PID_Pitch[6]={0}, PID_Yaw[6]={0};
float roll = 0, pitch = 0, yaw = 0;
int gyro[3]={0}, accel[3]={0}, magne[3]={0};
uchar RX = 0, TX = 0, CALC = 0, Fly = 0, EEPROMWR = 0;
uint ms2 = 0, ms10 = 0, ms500 = 0;
/********************************Main*********************************/
void main(void)
{
uchar Flag_TX = 0;
Init_Sys(); //系統功能初始化
_EINT(); //開啟中斷功能
HMC5883_Init();
MPU6050_Init();
NRF24L01_Init();
while(AT24CXX_Check());//檢測不到24c16
AT24CXX_Read(20,EEPROMBuffer,SIZE);
Init_datas(PID_Roll, PID_Pitch, PID_Yaw, desire, speed, EEPROMBuffer);
/********************************Start*********************************/
while(1)
{
if(RX)
{
NRF24L01_RX_Mode();
if(NRF24L01_RxPacket(RxBuf)==RX_OK) //判斷是否收到數據
{
if(RxBuf[0]==0xAA)//有數據更新
{
Data_Update(PID_Roll, PID_Pitch, PID_Yaw, desire, speed, RxBuf);
Fly = RxBuf[START_STOP];//Stop?
}
}
}
if(TX)
{
TX = 0;
if(Flag_TX == 0)
{
Flag_TX = 1;
Data_Send_Status(roll, pitch, yaw, desire, speed, TxBuf);
}
else if(Flag_TX == 1)
{
Flag_TX = 0;
// Data_Send_Senser(accel, gyro, magne, TxBuf);
Data_Send_PID(PID_Roll, PID_Pitch, PID_Yaw, TxBuf);
}
NRF24L01_TxPacket(TxBuf);
}
if(CALC)
{
CALC = 0;
MPU6050_Read(accel, gyro);
AHRSupdate(accel, gyro, magne);
yaw = get_angle(magne, roll, pitch);
if(Fly)
{
EEPROMWR = 1;
MotoCtrl(PID_Roll, PID_Pitch, PID_Yaw, desire, speed, roll, pitch, yaw, gyro, 1);
}
else
{
M1=900;M2=900;M3=900;M4=900;
if(EEPROMWR)
{
EEPROMWR = 0;
AT24CXX_BufferWrite(PID_Roll, PID_Pitch, PID_Yaw, desire, speed, EEPROMBuffer);
}
}
}
}
}
//***********************************************************************
//TIMERA中斷服務程序
//定時時間：1ms
//***********************************************************************
#pragma vector = TIMERA1_VECTOR
__interrupt void Timer_A1(void)
{
if(TAIV==2)//定時溢出標志位
{
ms2++;
ms10++;
// ms500++;
if(ms2 == 2) {CALC = 1; ms2 = 0;}
if(ms10 == 4) {TX = 1; }
if(ms10 == 8) {TX = 1; }
if(ms10 == 12) {TX = 1; }
if(ms10 == 16) {TX = 1; }
if(ms10 == 20) {RX = 1; ms10 = 0;}
// if(ms500 == 5000)
// {
// ms500 = 0;
// }
}
}