Example #1
0
int main(int argc, char *argv[])
{
    char host[100]="localhost";
    char rob_name[20]="robsample";
    float lPow,rPow;
    int state=STOP, stoppedState=RUN, rob_id = 1;
    int beaconToFollow=0;

    printf( " Sample Robot\n Copyright (C) 2001-2013 Universidade de Aveiro\n" );

     /* processing arguments */
    while (argc > 2) /* every option has a value, thus argc must be 1, 3, 5, ... */
    {
        if (strcmp(argv[1], "-host") == 0)
        {
           strncpy(host, argv[2], 99);
           host[99]='\0';
        }
        else if (strcmp(argv[1], "-robname") == 0)
        {
           strncpy(rob_name, argv[2], 19);
           rob_name[19]='\0';
        }
        else if (strcmp(argv[1], "-pos") == 0)
        {
            if(sscanf(argv[2], "%d", &rob_id)!=1)
               argc=0; /* error message will be printed */
        }
        else
        {
                break; /* the while */
        }
        argc -= 2;
        argv += 2;
    }

    if (argc != 1)
    {
        fprintf(stderr, "Bad number of parameters\n"
                "SYNOPSIS: mainRob [-host hostname] [-robname robotname] [-pos posnumber]\n");

        return 1;
    }

    /* Connect Robot to simulator */
    if(InitRobot(rob_name, rob_id, host)==-1)
    {
       printf( "%s Failed to connect\n", rob_name); 
       exit(1);
    }
    printf( "%s Connected\n", rob_name );
    state=STOP;
    while(1)
    {
        /* Reading next values from Sensors */
        ReadSensors();

        if(GetFinished()) /* Simulator has received Finish() or Robot Removed */
        {
           printf(  "%s Exiting\n", rob_name );
           exit(0);
        }
        if(state==STOP && GetStartButton()) state=stoppedState;  /* Restart     */
        if(state!=STOP && GetStopButton())  {
            stoppedState=state;
            state=STOP; /* Interrupt */
        }

        switch (state) { 
                 case RUN:    /* Go */
		  if( GetVisitingLed() ) state = WAIT;
                  if(GetGroundSensor()==0) {         /* Visit Target */
                     SetVisitingLed(1);
                     printf("%s visited target at %d\n", rob_name, GetTime());
                  }

                  else {
                     DetermineAction(0,&lPow,&rPow);
                     DriveMotors(lPow,rPow);
                  }
                  break;
		 case WAIT: /* Wait for others to visit target */
		     if(GetReturningLed()) state = RETURN;

                     DriveMotors(0.0,0.0);
                     break;
		 case RETURN: /* Return to home area */
		     if(GetGroundSensor()==1) { /* Finish */
                         Finish();
                         printf("%s found home at %d\n", rob_name, GetTime());
                     }
                     else {
                        DetermineAction(1,&lPow,&rPow);
                        DriveMotors(lPow,rPow);
                     }
                     break;
	}

        Say(rob_name);

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

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

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

    }
    return 1;
}
Example #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;
}