2024电赛北京市市一开源 H题 M0G3507 状态机(FSM)与双环PID

目前只更新了我的用户任务，后续有时间还会陆续更新与细致讲解

app.h

#ifndef APP_h
#define APP_h
void App_Init();//用户初始化，在执行用户任务之前执行，只执行一次
void App_task();//用户任务，3ms执行一次
typedef struct {
	char stage;
    char last_stage;
    uint64_t App_cnt_now;
    char App_cnt_flag;
} APP;//定义一个用户状态
extern  APP topic3;
#endif 

app.c

#include "ti_msp_dl_config.h"
#include "encoder.h"
#include "app.h"
#include "pid.h"
#include "data.h"
#include "global.h"
#include "carry.h"
#include "rean_and_out.h"
char topic_stage;

uint64_t App_cnt;

void stage_TASK();
void stage_TASK_1();
void stage_TASK_2();
void stage_TASK_3();
void stage_TASK_4();
void PID_TASK();
void PID_TASK_1();
void PID_TASK_2();
void PID_TASK_3();
void PID_TASK_4();
void  out_put();
void  out_put_1();
void  out_put_2();
void  out_put_3();
void  out_put_4();

 
 
APP  topic1;
APP  topic2;
APP  topic3;
int  topic4_cnt;
void App_Init()
{   topic4_cnt=0;
    topic1.App_cnt_flag=0;
    topic1.stage=0;

    topic2.App_cnt_flag=0;
    topic2.stage=0;
    App_cnt=0;
    NVIC_EnableIRQ(TIMER_1_INST_INT_IRQN);
    pid_set(0,1.92, 0, 30, 10000,0, 10000, 10000,20000);
    pid_set(1,1.92, 0, 30, 10000,0, 10000, 10000,20000);
    pid_set(2,0.8, 0, 8.0, 10,5, 10, 10,0);//直线

    pid_set(3,5, 0,5 , 30,0, 30, 30,0);//寻线


  

}

void App_task()
{   



    /*speed[2].ref=3600;
    speed[2].fdb=speed__;
    pid_NormalCalc(2);

   
    speed[0].ref=30-speed[2].output;//(60+speed[2].output);//右
    speed[0].fdb=	encoder[0].speed_sum;
    pid_NormalCalc(0);

   speed[1].ref=-(30+speed[2].output);//-speed[2].output;
    speed[1].fdb=	encoder[1].speed_sum;
    pid_NormalCalc(1);*/
//测试走直线调参代码，把speed[2].ref=0就是走直线


    if(sw[0]==0&&sw[1]==0&&sw[2]==0&&sw[3]==0){topic_stage=1;}
    if(sw[0]!=0&&sw[1]==0&&sw[2]==0&&sw[3]==0){topic_stage=2;}
    if(sw[0]!=0&&sw[1]!=0&&sw[2]==0&&sw[3]==0){topic_stage=3;}
    if(sw[0]!=0&&sw[1]!=0&&sw[2]!=0&&sw[3]==0){topic_stage=4;}
//根据拨码开关选择不同任务
    stage_TASK();//状态切换
    PID_TASK();//根据不同状态确定PID
    out_put();//根据不同状态确定声光提醒

    
    App_cnt++;//计次，用于做延时

}


void TIMER_1_INST_IRQHandler(void)

{
    App_task();//配置定时器中断3ms执行一次
   
}

/*根据不同题目状态选择不同状态机*/
void stage_TASK()
{
    if(topic_stage==1)
    {
        stage_TASK_1();
    }

    if(topic_stage==2)
    {
        stage_TASK_2();
    }
    if(topic_stage==3)
    {
        stage_TASK_3();
    }

    if(topic_stage==4)
    {
        stage_TASK_4();
    }
    
    

}


