示例#1
0
/**
 * @brief DetermineAction
 *
 * Calculate the power of left and right motors, by determining the actions
 * of the cat
 *
 * @param beaconToFollow - id of the beacon for the cat to follow
 * @param lPow - power of left motor
 * @param rPow - power of right motor
 * @param state - state of the robot relating to wall bypassing
 */
void DetermineAction(int beaconToFollow, double *lPow, double *rPow, int *state)
{
    static int counter=0;
    static float CollisionOrientation = 0.0;

    bool   beaconReady;
    static struct beaconMeasure beacon; // beacon sensor
    static float  left; //value of frontal left sonar sensor
    static float right; //value of frontal rigth sonar sensor
    static float center; //value of frontal center sonar sensor
    //static int    Ground;
    static bool   Collision;// collision sensor
    static float Compass; //compass sensor




  // SENSORS ACCESS

    /*Access to values from Sensors - Only ReadSensors() gets new values */
    if(IsObstacleReady(LEFT))
        left=     GetObstacleSensor(LEFT);
    if(IsObstacleReady(RIGHT))
        right=    GetObstacleSensor(RIGHT);
    if(IsObstacleReady(CENTER))
        center=   GetObstacleSensor(CENTER);

    beaconReady = IsBeaconReady(beaconToFollow);
    if(beaconReady) {
       beacon =  GetBeaconSensor(beaconToFollow);
    }
    else beaconReady=0;

    //if(IsGroundReady())
    //    Ground=    GetGroundSensor();
    if(IsBumperReady())
        Collision= GetBumperSensor();
    if(IsCompassReady()){
        Compass= GetCompassSensor();
    }

    if(beaconReady){
        if(beacon.beaconVisible){
            if(center < 3.0){
                if(beacon.beaconDir > 20.0 && left < 4.0){
                    *lPow=0.0;
                    *rPow=0.1;
                    *state = RUNNING;
                }
                else if(beacon.beaconDir < -20.0 && right < 4.0){
                    *lPow=0.1;
                    *rPow=0.0;
                    *state = RUNNING;
                }
                else { /* Full Speed Ahead */
                   *lPow=0.1;
                   *rPow=0.1;
                }
                counter++;
                return;
            }
        }
    }

    if(center>3.0 || right> 4.0 || left>4.0 || Collision) { /* Close Obstacle - Rotate */
        if(right < left) {
              *lPow=0.06;
               *rPow=-0.06;
            if(*state != BYPASSING_RIGTH){
                CollisionOrientation = Compass;
               *state = BYPASSING_RIGTH;
            }
        }  else {
               *lPow=-0.06;
               *rPow=0.06;
            if(*state != BYPASSING_LEFT){
                CollisionOrientation = Compass;
               *state = BYPASSING_LEFT;
            }
        }

    } else if(*state == BYPASSING_RIGTH && left>3.0){ // if its still bypassing an obstacle through the right side
        if(left < 3.5 ){
            *lPow=0.05;
            *rPow=0.07;
        } else if(left > 3.7 ){
            *lPow=0.07;
            *rPow=0.05;
        } else {
            *lPow=0.05;
            *rPow=0.05;
        }
    } else if(*state == BYPASSING_LEFT && right>3.0){ // if its still bypassing an obstacle through the left side
        if(right < 3.5 ){
            *lPow=0.07;
            *rPow=0.05;
        } else if(right > 3.7 ){
            *lPow=0.05;
            *rPow=0.07;
        } else {
            *lPow=0.05;
            *rPow=0.05;
        }
    } else {
        if(*state != RUNNING){
            //difference between orientation when approached an obstacle and current one
            float diff = Compass - CollisionOrientation;
            if(diff < 10 && diff > -10){
                *state = RUNNING;
            }
        }

        if(*state == BYPASSING_LEFT){
            *lPow=0.07;
            *rPow=0.01;

        }
        else if(*state == BYPASSING_RIGTH){
            *lPow=0.01;
            *rPow=0.07;
        }
        else { /* Full Speed Ahead */
           *lPow=0.1;
           *rPow=0.1;
        }
    }




    counter++;
}
示例#2
0
int main(int argc, char *argv[])
{
	char cfg[MAX_FILE_NAME_SIZE]="config.json";
	
	float lPow,rPow;
	int state=STOP, stoppedState=RUN;
	int beaconToFollow=0;
	int ret = 0;
	rob_cfg_t rob_cfg;
	rob_state_t rob_state;
	struct beaconMeasure beacon;
	int totalBeacons = 0,curGroundSensor = -1;
	double elapsed1 = 0.0, elapsed2 = 0.0, realTotal = 0.0;
	struct timeval t1, t2, t3;
	bool firstTimeStart = 1;

	memset(&rob_state, 0, sizeof(rob_state_t));


	 /* processing arguments */
	while (argc > 2)
	{
		if (strcmp(argv[1], "-cfg") == 0)
		{
		   strncpy(cfg, argv[2], 99);
		   cfg[MAX_FILE_NAME_SIZE-1]='\0';
		}
		else
		{
				break; /* the while */
		}
		argc -= 2;
		argv += 2;
	}

	cfg_parser_parse(cfg, &rob_cfg);
	// int i;
	// for(i = 0; i < rob_cfg.rob_viewer_size; i++)
	// 	printf("Viewer: %s:%d\n", rob_cfg.rob_viewers[i].hostname, rob_cfg.rob_viewers[i].port);

	InitJoystick(rob_cfg.joys_dev);

	cfg_parser_connect_viewers(&rob_cfg);

	/* Connect Robot to simulator */
	if( InitRobot(rob_cfg.robo_name, rob_cfg.robo_pos, rob_cfg.hostname) == -1)
	{
		ret = 1;
		printf( "%s Failed to connect\n", rob_cfg.robo_name);
	}

	else
	{
		totalBeacons = GetNumberOfBeacons();

		printf( "Connected: %s, Total beacons: %d\n", rob_cfg.robo_name, totalBeacons);

		state=STOP;
		while(1)
		{
			/* Reading next values from Sensors */
			ReadSensors();

			if(GetFinished()) /* Simulator has received Finish() or Robot Removed */
			{
				printf(  "Exiting: %s\n", rob_cfg.robo_name );
				state = FINISHED;

				gettimeofday(&t3, NULL);

				elapsed2 = _get_elapsed_secs(&t2, &t3);
				realTotal = _get_elapsed_secs(&t1, &t3);

				printf("to beacon | to start | total | real total\n");
				printf("& %.2f & %.2f & %.2f & %.2f \n", elapsed1, elapsed2, elapsed1 + elapsed2, realTotal);

				break;
			}

			if(state==STOP && GetStartButton()) 
			{
				state=stoppedState;  /* Restart     */

				if( firstTimeStart )
				{
					firstTimeStart = 0;
					printf("Started counting elapsed time\n");
					
					gettimeofday(&t1, NULL);
				}
			}
			if(state!=STOP && GetStopButton())  {
				stoppedState=state;
				state=STOP; /* Interrupt */
			}

			curGroundSensor = GetGroundSensor();

			switch (state)
			{
				case RUN:    /* Go */
					
					if( GetVisitingLed() )
					{
						gettimeofday(&t2, NULL);

						elapsed1 = _get_elapsed_secs(&t1, &t2);

						printf("Elapsed from origin to beacon: %f\n", elapsed1);


						state = WAIT;
						DriveMotors(0.0,0.0);
					}
					else
					{
						if( curGroundSensor == beaconToFollow )
						{
							beaconToFollow++;
							SetVisitingLed(1);
							printf("%s visited target at %d\n", rob_cfg.robo_name, GetTime());
						}
						else {

							DetermineAction(beaconToFollow, &lPow, &rPow);
							DriveMotors(lPow, rPow);
						}
					}
				
					break;

				case RETURN:    /* Go */

					if( curGroundSensor == totalBeacons )
					{
						printf("%s found home at %d\n", rob_cfg.robo_name, GetTime());
						Finish();
					}
					else {
						DetermineAction(beaconToFollow, &lPow, &rPow);
						DriveMotors(lPow, rPow);
					}
				
					break;

				case WAIT: /* Wait for others to visit target */

					if(GetReturningLed())
					{
						SetVisitingLed(0);
						state = RETURN;

						gettimeofday(&t2, NULL);
					}

					DriveMotors(0.0,0.0);

					break;
			}

			//Say(rob_cfg.robo_name);


			rob_state.state = state;

			if( (rob_state.leftAvail = IsObstacleReady(LEFT)) )
				rob_state.left = GetObstacleSensor(LEFT);

			if( (rob_state.rightAvail = IsObstacleReady(RIGHT)) )
				rob_state.right = GetObstacleSensor(RIGHT);

			if( (rob_state.centerAvail = IsObstacleReady(CENTER)) )
				rob_state.center = GetObstacleSensor(CENTER);


			if(IsGPSReady())
			{
				rob_state.x = GetX();
				rob_state.y = GetY();
			}

			// if( IsGPSDirReady() )
			// 	rob_state.dir = GetDir();

			if( IsCompassReady() )
				rob_state.dir = GetCompassSensor();


			if( ( rob_state.beaconVis = IsBeaconReady(beaconToFollow) ) )
			{
				beacon = GetBeaconSensor(beaconToFollow);

				if( ( rob_state.beaconVis = beacon.beaconVisible ) )
					rob_state.beaconDir = beacon.beaconDir;
			}

			send_all_viewer_state_message(&rob_cfg, &rob_state);


			RequestCompassSensor();

			//Request Sensors for next cycle
			if(GetTime() % 2 == 0) {

				RequestObstacleSensor(CENTER);

				if(    (GetTime() % 8) == 0
					|| beaconToFollow == totalBeacons )
					RequestGroundSensor();
				else
					RequestBeaconSensor(beaconToFollow);

			}
			else {
				RequestSensors(2, "IRSensor1", "IRSensor2");
			}
		}

		send_all_viewer_state_message(&rob_cfg, &rob_state);

	}

	printf("Doing cleanup: %s\n", rob_cfg.robo_name);

	CloseAndFreeJoystick();

	cfg_parser_close(&rob_cfg);

	return ret;
}
示例#3
0
/* Calculate the power of left and right motors */
void DetermineMouseAction(double *lPow, double *rPow, int *, RobotMap *map,externalRobot *robots)
{
    static int counter=0;


    static float  left; //value of frontal left sonar sensor
    static float right; //value of frontal rigth sonar sensor
    static float center; //value of frontal center sonar sensor
    static bool   Collision;// collision sensor
    static float Compass = 0; //compass sensor
    static float X = -1; // GPS x value
    static float Y = -1; //GPS yvalue


  // SENSORS ACCESS

    /*Access to values from Sensors - Only ReadSensors() gets new values */
    if(IsObstacleReady(LEFT))
        left=     GetObstacleSensor(LEFT);
    if(IsObstacleReady(RIGHT))
        right=    GetObstacleSensor(RIGHT);
    if(IsObstacleReady(CENTER))
        center=   GetObstacleSensor(CENTER);

    if(IsBumperReady())
        Collision= GetBumperSensor();
    if(IsCompassReady()){
        Compass= GetCompassSensor();
        //printf("orientation %f\n", Compass);
    }
    if(IsGPSReady()){
        if(X < 0 || Y < 0){
            map->setOffset(GetX(),GetY());
        }
        X= GetX();
        Y= GetY();
    }



    float multiplier[8] = {1,1,1,1,1,1,1,1};


    if(center>3.0 || right> 4.0 || left>4.0 || Collision) { /* Close Obstacle - Rotate */
        if(right < left) {
              *lPow=0.06;
               *rPow=-0.06;
        }  else {
               *lPow=-0.06;
               *rPow=0.06;
        }

    } else {
        if(left > 1.0){
            //give a penalty on score if theres an obstacle on the left side
            multiplier[side_frontleft] = multiplier[side_left] = multiplier[side_backleft] = 1/left;
        }
        if(right > 1.0){
            //give a penalty on score if theres an obstacle on the right side
            multiplier[side_frontright] = multiplier[side_right] = multiplier[side_backright] = 1/right;
        }
        if(center > 1.0){
            //give a penalty on score if theres an obstacle on the front
            multiplier[side_frontright] = multiplier[side_frontleft] = multiplier[side_front] = 1.25/right;
        }

        //always favor going to front to avoid slowing down
        multiplier[side_frontright] *= 1.25;
        multiplier[side_frontleft] *= 1.25;
        multiplier[side_front] *= 1.25;





        float orientations[8] = {Compass,Compass + 45,Compass + 90,Compass + 135,Compass + 180,
                Compass - 135,Compass - 90,Compass - 45};

        float distance = 2;

        float score[8];

        float maxScore = 0;
        int maxScoreSide = 0;


        for(int i=0;i<8;++i){
            float x= X + distance * cos(M_PI * orientations[i] / 180);
            float y= Y + distance * sin(M_PI * orientations[i] / 180);

            if(i == side_front){
                double val = map->getValue(x,y);
                if(center < 2.0){
                    val += 0.2;
                    map->setValue(x,y,(val > 1.0)?1.0:val);
                } else {
                    val -= 0.2;
                    map->setValue(x,y,(val < 0.0)?0.0:val);
                }
            } else if(i == side_frontleft){
                double val = map->getValue(x,y);
                if(left < 2.0){
                    val += 0.2;
                    map->setValue(x,y,(val > 1.0)?1.0:val);
                } else {
                    val -= 0.2;
                    map->setValue(x,y,(val < 0.0)?0.0:val);
                }
            }else if(i == side_frontright){
                double val = map->getValue(x,y);
                if(right < 2.0){
                    val += 0.2;
                    map->setValue(x,y,(val > 1.0)?1.0:val);
                } else {
                    val -= 0.2;
                    map->setValue(x,y,(val < 0.0)?0.0:val);
                }
            }

            float points = 0;
            for(int j=0;j<5;++j){
                if(robots[j].isCat){
                    points += negamax(x,y,robots[j].x,robots[j].y);
                }
            }

            multiplier[i] -= penaltyScore(map,x,y);


            points *= multiplier[i];
            score[i] = points;
            if(i==0){
               maxScore = points;

            } else if(points > maxScore){
                maxScore = points;
                maxScoreSide = i;
            }
        }
        switch (maxScoreSide) {
        case side_front:
            *lPow=0.1;
            *rPow=0.1;
            break;
        case side_frontleft:
            *lPow=0.08;
            *rPow=0.1;
            break;
        case side_left:
            *lPow=0.03;
            *rPow=0.1;
            break;
        case side_backleft:
            *lPow=-0.02;
            *rPow=0.1;
            break;
        case side_back:
            if(score[3] > score[5]){
                *lPow=-0.1;
                *rPow=0.1;
            } else {
                *lPow=0.1;
                *rPow=-0.1;
            }
            break;
        case side_backright:
            *rPow=-0.02;
            *lPow=0.1;
            break;
        case side_right:
            *rPow=0.03;
            *lPow=0.1;
            break;
        case side_frontright:
            *rPow=0.08;
            *lPow=0.1;
            break;
        default:
            break;
        }
    }




    counter++;
}
示例#4
0
/* Calculate the power of left and right motors */
void DetermineAction(int beaconToFollow, float *lPow, float *rPow)
{
	static int counter=0;

	bool   beaconReady;
	static struct beaconMeasure beacon;
	static float  left, right, center;
	static int    Ground;
	static bool   Collision;
	static float Compass;

	/*Access to values from Sensors - Only ReadSensors() gets new values */
	//Calcula a distancia 
	if(IsObstacleReady(LEFT))
	left=     GetObstacleSensor(LEFT);
	if(IsObstacleReady(RIGHT))
	right=    GetObstacleSensor(RIGHT);
	if(IsObstacleReady(CENTER))
	center=   GetObstacleSensor(CENTER);

	beaconReady = IsBeaconReady(beaconToFollow);
	if(beaconReady) {
	beacon =  GetBeaconSensor(beaconToFollow);
	}
	else beaconReady=0;

	if(IsGroundReady())
	Ground=    GetGroundSensor();
	if(IsBumperReady())
	Collision= GetBumperSensor();
	if(IsCompassReady())
	Compass= GetCompassSensor();



	/*
	if(Collision) { 
		if(counter % 600 < 300) {
		   *lPow=0.06;
		   *rPow=-0.06; }
		else {
		   *lPow=-0.06;
		   *rPow=0.06; }
	}else if(center > 8){
		if(counter % 600 < 300) {
		   *lPow=0.06;
		   *rPow=0.03; }
		else {
		   *lPow=0.03;
		   *rPow=0.06; }
	}
	else if(right>3) { 
	  	*lPow=0.01;
		*rPow=0.03;
	}
	else if(left>3) {
		*lPow=0.03;
		*rPow=0.01;
	}
	else {
		if(beaconReady && beacon.beaconVisible && beacon.beaconDir>20.0) { 
		   *lPow=0.0;
		   *rPow=0.3;
		}
		else if(beaconReady && beacon.beaconVisible && beacon.beaconDir<-20.0) {
		   *lPow=0.3;
		   *rPow=0.0;
		}
		else { 
		  *lPow=0.1;
		   *rPow=0.1;
		}
	}*/

	//if(Collision && 
    if(center>4.5||Collision) { /* Close Obstacle - Rotate */
        
	/*if(center>4.5){
		if(Compass < -90 && Compass > 90){
			 *lPow=-0.05;
          		 *rPow=0.05; 
		}else{
			 *lPow=0.05;
      			 *rPow=-0.05; 
		}
	}else{*/
		if(counter % 400 < 200) {
		   *lPow=0.05;
		   *rPow=-0.05; }
		else {
		   *lPow=-0.05;
		   *rPow=0.05; }
		
	//}	
	counter++;
	return;

    }


if(beaconReady && beacon.beaconVisible && beacon.beaconDir>20.0) { /* turn to Beacon */
           *lPow=0.0;
           *rPow=beacon.beaconDir*0.08;
        }
        else if(beaconReady && beacon.beaconVisible && beacon.beaconDir<-20.0) {
           *lPow=-(beacon.beaconDir*0.08);
           *rPow=0.0;
        } else if(beaconReady && beacon.beaconVisible ) {
		*lPow=0.1;
          	*rPow=0.1;
	}
    /*else if(center > 2){
	if(r == -10.0){
		r = ((rand() % 10)-5)*0.1;
		counter++;
		return;
	}
}*/else
	        if(center > 1 && right > 2.0){
			*lPow=0.03;
			*rPow=0.1;
			counter++;
			return;
		}else if(center > 1 && left > 4.0){
			*lPow=0.1;
			*rPow=0.03;
			counter++;
			return;
		}else /*if(center > 2 && right > 4.0 && Compass < -90 && Compass > 90){
			*lPow=0.1;
			*rPow=0.07;

		}else if(center > 2 && left > 4.0 && (Compass >= -90 || Compass <= 90)){
			*lPow=0.07;
			*rPow=0.1;

		}else*/
	
	
   if(right>4) { 
        *lPow=-0.04;
        *rPow=0.04;
    }
    else if(left>4) {
        *lPow=0.04;
        *rPow=-0.04;

   }else    if(right>1.5) {
        *lPow=0.0;
        *rPow=0.1;
    }
    else if(left>1.5) {
        *lPow=0.1;
        *rPow=0.0;
    }
    else { 
       
      
           *lPow=0.1;
           *rPow=0.1;
       
    }
	counter++;
}