基于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;
// }
}
}

复制代码