ZRGameInternal.cpp

Go to the documentation of this file.

//ZRHS2014
#include <string.h>
#include <math.h>

#include "ZRGame.h"
#include "ZRGameInternal.h"
#include "ZR_API_internal.h"
#include "ZR_API.h"
#include "Constants.h"

//SPHERES C includes
extern "C" {
#include "commands.h"
#include "system.h"
#include "math_matrix.h"
#include "gsp.h"
#include "control.h"
#include "find_state_error.h"
#include "housekeeping_internal.h"
}

/**
 * This file is used to define the C++ implementation of both the public and the
 * private game API functions.  Public functions will be prefixed with ZeroRoboticsGame::
 * while private implementation functions will be prefixed with ZeroRoboticsGameImpl::
 */

 /**
  * Use these vectors to choose the satellites state when the user is given control.
  */
#if SPHERE_ID==SPHERE1
state_vector initStateZR = {0.4,-0.6,0, 0,0,0, 0.7071,-0.7071,0,0, 0,0,0};
#else
state_vector initStateZR = {-0.4,-0.6,0, 0,0,0, 0.7071,-0.7071,0,0, 0,0,0};
#endif

//======= Singleton Instance of Game and Implementation=====//
ZeroRoboticsGameImpl &ZeroRoboticsGameImpl::instance() {
  ///Singleton instance of the game implementation
  static ZeroRoboticsGameImpl gameImplInstance(ZeroRoboticsAPIImpl::instance());
  return gameImplInstance;
}
ZeroRoboticsGame &ZeroRoboticsGame::instance() {
  static ZeroRoboticsGame gameInstance(ZeroRoboticsGameImpl::instance(), ZeroRoboticsAPIImpl::instance());
  return gameInstance;
}

//======== Constructors ========//

//Constructor for ZeroRobotics game with initializers for the ZR API.  Do not modify.
ZeroRoboticsGame::ZeroRoboticsGame(ZeroRoboticsGameImpl &impl, ZeroRoboticsAPIImpl &apiImpl): pimpl(impl), apiImpl(apiImpl)
{
  impl.game = this;
}

ZeroRoboticsGameImpl::ZeroRoboticsGameImpl(ZeroRoboticsAPIImpl &apiImpl) : apiImpl(apiImpl) {}


//Internal accessor for game implementation
ZeroRoboticsGameImpl *getGameImpl()
{
  return &ZeroRoboticsGameImpl::instance();
}

//======== GAME IMPLEMENTATION METHODS ========//


//Add methods here for the public game

void ZeroRoboticsGameImpl::init()
{
  /*  Game Initialization setup.
  * Then set the locations of the memory packs.
  */

  apiImpl.gameTime = GAME_TIME;
  memset(&challInfo,0,sizeof(ChallengeInfo));   // general clear of challInfo to 0 = false = empty

  // update the state data on challInfo
  updateStates();
  challInfo.me.score = START_SCORE;

  // Lights, Camera, Action!
  initLight();
  initCamera();
  initEnergy();

  initItems();



  // send information to ground and sat2 at the end of init
  sendDebug();

}

void ZeroRoboticsGameImpl::updateStates() {
  apiImpl.api->getMyZRState(challInfo.me.zrState);
  apiImpl.api->getOtherZRState(challInfo.other.zrState);

  apiImpl.api->getMySphState(challInfo.me.sphState);
  apiImpl.api->getOtherSphState(challInfo.other.sphState);

  challInfo.currentTime = apiImpl.api->getTime();

  challInfo.camera.tookPicture = false;
  challInfo.camera.uploadedPictures = false;
}

bool ZeroRoboticsGameImpl::update(float forceTorqueOut[6]) {
  updateStates();
  updateLight();
  updateEnergy();

  // empty the userForces array
  memset(challInfo.me.userForces,0,sizeof(float)*6);

  // declare a static finalResult variable
  // updateGameOverManuever will change it
  static unsigned char finalResult = 0;

  unsigned char useForces = false;

  // maneuver 3 = game mode
  // maneuver 202 = end game (get final score)
  // maneuver 204 = terminate

  switch (ctrlManeuverNumGet())
  {

    case 3:
      useForces = updateGameModeManeuver(forceTorqueOut);
      break;

    //------End main game code------//

    case 202: // Endgame scenario
      useForces = updateGameOverManeuver(forceTorqueOut, finalResult);
      break;

    case 203: // terminate test after determining final score
      {
        ctrlTestTerminate(finalResult);
      }
      break;
  }

  //Store user forces for sending to ground
  memcpy(challInfo.me.userForces, forceTorqueOut, sizeof(float)*6);

#ifdef ZR2D
  memset(&challInfo.me.userForces[2], 0, sizeof(float)*3);
#endif

  //Send Telemetry
  sendDebug();

  return useForces;

}

