// Calculate the current round int calculateRound (char *pastPlays) { //Setup Variables assert(pastPlays != NULL); int currRound = 0; //Divide no. of turns by no. of players to find round currRound = calculateTurn(pastPlays) / NUM_PLAYERS; return currRound; }
// Calculate the current player int calculatePlayer (char *pastPlays) { //Setup Variables assert(pastPlays != NULL); int player = 0; //Case if first turn if (calculateTurn(pastPlays) == 0) { return PLAYER_LORD_GODALMING; } //The current player is determined by looking at the turn and round player = calculateTurn(pastPlays) - (calculateRound(pastPlays) * NUM_PLAYERS); //Adjust to ensure valid players if (player == PLAYER_DRACULA + 1) { player = PLAYER_LORD_GODALMING; } return player; }
// Creates a new GameView to summarise the current state of the game GameView newGameView(char *pastPlays, PlayerMessage messages[]) { //Initialise GameView GameView gameView = malloc(sizeof(struct gameView)); assert(gameView != NULL); //Add messages to struct //gameView->messages = messages; //Establish struct fields gameView->currScore = calculateScore(pastPlays); gameView->currRound = calculateRound(pastPlays); gameView->currTurn = calculateTurn(pastPlays); gameView->currPlayer = calculatePlayer(pastPlays); gameView->currHealth[PLAYER_LORD_GODALMING] = calculateHunterHealth(pastPlays, PLAYER_LORD_GODALMING); gameView->currHealth[PLAYER_DR_SEWARD] = calculateHunterHealth(pastPlays, PLAYER_DR_SEWARD); gameView->currHealth[PLAYER_VAN_HELSING] = calculateHunterHealth(pastPlays, PLAYER_VAN_HELSING); gameView->currHealth[PLAYER_MINA_HARKER] = calculateHunterHealth(pastPlays, PLAYER_MINA_HARKER); gameView->currHealth[PLAYER_DRACULA] = calculateDraculaHealth(pastPlays); int trail[TRAIL_SIZE] = {}; int i = 0; calculateTrail(pastPlays, PLAYER_LORD_GODALMING, trail); for (i = 0; i < TRAIL_SIZE; i++) { gameView->trail[PLAYER_LORD_GODALMING][i] = trail[i]; } calculateTrail(pastPlays, PLAYER_DR_SEWARD, trail); for (i = 0; i < TRAIL_SIZE; i++) { gameView->trail[PLAYER_DR_SEWARD][i] = trail[i]; } calculateTrail(pastPlays, PLAYER_VAN_HELSING, trail); for (i = 0; i < TRAIL_SIZE; i++) { gameView->trail[PLAYER_VAN_HELSING][i] = trail[i]; } calculateTrail(pastPlays, PLAYER_MINA_HARKER, trail); for (i = 0; i < TRAIL_SIZE; i++) { gameView->trail[PLAYER_MINA_HARKER][i] = trail[i]; } calculateTrail(pastPlays, PLAYER_DRACULA, trail); for (i = 0; i < TRAIL_SIZE; i++) { gameView->trail[PLAYER_DRACULA][i] = trail[i]; } gameView->map = newMap(); return gameView; }
//Get pose data from main computer and correct the data using odom and gyro void RosThread::poseListCallback(const ISLH_msgs::robotPositions::ConstPtr& msg) { // If its first data then reset time variables if(!firstDataCame) { current_timeG = ros::Time::now(); last_timeG = ros::Time::now(); current_timeO = ros::Time::now(); last_timeO = ros::Time::now(); } firstDataCame = true; //change pose datas of robots for(int i=0;i<msg->positions.size();i++){ bin[i+1][1] = msg->positions[i].x; bin[i+1][2] = msg->positions[i].y; bin[i+1][3] = robot.radius; } if(!poseFile.exists()) poseFile.open(QFile::WriteOnly); else poseFile.open(QFile::Append); QTextStream stream(&poseFile); stream<<QDateTime::currentDateTime().toTime_t()<<" "<<bin[robot.robotID][1]<<" "<<bin[robot.robotID][2]<<" "<<robot.radius<<" "; if(!turning){ if(!turning2){ //if robot is not turning then get angle from main computer(msg) radYaw = msg->directions[robot.robotID-1]; qDebug() << "MRad yaw: " << radYaw; stream << "M "; } turning2 = false; }else turning2 = true; double calYaw = atan2(vel[1],vel[0]); if(calYaw < 0) calYaw += M_PI*2; if(radYaw < 0) radYaw += M_PI*2; stream << radYaw << " " << calYaw << " " << (turning?"Y":"N") << "\n"; poseFile.close(); geometry_msgs::Twist twist; twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0; twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0; //if we reached to our target send targetReached msg and return from function double dist2Target = sqrt((robot.targetX-bin[robot.robotID][1])*(robot.targetX-bin[robot.robotID][1]) + (robot.targetY-bin[robot.robotID][2])*(robot.targetY-bin[robot.robotID][2])); if (dist2Target <= distanceThreshold){ if(!targetReached){ std_msgs::UInt8 _msg; _msg.data = 1; targetReachedPublisher.publish(_msg); targetReached = true; } } if(isFinished && dist2Target <= distanceThreshold){ /* if(!targetReached){ std_msgs::UInt8 _msg; _msg.data = 1; targetReachedPublisher.publish(_msg); targetReached = true; } */ velocityVector = twist; return; } qDebug() << "radYaw : " << radYaw << " calYaw : " << calYaw; qDebug() << "fabs(radYaw-calYaw)" << fabs(radYaw-calYaw); qDebug() << "angleThreshold*M_PI/180.0" << angleThreshold*M_PI/180.0; qDebug() << "turning : " << (turning?"true":"false"); double diff = fabs(radYaw - calYaw); if(diff > M_PI){ diff -= 2*M_PI; diff = fabs(diff); } //if difference between our angle and target angle is too big then stop and turn if((diff > angleThreshold*M_PI/180.0 && !turning) || (diff > angleThreshold/2*M_PI/180.0 && turning) ) { turning = true; calculateTurn(calYaw,radYaw,&twist); qDebug() << "Mbin[robot.RobotID][1] : " << bin[robot.robotID][1] << "[2] : " << bin[robot.robotID][2] << "[3] : " << bin[robot.robotID][3]; qDebug() << "bt[robot.RobotID][1] : " << bt[robot.robotID][1] << "[2] : " << bt[robot.robotID][2]; qDebug()<<"LinearVel:X:"<<twist.linear.x<<"Y:"<<twist.linear.y<<"Z:"<<twist.linear.z; qDebug()<<"AngularVel:X:"<<twist.angular.x<<"Y:"<<twist.angular.y<<"Z:"<<twist.angular.z; } else { turning = false; // if robot is not at target position then continue double dist2Target = sqrt((robot.targetX-bin[robot.robotID][1])*(robot.targetX-bin[robot.robotID][1]) + (robot.targetY-bin[robot.robotID][2])*(robot.targetY-bin[robot.robotID][2])); if(!isFinished || dist2Target > distanceThreshold){ twist.linear.x = linearVelocity; // if robots direction is not correct then fix it if((diff > angleThreshold/5*M_PI/180.0 && !turning)) calculateTurn(calYaw,radYaw,&twist); } // if robot is at target position then stop else twist.linear.x = 0; qDebug() << "Mbin[robot.RobotID][1] : " << bin[robot.robotID][1] << "[2] : " << bin[robot.robotID][2] << "[3] : " << bin[robot.robotID][3]; qDebug() << "bt[robot.RobotID][1] : " << bt[robot.robotID][1] << "[2] : " << bt[robot.robotID][2]; qDebug()<<"LinearVel:X:"<<twist.linear.x<<"Y:"<<twist.linear.y<<"Z:"<<twist.linear.z; qDebug()<<"AngularVel:X:"<<twist.angular.x<<"Y:"<<twist.angular.y<<"Z:"<<twist.angular.z; } velocityVector = twist; }
//Use gyro data to correct angle between two poseListCallback void RosThread::turtlebotGyroCallback(const sensor_msgs::Imu::ConstPtr& msg) { if(!firstDataCame) return; current_timeG = ros::Time::now(); qDebug() << "Gyro X:" << msg->angular_velocity.x << "Y:" << msg->angular_velocity.y << "Z:" << msg->angular_velocity.z; radYaw += msg->angular_velocity.z*((current_timeG - last_timeG).toSec()); last_timeG = current_timeG; double calYaw = atan2(vel[1],vel[0]); if(calYaw < 0) calYaw += M_PI*2; if(radYaw < 0) radYaw += M_PI*2; if(radYaw > M_PI*2) radYaw -= M_PI*2; geometry_msgs::Twist twist; twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0; twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0; //if we reached to our target send targetReached message and return from function double dist2Target = sqrt((robot.targetX-bin[robot.robotID][1])*(robot.targetX-bin[robot.robotID][1]) + (robot.targetY-bin[robot.robotID][2])*(robot.targetY-bin[robot.robotID][2])); if (dist2Target <= distanceThreshold){ if(!targetReached){ std_msgs::UInt8 msg; msg.data = 1; targetReachedPublisher.publish(msg); targetReached = true; } } if(isFinished && dist2Target <= distanceThreshold){ /* if(!targetReached){ std_msgs::UInt8 msg; msg.data = 1; targetReachedPublisher.publish(msg); targetReached = true; } */ velocityVector = twist; return; } if(!poseFile.exists()) poseFile.open(QFile::WriteOnly); else poseFile.open(QFile::Append); QTextStream stream(&poseFile); stream<<"G " << QDateTime::currentDateTime().toTime_t()<<" "<<bin[robot.robotID][1]<<" "<<bin[robot.robotID][2]<<" "<<robot.radius<<" "; stream << radYaw << "\n"; poseFile.close(); qDebug() << "radYaw : " << radYaw << " calYaw : " << calYaw; qDebug() << "fabs(radYaw-calYaw)" << fabs(radYaw-calYaw); qDebug() << "angleThreshold*M_PI/180.0" << angleThreshold*M_PI/180.0; qDebug() << "turning : " << (turning?"true":"false"); double diff = fabs(radYaw - calYaw); if(diff > M_PI){ diff -= 2*M_PI; diff = fabs(diff); } //if difference between our angle and target angle is too big then stop and turn if((diff > angleThreshold*M_PI/180.0 && !turning) || (diff > angleThreshold/2*M_PI/180.0 && turning)) { turning = true; calculateTurn(calYaw,radYaw,&twist); qDebug() << "bin[robot.RobotID][1] : " << bin[robot.robotID][1] << "[2] : " << bin[robot.robotID][2] << "[3] : " << bin[robot.robotID][3]; qDebug() << "bt[robot.RobotID][1] : " << bt[robot.robotID][1] << "[2] : " << bt[robot.robotID][2]; qDebug()<<"LinearVel:X:"<<twist.linear.x<<"Y:"<<twist.linear.y<<"Z:"<<twist.linear.z; qDebug()<<"AngularVel:X:"<<twist.angular.x<<"Y:"<<twist.angular.y<<"Z:"<<twist.angular.z; } else { turning = false; // if robot is not at target position then continue double dist2Target = sqrt((robot.targetX-bin[robot.robotID][1])*(robot.targetX-bin[robot.robotID][1]) + (robot.targetY-bin[robot.robotID][2])*(robot.targetY-bin[robot.robotID][2])); if(!isFinished || dist2Target > distanceThreshold){ twist.linear.x = linearVelocity; // if robots direction is not correct then fix it if((diff > angleThreshold/5*M_PI/180.0 && !turning)) calculateTurn(calYaw,radYaw,&twist); } // if robot is at target position then stop else twist.linear.x = 0; qDebug() << "bin[robot.RobotID][1] : " << bin[robot.robotID][1] << "[2] : " << bin[robot.robotID][2] << "[3] : " << bin[robot.robotID][3]; qDebug() << "bt[robot.RobotID][1] : " << bt[robot.robotID][1] << "[2] : " << bt[robot.robotID][2]; qDebug()<<"LinearVel:X:"<<twist.linear.x<<"Y:"<<twist.linear.y<<"Z:"<<twist.linear.z; qDebug()<<"AngularVel:X:"<<twist.angular.x<<"Y:"<<twist.angular.y<<"Z:"<<twist.angular.z; } velocityVector = twist; }
bool instructionToCommands(int startX, int startY, int degree, int endX, int endY, char**** commands) { printf("\nEntered instuctionToCommands! Parameters are:\n"); printf("startX: %d, startY: %d, degree: %d, endX: %d, endY: %d\n", startX, startY, degree, endX, endY); bool result = false; //Allocate the 2-D array of strings *commands = malloc(MAX_COMMANDS * sizeof(char**)); int m, n; for (m = 0; m < MAX_COMMANDS; m++) { (*commands)[m] = malloc(3 * sizeof(char*)); for (n = 0; n < 3; n++) { (*commands)[m][n] = malloc(INSTRUCTION_SIZE * sizeof(char)); strcpy((*commands)[m][n], "END0"); } } printf("MAX_COMMANDS: %d\n", MAX_COMMANDS); //Turning Phase int turningAngle = calculateTurn(startX, startY, endX, endY, degree); printf("The turn angle is: %d\n", turningAngle); //Convert -180 to 180 scale turn value to an absolute value + a direction int direction = 0; if (turningAngle > 0) direction = 1; turningAngle = abs(turningAngle); //Calculate how long the turning phase lasts int time = round((turningAngle/15) * INTERVAL_15); if (10 > time) time = 10; //Add instruction: Turn sensitivity MAX, speed 32, time milliseconds if (1 == direction) strcpy((*commands)[0][0], "81 3f"); //Right turn else strcpy((*commands)[0][0], "81 40"); //Left turn strcpy((*commands)[0][1], "82 1f"); sprintf((*commands)[0][2], "%d", time); //Linear Phase int distance = calculateDist(startX, startY, endX, endY); printf("Distance = %d\n", distance); //Add instruction: Turn sensitivity 0, SPEED, distance*10 milliseconds //We're assuming here that it takes 10 milliseconds to travel 1 unit (it probably doesn't) int trueDist = distance*10; //This shouldn't happen (and thus we'd probably need to recalibrate the speed, as its taking >10 seconds for one movement command) if (9999 < trueDist) trueDist = 9000; strcpy((*commands)[1][0], "81 00"); strcpy((*commands)[1][1], "82 1f"); sprintf((*commands)[1][2], "%d", trueDist % 9999); if (NULL != (*commands)) result = true; printf("Exiting instructionToCommands!\n"); return result; }
void MovementSystem::update(Entity& entity) { if(entity.hasComponent(Component::ComponentType::PacMove)) { std::shared_ptr<PacMoveCom> moveCom = std::dynamic_pointer_cast<PacMoveCom>(entity.getComponent(Component::ComponentType::PacMove)); std::shared_ptr<AnimationCom> animCom = std::dynamic_pointer_cast<AnimationCom>(entity.getComponent(Component::ComponentType::Animation)); switch(moveCom->state) { case PacMoveCom::State::Passive: if(searchForPlayer(entity)) { moveCom->state = PacMoveCom::State::Aggressive; animCom->currentState = 1; } else { moveCom->elapsedTime += deltaTime.asSeconds(); if(moveCom->collided || moveCom->elapsedTime > moveCom->travelTime) { calculateTurn(entity); moveCom->collided = false; moveCom->travelTime = (float)std::rand() / RAND_MAX * moveCom->randInterval + moveCom->randOffset; moveCom->elapsedTime = 0.0f; } break; } case PacMoveCom::State::Aggressive: if(moveCom->collided) { moveCom->state = PacMoveCom::State::Passive; calculateTurn(entity); moveCom->collided = false; animCom->currentState = 0; } break; } } else if(entity.hasComponent(Component::ComponentType::JimmyMove)) { std::shared_ptr<JimmyMoveCom> moveCom = std::dynamic_pointer_cast<JimmyMoveCom>(entity.getComponent(Component::ComponentType::JimmyMove)); std::shared_ptr<VelocityCom> velCom = std::dynamic_pointer_cast<VelocityCom>(entity.getComponent(Component::ComponentType::Velocity)); std::shared_ptr<AccelerationCom> accelCom = std::dynamic_pointer_cast<AccelerationCom>(entity.getComponent(Component::ComponentType::AccelDecel)); sf::Vector2f direction = moveCom->centerPosition - entity.position; float directionRad = std::atan(direction.y / direction.x); if(direction.x < 0.0f) directionRad += (float)M_PI; float randomRadAmount = 2.0f; directionRad += (float)std::rand() / (float)RAND_MAX * randomRadAmount - randomRadAmount / 2; float newDirX = std::cos(directionRad); float newDirY = std::sin(directionRad); accelCom->acceleration = sf::Vector2f(newDirX, newDirY) * moveCom->accelerationSpeed; } }
std::vector<Move> General::getDeployment() { if (map.isDirty()) calculateTurn(); return deployment; }
std::vector<Move> General::getAttack() { if (map.isDirty()) calculateTurn(); return attacks; }