gfc.cpp

机器人足球的一个自学的源代码

                if(vl <= 0)
                        vl = 1;
                if(beta < 1)
                        vl = 125;
        }
        run(robot, rID,vl,vr);
        length = l(x,y,rx,ry);
        if(beta > 90.0)
                beta = 180.0 - beta;
        if(length < 3 && beta > 30.0)
                sRotate(robot,rID,alpha);
        beta = fabs(HOMEVDIRECTION[rID] - ra);
        if(HOMEVDIRECTION[rID] != -1)
                if((beta>180.0?360-beta:beta) > 90.0)
                        run(robot,rID,0,0);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%d %d %d %f %d %f\n",rID,vl,vr,beta,HOMEEV[rID],HOMEVDIRECTION[rID]);
        //fclose(DEBUGFILE);
}

void sGo(Robot *robot, int rID, double x, double y){
        //
        double rx,ry;
        double ra,la; //robot angel,line angel
        double beta;

        rx = robot->pos.x;
        ry = robot->pos.y;
        la = lineAngel(rx,ry,x,y);
        ra = robotAngel(robot, rID);
        beta = fabs(ra - la);
        if(beta > 180.0)
                beta = 360.0 - beta;
        if(beta < 90.0)
                goTo(robot,rID,x,y);
        else
                goBackTo(robot,rID,x,y);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%f %f\n",ra,la);
        //fclose(DEBUGFILE);
}

void to(Robot *robot, int rID, double x, double y){
        //
        double rx,ry;
        double length;
        double alpha,ra,beta; //直线、机器人角度,夹角
        int dire; //1 正 -1逆 时针
        int vl,vr;
        int v0;
        double le; //误差

        le = 0.3;
        rx = robot->pos.x;
        ry = robot->pos.y;
        length = l(rx,ry,x,y);
        alpha = lineAngel(rx,ry,x,y);
        ra = robotAngel(robot,rID);
        beta = fabs(ra - alpha); //计算夹角
        dire = direction(ra,alpha);
        if(beta > 180)
                beta = 360 - beta;
        else 
                beta = beta;
        if(length > 8.0)
                v0 = 125;
        else if(length > le){
                if(HOMEEV[rID] >= int(11*length))
                        v0 = 1;
                else if(HOMEEV[rID] <= int(length))
                        v0 = int(125*length);
                else if(HOMEEV[rID] <= int(2*length))
                        v0 = int(100*length);
                else if(HOMEEV[rID] <= int(5*length))
                        v0 = int(80*length);
                else if(HOMEEV[rID] <= int(8*length))
                        v0 = int(50*length);
                else
                        v0 = int(10*length) +1;
                if(v0 > 125)
                v0 = 125;
        }
        else if(length < le)
                v0 = 0;
        if(length > le){
                if(dire == 1){
                        vl = v0;
                        vr = int(v0 - beta*25/36); //假定在 6 个周期内完成转动
                        if(vr <= 0)
                                vr = 1;
                        if(beta < 1.0)
                                vr = v0;
                }
                else if(dire == -1){
                        vr = v0;
                        vl = int(v0 - beta*25/36); //假定在 6 个周期内完成转动
                        if(vl <= 0)
                                vl = 1;
                        if(beta < 1.0)
                                vl = v0;
                }
                run(robot, rID,vl,vr);
        }
        else{
                run(robot,rID,0,0);
        }
        if(beta > 90.0)
                beta = 180.0 - beta;
        if(length < 3 && beta > 30.0)
                sRotate(robot,rID,alpha);
        beta = fabs(HOMEVDIRECTION[rID] - ra);
        if(HOMEVDIRECTION[rID] != -1)
                if((beta>180.0?360-beta:beta) > 90.0)
                        run(robot,rID,0,0);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%d %d %f %d %f %f\n",dire, HOMEEV[rID],length,v0,HOMEVDIRECTION[rID] ,ra);
        //fclose(DEBUGFILE);
}
void backTo(Robot *robot, int rID, double x, double y){
        //
        double rx,ry;
        double length;
        double alpha,ra,beta; //直线、机器人角度,夹角
        int dire; //1 正 -1逆 时针
        int vl,vr;
        int v0;
        double le; //误差

        le = 0.3;
        rx = robot->pos.x;
        ry = robot->pos.y;
        length = l(rx,ry,x,y);
        alpha = lineAngel(rx,ry,x,y);
        ra = robotAngel(robot,rID);
        ra += 180.0;
        if(ra >= 360.0)
                ra = ra - 360.0;
        beta = fabs(ra - alpha); //计算夹角
        dire = direction(ra,alpha);
        if(beta > 180)
                beta = 360 - beta;
        else 
                beta = beta;
        if(length > 8.0)
                v0 = -125;
        else if(length > le){
                if(HOMEEV[rID] >= int(11*length))
                        v0 = -3;
                else if(HOMEEV[rID] <= int(length))
                        v0 = int(-125*length);
                else if(HOMEEV[rID] <= int(2*length))
                        v0 = int(-100*length);
                else if(HOMEEV[rID] <= int(5*length))
                        v0 = int(-80*length);
                else if(HOMEEV[rID] <= int(8*length))
                        v0 = int(-50*length);
                else
                        v0 = int(-10*length) - 1;
                if(v0 < -125)
                        v0 = -125;
        }
        else if(length < le)
                v0 = 0;
        if(length > le){
                if(dire == 1){
                        vr = v0;
                        vl = int(v0 + beta*25/36); //假定在 6 个周期内完成转动
                        if(vl >= 0)
                                vl = -1;
                        if(beta < 1.0)
                                vl = v0;
                }
                else if(dire == -1){
                        vl = v0;
                        vr = int(v0 + beta*25/36); //假定在 6 个周期内完成转动
                        if(vr >= 0)
                                vr = -1;
                        if(beta < 1.0)
                                vr = v0;
                }
                run(robot, rID,vl,vr);
        }
        else{
                run(robot,rID,0,0);
        }
        if(beta > 90.0)
                beta = 180.0 - beta;
        if(length < 3 && beta > 30.0)
                sRotate(robot,rID,alpha);
        beta = fabs(HOMEVDIRECTION[rID] - ra);
        if(HOMEVDIRECTION[rID] != -1)
                if((beta>180.0?360.0-beta:beta) > 90.0)
                        run(robot,rID,0,0);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%d %d %f %d %f %f %f %d %d\n",dire, HOMEEV[rID],length,v0,beta,ra,alpha,vl,vr);
        //fclose(DEBUGFILE);
}
void sTo(Robot *robot, int rID, double x, double y){
        //
        double rx,ry;
        double ra,la; //robot angel,line angel
        double beta;

        rx = robot->pos.x;
        ry = robot->pos.y;
        la = lineAngel(rx,ry,x,y);
        ra = robotAngel(robot, rID);
        beta = fabs(ra - la);
        if(beta > 180.0)
                beta = 360.0 - beta;
        if(beta < 90.0)
                to(robot,rID,x,y);
        else
                backTo(robot,rID,x,y);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%f %f\n",ra,la);
        //fclose(DEBUGFILE);
}