#ifdef ZR2D
static float pintegralX = 0.0f, pintegralY = 0.0f, pintegralZ = 0.0f;
#endif

void ZeroRoboticsGameImpl::modify2DForceTorque(float forceTorque[6])
{
  state_vector myState, targetState;
  apiImpl.api->getMySphState(myState);
  memcpy(targetState, myState, sizeof(state_vector));   
  
  targetState[POS_Z] = 0.0f;
  targetState[VEL_Z] = 0.0f;
  targetState[10] = 0.0f;
  targetState[11] = 0.0f;
  targetState[12] = 0.0f;

  float q_sw[4], q_tw[4];
  float axis[3] = {0.0f, 0.0f, -1.0f};
  swingTwistDecomposition(&targetState[QUAT_1], q_sw, q_tw, axis);
  quatToPositive(q_tw);
  memcpy(&targetState[QUAT_1], q_tw, 4*sizeof(float));
  
  state_vector error;
  findStateError(error, myState, targetState);

  float newForces[6] = {0};
  
  ctrlPositionPD(KPpositionPD, KDpositionPD, error, newForces);
  ctrlAttitudeNLPIDwie(KPattitudePID, KIattitudePID, KDattitudePID, 
             KPattitudePID, KIattitudePID, KDattitudePID, 
             KPattitudePID, KIattitudePID, KDattitudePID, 
             1.0f, error, newForces);
  forceTorque[FORCE_Z] = newForces[FORCE_Z];
  forceTorque[TORQUE_X] = newForces[TORQUE_X];
  forceTorque[TORQUE_Y] = newForces[TORQUE_Y];
}

// Decomposes the quaternion into swing and twist rotation quaternions.
// More info http://www.alinenormoyle.com/weblog/?p=726.
void swingTwistDecomposition(float q[4], float q_sw[4],
  float q_tw[4], float tw_axis[3]) {
  
  // p = dot(q[0:2], tw_axis) * tw_axis
  // q_tw = [p[0], p[1], p[2], q[3]]
  float c = mathVecInner(q, tw_axis, 3);
  for (int i = 0; i < 3; i++) {
    q_tw[i]= tw_axis[i] * c;    
  }
  q_tw[3] = q[3];
  mathVecNormalize(q_tw, 4);
  
  // invert twist quaternion
  float q_tw_inv[4];
  memcpy(q_tw_inv, q_tw, 4*sizeof(float));  
  for (int i = 0; i < 3; i++) {
    q_tw_inv[i] *= -1.0f;
  }
    
  quatMult(q_sw, q_tw_inv, q);
}

void quatToPositive(float quat[4]) {
    if (quat[3] < 0) {
      for (int i = 0; i < 4; i++) {
        quat[4] *= -1;
      }
    }
}