void stage_TASK_1()
{
    if(topic1.stage==0)//零状态、发车状态、车不动
    {
        if(App_cnt>300) //一个几次是3ms，300次是0.9s
        {
            topic1.stage=1;
        }
    }
    if(topic1.stage==1)//1状态、盲走直线状态、里程计双环PID
    {
        if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
        {  
            if( topic1.App_cnt_flag==0)
            {
                topic1.App_cnt_now=App_cnt;
                topic1.App_cnt_flag=1;
            }
            if(topic1.App_cnt_flag==1)
            {
                if(App_cnt-topic1.App_cnt_now>10)
                {
                    if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
                    {
                         topic1.stage=2;
                    }
                    topic1.App_cnt_flag=0;
                }
            } 
             
           
        }
    }
     topic1.last_stage= topic1.stage;
}

void stage_TASK_2()
{
    if( topic2.stage==0)//A点发车
    {
        if(App_cnt>300) 
        {
            topic2.stage=1;
        }
    }

  
    if(topic2.stage==1)//A-BC
    {
         if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
        {  
            if( topic2.App_cnt_flag==0)
            {
                topic2.App_cnt_now=App_cnt;
                topic2.App_cnt_flag=1;
            }
            if(topic2.App_cnt_flag==1)
            {
                if(App_cnt-topic2.App_cnt_now>5)
                {
                    if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
                    {
                         topic2.stage=2;
                    }
                    topic2.App_cnt_flag=0;
                }
            } 
             
           
        }
    }


    if( topic2.stage==2)//B点响
    {
        if(topic2.last_stage==1)
        {
            topic2.App_cnt_now=App_cnt;
        }
        if(App_cnt-topic2.App_cnt_now>50)
        {
            topic2.stage=3;
        }
    }
    if( topic2.stage==3)//B-C
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic2.App_cnt_flag==0)
            {
                topic2.App_cnt_now=App_cnt;
                topic2.App_cnt_flag=1;
            }
            if(topic2.App_cnt_flag==1)
            {
                if(App_cnt-topic2.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic2.stage=4;
                    }
                    topic2.App_cnt_flag=0;
                }
            } 
             
           
        }
    }

    if( topic2.stage==4)//C点响并顺
    {
        if(speed__>180+10)
        {
            topic2.stage=5;
        }

    }
    if( topic2.stage==5)//C-AD
    {
        if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
        {  
            if( topic2.App_cnt_flag==0)
            {
                topic2.App_cnt_now=App_cnt;
                topic2.App_cnt_flag=1;
            }
            if(topic2.App_cnt_flag==1)
            {
                if(App_cnt-topic2.App_cnt_now>5)
                {
                    if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
                    {
                         topic2.stage=6;
                    }
                    topic2.App_cnt_flag=0;
                }
            } 
             
           
        }
    }

    if( topic2.stage==6)//逆自转
    {
         if(speed__-(180-45)<5)
        {
            topic2.stage=7;
        }
    }
     if( topic2.stage==7)///AD-D
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic2.App_cnt_flag==0)
            {
                topic2.App_cnt_now=App_cnt;
                topic2.App_cnt_flag=1;
            }
            if(topic2.App_cnt_flag==1)
            {
                if(App_cnt-topic2.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic2.stage=8;
                    }
                    topic2.App_cnt_flag=0;
                }
            } 
             
           
        }
    }

    if( topic2.stage==8)//D
    {
        

        
        if(speed__-180>-5)
        {
            topic2.stage=9;
        }
    }

    if(topic2.stage==9)//D-A
    {
        if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic2.App_cnt_flag==0)
            {
                topic2.App_cnt_now=App_cnt;
                topic2.App_cnt_flag=1;
            }
            if(topic2.App_cnt_flag==1)
            {
                if(App_cnt-topic2.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic2.stage=10;
                    }
                    topic2.App_cnt_flag=0;
                }
            } 
             
           
        }
    }
   //状态10 A
    topic2.last_stage= topic2.stage;
}

