/* 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++; }
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; }