广茂达机器人灭火程序(纯C语言版)

float tim_1=0.0;  

float gf_1=0.0;  

int mic_1=0;   

int gi_1=0;    

int gi_2=0;    

int gi_3=0;    

int gi_4=0;     

int gi_5=0;   

int gi_6=0;   

int gi_7=0;   

int gi_8=0;   

int gi_9=0;   

int gi_10=0;  

int ma_1=0;  

int ma_2=0;  

int ma_3=0;  

int ma_4=0;  

int ma_5=0;  

int ma_6=0;  

int ma_7=0;  

int ma_8=0;  

int ma_9=0;  

int ma_10=0;  

int sci_1=0;  

void main()

{

    SCI_Set(0,9600,0,8,1 );

   

   

    gi_10=450;     

    gi_8=310;    

    gi_9=280;   

    ma_10=280; 

    gi_5=1;

   

    gf_1=1.0;

   

   

   

    while(1)

    {

        mic_1=AI(8);

        if(mic_1 > 750)

        {

            break;

        }

    }

    //SetMotor(0x1111,100,100,0,0);

    SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0);

    wait( 0.300000 );

    while(1)

    {                      

   

        while(1)

        {

            ma_7 = AI(6);

            if(ma_7 < gi_8)

            {

                gi_2=AI(9);

                gi_4=AI(11);

                if((gi_2 < gi_10) || (gi_4 < gi_10))

                {

                    //SetMotor(0x1111,90,10,0,0);

                    SetMotor(0x1111,(int)(90*gf_1),(int)(10*gf_1),0,0);    

                    wait( 0.100000 );

                    //SetMotor(0x1111,80,80,0,0);

                    SetMotor(0x1111,(int)(80*gf_1),(int)(80*gf_1),0,0);    

                    wait( 0.050000 );

                    while(1)

                    {

                        ma_7 = AI(6);

                        gi_2=AI(9);

                        if((ma_7 < gi_8) && (gi_2 < 300))

                        {

                            StopMotor(0x1111);

                            //SetMotor(0x1111,100,-100,0,0);

                            SetMotor(0x1111,(int)(100*gf_1),(int)(-100*gf_1),0,0);

                            wait( 0.080000 );

                            StopMotor(0x1111);

                            DO( 0x1, 1 );

                            tim_1 =seconds();

                            wait( 0.500000 );

                            DO( 0x1, 0 );

                           

                            break;

                        }

                        else

                        {

    SubRoutine_2 ();

   

                        }

                    }

                    if(gi_5 == 1)

                    {

                        gi_1=1;

                        break;

                    }

    SubRoutine_3 ();

   

                    gi_1 = 1 ;

                    break;

                }

                else     

                {

                    StopMotor(0x1111);

                    wait( 0.180000 );

                    //SetMotor(0x1111,-60,-100,0,0);

                    SetMotor(0x1111,(int)(-40*gf_1),(int)(-100*gf_1),0,0);

                    wait( 0.300000 );

                    break;

                }

            }

            else

            {

    SubRoutine_1 ();

   

            }

        }

        if(gi_1 == 1)

        {

            break;

        }

                              

     

        while(1)

        {

            ma_8 = AI(7);

            if(ma_8 < gi_8)

            {

                gi_2=AI(13);

                gi_4=AI(15);

                if((gi_2 < gi_10) || (gi_4 < gi_10))

                {

                    //SetMotor(0x1111,-80,-10,0,0);

                    SetMotor(0x1111,(int)(-80*gf_1),(int)(-10*gf_1),0,0);  

                    wait( 0.100000 );

                    while(1)

                    {

                        ma_8 = AI(7);

                        gi_2=AI(15);

                        if((ma_8 < gi_8) && (gi_2 < 300)) 

转载于:https://www.cnblogs.com/shiningrise/archive/2009/05/08/1452352.html