void stage_TASK_3()
{
    if( topic3.stage==0)//A点发车
    {
        if(App_cnt>300) 
        {
            topic3.stage=1;
        }
    }
    if( topic3.stage==1)//顺自旋
    {
        if(speed__-20>-5) 
        {
            topic3.stage=2;
        }
    }
    if(topic3.stage==2)//A-BC
    {
         if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>5)
                {
                    if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
                    {
                         topic3.stage=3;
                    }
                    topic3.App_cnt_flag=0;
                }
            } 
             
           
        }
    }

    if( topic3.stage==3)//BC点  并顺旋转
    {
        
        if(speed__>90-5)
        {
            topic3.stage=4;
        }
    }
    if( topic3.stage==4)//BC-C顺巡线
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic3.stage=5;
                    }
                    topic3.App_cnt_flag=0;
                }
            }   
        }
    }

      if( topic3.stage==5)//C点响并逆旋转
    {
        
        if(speed__<0+5)
        {
            topic3.stage=6;
        }
    }
    if( topic3.stage==6)//C-B
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic3.stage=7;
                    }
                    topic3.App_cnt_flag=0;
                }
            }   
        }
    }
    if( topic3.stage==7)//b点响并逆旋转
    {
       
        if(speed__-(-10.0-180)<5)
        {
            topic3.stage=8;
        }
    }

    if(topic3.stage==8)//B-AD
    {
         if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>5)
                {
                    if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
                    {
                         topic3.stage=9;
                    }
                    topic3.App_cnt_flag=0;
                }
            } 
             
           
        }
    }
    if( topic3.stage==9)//AD点  并逆旋转
    {
        
        if(speed__<-(180+45)+5)
        {
            topic3.stage=10;
        }
    }
    if( topic3.stage==10)//AD-D逆
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic3.stage=11;
                    }
                    topic3.App_cnt_flag=0;
                }
            }   
        }
    }
   
    if( topic3.stage==11)//D点响并顺旋转
    {
        
        if(speed__-(-180)>-5)
        {
            topic3.stage=12;
        }
    }
       if( topic3.stage==12)//D-A
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic3.stage=13;
                    }
                    topic3.App_cnt_flag=0;
                }
            }   
        }
    }
    //13 停车

    topic3.last_stage= topic3.stage;

}
void stage_TASK_4()
{

   if( topic3.stage==0)//A点发车
    {
        if(App_cnt>300) 
        {
            topic3.stage=1;

        }

        speed__=0;
    }
    if( topic3.stage==1)//顺自旋
    {
        if(speed__-20>-5) 
        {
            topic3.stage=2;
        }
    }
    if(topic3.stage==2)//A-BC
    {
         if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>5)
                {
                    if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
                    {
                         topic3.stage=3;
                    }
                    topic3.App_cnt_flag=0;
                }
            } 
             
           
        }
    }

    if( topic3.stage==3)//BC点  并顺旋转
    {
        
        if(speed__>85)
        {
            topic3.stage=4;
        }
    }
    if( topic3.stage==4)//BC-C顺巡线
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic3.stage=5;
                    }
                    topic3.App_cnt_flag=0;
                }
            }   
        }
    }

      if( topic3.stage==5)//C点响并逆旋转
    {
        
        if(speed__<0+5)
        {
            topic3.stage=6;
        }
    }
    if( topic3.stage==6)//C-B
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic3.stage=7;
                    }
                    topic3.App_cnt_flag=0;
                }
            }   
        }
    }
    if( topic3.stage==7)//b点响并逆旋转
    {
       
        if(speed__-(-10.0-180)<5)
        {
            topic3.stage=8;
        }
    }

    if(topic3.stage==8)//B-AD
    {
         if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>5)
                {
                    if(gary[0]||gary[1]||gary[2]||gary[3]||gary[4]||gary[5]||gary[6])
                    {
                         topic3.stage=9;
                    }
                    topic3.App_cnt_flag=0;
                }
            } 
             
           
        }
    }
    if( topic3.stage==9)//AD点  并逆旋转
    {
        
        if(speed__<-(180+45)+5)
        {
            topic3.stage=10;
        }
    }
    if( topic3.stage==10)//AD-D逆
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic3.stage=11;
                    }
                    topic3.App_cnt_flag=0;
                }
            }   
        }
    }
   
    if( topic3.stage==11)//D点响并顺旋转
    {
        
        if(speed__-(-180)>-5)
        {
            topic3.stage=12;
        }
    }
       if( topic3.stage==12)//D-A
    {
         if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
        {  
            if( topic3.App_cnt_flag==0)
            {
                topic3.App_cnt_now=App_cnt;
                topic3.App_cnt_flag=1;
            }
            if(topic3.App_cnt_flag==1)
            {
                if(App_cnt-topic3.App_cnt_now>10)
                {
                    if(gary[0]==0&&gary[1]==0&&gary[2]==0&&gary[3]==0&&gary[4]==0&&gary[5]==0&&gary[6]==0)
                    {
                         topic3.stage=0;
                         topic4_cnt++;
                         if(topic4_cnt>=4)
                         {
                            topic3.stage=13;
                         }
                    }
                    topic3.App_cnt_flag=0;
                }
            }   
        }
    }
    //13 停车

    topic3.last_stage= topic3.stage;


}
void PID_TASK()
{

  
   
    
    
    if(topic_stage==1)
    {
        PID_TASK_1();
    }

    if(topic_stage==2)
    {
       PID_TASK_2();
    }

    if(topic_stage==3)
    {
        PID_TASK_3();
    }

    if(topic_stage==4)
    {
       PID_TASK_4();
    }
    
}
void PID_TASK_1()
{
    speed[2].ref=0;
    speed[2].fdb=speed__;
    pid_NormalCalc(2);


    
    if(topic1.stage==0||topic1.stage==2){speed[0].ref=0;}

    if(topic1.stage==1){speed[0].ref=30*1.15-speed[2].output;;}
    speed[0].fdb=	encoder[0].speed_sum;
    pid_NormalCalc(0);


    
    if(topic1.stage==0||topic1.stage==2){speed[1].ref=0;}
    if(topic1.stage==1){speed[1].ref=-(30*1.15+speed[2].output);}
    speed[1].fdb=	encoder[1].speed_sum;
    pid_NormalCalc(1);
}
void PID_TASK_2()
{
    if(topic2.stage==0||topic2.stage==1||topic2.stage==2||topic2.stage==3)
    {
        speed[2].ref=0;
    }
    else{
        speed[2].ref=15+180;
    }
    speed[2].fdb=speed__;
    pid_NormalCalc(2);

    if(topic2.stage==1||topic2.stage==5)//直行
    {
        speed[0].ref=35*1.15-speed[2].output;
        speed[1].ref=-(35*1.15+speed[2].output);
    }
     if(topic2.stage==2||topic2.stage==3||topic2.stage==9)//巡线
    {
        speed[3].ref=0;
        speed[3].fdb=(5* (gary[0]!=0)+3*(gary[1]!=0)+ (gary[2]!=0)- (gary[4]!=0)-3* (gary[5]!=0)-5* (gary[6]!=0));
        pid_NormalCalc(3);
        speed[0].ref=35-speed[3].output;
        speed[1].ref=-(35*1.3358+speed[3].output);
        
    }
    if(topic2.stage==7)
    {
        speed[3].ref=0;
        speed[3].fdb=(5* (gary[0]!=0)+3*(gary[1]!=0)+ (gary[2]!=0)- (gary[4]!=0)-3* (gary[5]!=0)-5* (gary[6]!=0));
        pid_NormalCalc(3);
        speed[0].ref=35*1.3358-speed[3].output;
        speed[1].ref=-(35+speed[3].output);
    }
    if( topic2.stage==6)//逆
    {
        speed[0].ref=25;
        speed[1].ref=-(-25);
    }
    if( topic2.stage==8||topic2.stage==4)//顺
    {
        speed[0].ref=-25;
        speed[1].ref=(-25);
    }
    if(topic2.stage==10||topic2.stage==0)
    {
        speed[0].ref=0;
        speed[1].ref=0;
    }

    speed[0].fdb=	encoder[0].speed_sum;
    pid_NormalCalc(0);
     speed[1].fdb=encoder[1].speed_sum;
    pid_NormalCalc(1);

}

