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