bool ZeroRoboticsGameImpl::updateGameModeManeuver(float forceTorqueOut[6]) {
  float collisionInfo[2];

  // get state-of-health of other satellite (mainly for end of test)
  comm_payload_soh soh_partner;
  #if (SPHERE_ID == SPHERE1)
    commBackgroundSOHGet(SPHERE2, &soh_partner);
  #else
    commBackgroundSOHGet(SPHERE1, &soh_partner);
  #endif

  // determine current speed of satellite, use to determine collision avoidance motion
  float speed = mathVecMagnitude(&challInfo.me.sphState[3],3);

  unsigned char useForces = false;

  // if camera was used (so it can't be used again for 3 seconds), check when to enable it again
  if((!challInfo.camera.cameraOn) && ((challInfo.currentTime - challInfo.camera.cameraOffTime) > DISABLE_CAMERA_TIME))
  {
    activateCamera();
  }

  // Run user's "loop" function.  apiImpl is used because we are accessing
  // a private part of the API containing a pointer to the user's API
  apiImpl.zruser->loop();

  //Check if items are being picked up
  for (int i = 0; i < NUM_ITEMS; i++)
    itemPickUp(i);

  useForces = useForces || apiImpl.getForceTorque(forceTorqueOut);
  
  prop_time tent_times;
  #ifdef ZRFLATFLOOR
    ctrlMixWLoc(&tent_times, forceTorqueOut, challInfo.me.sphState, 10, 40.0f, FORCE_FRAME_INERTIAL);
  #else
    ctrlMixWLoc(&tent_times, forceTorqueOut, challInfo.me.sphState, 10, 20.0f, FORCE_FRAME_INERTIAL);
  #endif

  float tent_time_sum = 0;
  for (int i = 0; i < 12; i++) {
    tent_time_sum += tent_times.off_time[i]-tent_times.on_time[i];
  }

  //Check for fuel or energy out at this point
  if((game->getFuelRemaining() <= 0.0f || game->getEnergy() <= tent_time_sum * ENERGY_COST_THRUSTERS))
  {
    memset(forceTorqueOut,0,6*sizeof(float));   // anything the user has commanded is ignored, only firings would be from out-of-bounds or collission avoidance
    useForces = false;  // assume there will be no firings
  }

  //Enforce boundaries of the game
  if (enforceBoundaries(forceTorqueOut))
  {
    //challInfo.me.fuelUsed += OFFSIDES_PENALTY;
    GAME_TRACE(("[%d] out of bounds penalty | ", apiImpl.api->getTime()));
    useForces = true;
  }

  //Collision avoidance
  //check to see that its speed is not neglible.
  if (speed > 0.01f) {
    challInfo.me.collisionActive = ctrlAvoidWithOverride(challInfo.me.sphState,&challInfo.other.sphState,1,forceTorqueOut,collisionInfo,3);
  }

  //------Thruster control------//
    // Run standard mixer to get fuel consumption
  prop_time times;
  ctrlMixWLoc(&times, forceTorqueOut, challInfo.me.sphState, 10, 20.0f, FORCE_FRAME_INERTIAL);

  //Tally fuel and energy used for firing thrusters
  for (int i=0; i<12; i++)
  {
    float thrusterTime = times.off_time[i]-times.on_time[i];

    challInfo.me.fuelUsed += thrusterTime / 1000.0f;

    tryToUseEnergy(thrusterTime * ENERGY_COST_THRUSTERS);


    //Protect against NAN or INF hacking
    if (challInfo.me.fuelUsed < 0.0f)
    {
      DEBUG(("WARNING: invalid forces/torques applied to satellite at t=%d. Fuel will be set to 0. Check your code for errors.\n", challInfo.currentTime));
      challInfo.me.fuelUsed = PROP_ALLOWED_SECONDS;
    }
  }
  
  #ifdef ZR2D
    modify2DForceTorque(forceTorqueOut);
    useForces = true;
  #endif

  //------Game end conditions------//
  if (challInfo.currentTime >= (MAX_GAME_TIME))
    ctrlManeuverNumSet(202);

  return useForces;
}