void rotateTo(Robot *robot, int rID, double x, double y){
        //
        int dire; //旋转方向
        double rx,ry,ra; //机器人坐标和角度
        double la,beta; //线角度
        int v0;

        rx = robot->pos.x;
        ry = robot->pos.y;
        ra = robotAngel(robot,rID);
        la = lineAngel(rx,ry,x,y);
        dire = direction(ra,la);
        beta = ra - la;
        if(beta < -180.0)
                beta += 360.0;
        if(beta > 180.0)
                beta = 360.0 - beta;
        v0 = int(fabs(beta)*5/12); //速度选择
        if(v0 < 3)
                v0 =3;
        else if(v0 > 60)
                v0 = 60;
        if(dire == 1)
                run(robot, rID,v0,-1*v0);
        else if(dire == -1)
                run(robot, rID, -1*v0,v0);
        if(fabs(beta) < 1.5)
                run(robot,rID,0,0);
        if(HOMEDISPLACEMENT[rID] > 0.05)
                run(robot,rID,0,0);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%d %f\n",dire,beta);
        //fclose(DEBUGFILE);
}

void rotateBackTo(Robot *robot,int rID, double x,double y){
        //
        double rx,ry;
        double dx,dy;

        rx = robot->pos.x;
        ry = robot->pos.y;
        dx = 2*rx - x;
        dy = 2*ry - y;
        rotateTo(robot,rID,dx,dy);
}

void sRotate(Robot *robot, int rID,double x, double y){
        //
        double rx,ry;
        double ra,la; //robot angel,line angel
        double beta;

        rx = robot->pos.x;
        ry = robot->pos.y;
        la = lineAngel(rx,ry,x,y);
        ra = robotAngel(robot, rID);
        beta = fabs(ra - la);
        if(beta > 180.0)
                beta = 360.0 - beta;
        if(beta < 90.0)
                rotateTo(robot,rID,x,y);
        else
                rotateBackTo(robot,rID,x,y);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%f %f\n",ra,la);
        //fclose(DEBUGFILE);
}

void sRotate(Robot *robot, int rID,double angel){
        //
        double dx;
        double dy,k;

        if(fabs(angel - PI/2) < 0.05){
                dx = 0;
                dy = 50;
        }
        else if(fabs(angel - PI*3/2) < 0.05){
                dx = 0;
                dy = -50;
        }
        else{
                k = tanf(angel/180.0*PI);
                dx = 100;
                dy = dx*k;
        }
        sRotate(robot,rID,robot->pos.x + dx,robot->pos.y + dy);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%f %f %f %f %f\n",dx,dy,angel,tanf(angel/180.0*PI),robotAngel(robot,rID));
        //fclose(DEBUGFILE);
}

void order(){
        //
        double rx[5],ry[5],len[5];
        double bx,by;
        int lt,ll;
        int temp;

        lt = 0;
        ll = 0;
        bx = ENV->currentBall.pos.x;
        by = ENV->currentBall.pos.y;
        for(lt = 0; lt < 5; lt++){
                rx[lt] = ENV->home[lt].pos.x;
                ry[lt] = ENV->home[lt].pos.y;
                len[lt] = l(rx[lt], ry[lt], bx, by);
        }
        for(lt = 0; lt < 5; lt++){
                for(ll = lt+1; ll < 5; ll++){
                        if(len[ORDER[lt]] > len[ORDER[ll]]){
                                temp = ORDER[ll];
                                ORDER[ll] = ORDER[lt];
                                ORDER[lt] = temp;
                        }
                }
        }
        for(lt = 0; lt< 5; lt++){
                for(ll = 0;ll <5; ll++){