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