bool ZeroRoboticsGameImpl::updateGameOverManeuver(float forceTorqueOut[6], unsigned char& finalResult) {
  unsigned char useForces = false;
  int result;

  float collisionInfo[2];

  // get state-of-health of other satellite (mainly for end of test)
  comm_payload_soh soh_partner;
  #if (SPHERE_ID == SPHERE1)
    commBackgroundSOHGet(SPHERE2, &soh_partner);
  #else
    commBackgroundSOHGet(SPHERE1, &soh_partner);
  #endif

  float myScore;
  float otherScore;

  myScore = game->getScore();
  otherScore = game->getOtherScore();

  useForces = ctrlAvoidWithOverride(challInfo.me.sphState, &challInfo.other.sphState, 1, forceTorqueOut, collisionInfo, 3) > 0;
  useForces = useForces || enforceBoundaries(forceTorqueOut);

  // break ties by making a tiny change based on the random state
  if (myScore == otherScore)
  {
    challInfo.me.score  -= (0.01f * sqrtf((challInfo.me.zrState[0]*challInfo.me.zrState[0])
      +(challInfo.me.zrState[1]*challInfo.me.zrState[1])+(challInfo.me.zrState[2]*challInfo.me.zrState[2])));
    GAME_TRACE (("Tie Breaker (%12.10f = %12.10f), new score = %12.10f | ", myScore, otherScore, challInfo.me.score));
  }

  // get final score
  if ((soh_partner.last_test_result || (soh_partner.maneuver_number > 200) || (NUM_SPHERES == 1)))
  {
    result = 10*floorf(myScore);
    if (result < 10) result = 10;
    if (result > 220) result = 220;
    if (myScore > otherScore)
      result += 10;
    finalResult = (result + apiImpl.getTeamId() + 1);
    useForces = false;
  }

  // terminate after we find the final result & timeout (4s)
  if (finalResult && (ctrlManeuverTimeGet() > 4000))
  {
    GAME_TRACE (("GAME ENDED! Final Score Float: %10.8f / Integer: %d | Test Result %d | ", myScore, result, finalResult));
    ctrlManeuverTerminate();
  }
  return useForces;
}

bool ZeroRoboticsGameImpl::enforceBoundaries(float forceTorqueOut[6])
{
  state_vector sphState;
  apiImpl.api->getMySphState(sphState);
  bool retValue = false;

  //Run velocity controller to stop motion out of volume
  //Zero out any controls that are leading the user out of the volume
  for (int i=0; i<3; i++)
  {
    if (sphState[POS_X+i] > limits[i])
    {
      limitDirection(sphState, forceTorqueOut, i, 1.0f);
      retValue = true;
    }
    else if (sphState[POS_X+i] < -limits[i])
    {
      limitDirection(sphState, forceTorqueOut, i, -1.0f);
      retValue = true;
    }
  }
  return retValue;
}

void ZeroRoboticsGameImpl::limitDirection(state_vector ctrlState, float ctrlControl[6], unsigned int idx, float dir)
{
  // if command is to move out further in same direction as limit
  if (ctrlControl[idx]*dir >= 0.0f)
  {
    ctrlControl[idx] = -1.0f * OOBgain * ctrlState[VEL_X+idx];  // slow down to get 0 velocity (ignore position)
  }
}

/******************Collision Avoidance Functions************************/
void ZeroRoboticsGameImpl::resolveCollision(float zrState[12], float forceTorqueOut[6])
{
  for (int i=0; i<3; i++)
  {
    if (zrState[i] > 0)
      limitDirection(zrState, forceTorqueOut, i, -1.0f);
    else if (zrState[i] < 0)
      limitDirection(zrState, forceTorqueOut, i, 1.0f);
  }
}

float ZeroRoboticsGameImpl::dist3d(float* pos1, float* pos2) const
{
  float tmpVec[3];
  mathVecSubtract(tmpVec,static_cast<float*>(pos1),static_cast<float*>(pos2),3);
  return mathVecMagnitude(tmpVec,3);
}



/*************************Score functions***********************/
float ZeroRoboticsGame::getScore()
{
  return pimpl.challInfo.me.score;
}

float ZeroRoboticsGame::getOtherScore()
{
  return pimpl.challInfo.other.score;
}

//======== PUBLIC API METHODS ========//
//------General Functions------//

float ZeroRoboticsGame::getFuelRemaining()
{
  ZeroRoboticsGameImpl::ChallengeInfo& challInfo = pimpl.challInfo;
  float total = PROP_ALLOWED_SECONDS-challInfo.me.fuelUsed;
  total = MIN_FUEL(total, 0.0f); //~Prevents fuel from being less than 0.0f
  total = MAX_FUEL(total, PROP_ALLOWED_SECONDS); //~Prevents fuel from being greater than MAX fuel in tank
  return total;
}

int ZeroRoboticsGame::getCurrentTime()
{
  return pimpl.challInfo.currentTime;
}

void ZeroRoboticsGame::sendMessage(unsigned char inputMsg)
{
  pimpl.challInfo.me.message = (short)inputMsg;
}

unsigned char ZeroRoboticsGame::receiveMessage()
{
  return (unsigned char)pimpl.challInfo.other.message;
}