void PID_TASK_3()
{
    if(topic3.stage==0||topic3.stage==1||topic3.stage==2||topic3.stage==3)
    {
        speed[2].ref=36.0-8.0;
    }
    else{
        speed[2].ref=-(15.0)-180;
    }
    speed[2].fdb=speed__;
    pid_NormalCalc(2);

    if(topic3.stage==2||topic3.stage==8)//直行
    {
        speed[0].ref=30*1.15-speed[2].output;
        speed[1].ref=-(30*1.15+speed[2].output);
    }
     if(topic3.stage==4||topic3.stage==12)//顺巡线
    {
        speed[3].ref=0;
        speed[3].fdb=(5* (gary[0]!=0)+3*(gary[1]!=0)+ (gary[2]!=0)- (gary[4]!=0)-3* (gary[5]!=0)-5* (gary[6]!=0));
        pid_NormalCalc(3);
        speed[0].ref=30-speed[3].output;
        speed[1].ref=-(30*1.3358+speed[3].output);
        
    }
    if(topic3.stage==6||topic3.stage==10)//逆巡线
    {
        speed[3].ref=0;
        speed[3].fdb=(5* (gary[0]!=0)+3*(gary[1]!=0)+ (gary[2]!=0)- (gary[4]!=0)-3* (gary[5]!=0)-5* (gary[6]!=0));
        pid_NormalCalc(3);
        speed[0].ref=30*1.3358-speed[3].output;
        speed[1].ref=-(30+speed[3].output);

    }

    if(topic3.stage==1||topic3.stage==11)//顺自旋出
    {
        speed[0].ref=-(25);
        speed[1].ref=-25;
    }
    if(topic3.stage==3)
    {
        speed[0].ref=-(25);
        speed[1].ref=-25;
    }
    if(topic3.stage==5||topic3.stage==7)//逆自旋入
    {
        speed[0].ref=25;
        speed[1].ref=-(-25);

    }
    if(topic3.stage==9)
    {
        speed[0].ref=+25;
        speed[1].ref=-(-25);

    }

 
    if(topic3.stage==13||topic3.stage==0)
    {
        speed[0].ref=0;
        speed[1].ref=0;
    }

    speed[0].fdb=	encoder[0].speed_sum;
    pid_NormalCalc(0);
     speed[1].fdb=encoder[1].speed_sum;
    pid_NormalCalc(1);

}
void PID_TASK_4()
{
    PID_TASK_3();

}

void  out_put()
{
   
   
  
    if(topic_stage==1)
    {
        out_put_1();
    }

    if(topic_stage==2)
    {
       out_put_2();
    }
     if(topic_stage==3)
    {
        out_put_3();
    }

    if(topic_stage==4)
    {
       out_put_4();
    }
    

}

void  out_put_1()
{
    if(topic1.stage==0||topic1.stage==2){ outputflag=1;}
    if(topic1.stage==1){ outputflag=0;}
}
void  out_put_2()
{
     if(topic2.stage==0||topic2.stage==2||topic2.stage==4|| topic2.stage==10||topic2.stage==8)
     {
        outputflag=1;
     }
     else 
     {
        outputflag=0;
     }
     

}

void  out_put_3()
{
    if(topic3.stage==1||topic3.stage==11||topic3.stage==5|| topic3.stage==7||topic3.stage==13)
     {
        outputflag=1;
     }
     else 
     {
        outputflag=0;
     }

}
void  out_put_4()
{
    out_put_3();

}