/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <math.h>

#include "Behaviors.h"

/*
 * This function is code for random testing
 */

void BehaviorSystem::test_generic(FeatureSet *FS)
{
  logCallChain(CALL_test);
  /*
  switch(subRole) {
  case 0:
    command->motion_cmd = MOTION_GOAL_HAPPY1;
    command->head_cmd = HEAD_LOOKAT;
    command->head_lookat = vector3d(500.0,0.0,0.0);
    break;
  case 1:
    command->motion_cmd = MOTION_WALK_TROT;
    command->vx = MaxDX;
    command->vy = 0;
    command->va = 0;
    command->head_cmd = HEAD_LOOKAT;
    command->head_lookat = FS->ball_vector3d;
    break;
  case 2:
    command->motion_cmd = MOTION_WALK_TROT;
    command->vx = MaxDX;
    command->vy = 0;
    command->va = 0;
    command->head_cmd = HEAD_SCAN_BALL;
    break;
  }
  */

  if (sensor_data->button & HeadFrontButton)
    command->motion_cmd = MOTION_KICK_DIVE;
}