基于MSP430单片机的四旋翼 PID Control种群遗传算法

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        {

                        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        {

                *(health+i) = evaluating_unit(*(colony+i));        //评估个体适应值

                if(*(health+i)                        mini=*(health+i);

        }

        for(i=0;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        {

                if(*(health+i)                {

                        mini=*(health+i);

                        id=i;

                }

        }

        return id;

}

uint variation(uint baby)        //对基因进行变异

{

        uchar i;

        for(i=0;i<16;i++)

        {

                if(rand()                {

                        if(0==(baby&(1<                                baby|=(1<                        else

                                baby &= ~(1<                }