Colony

/**

 * @file PositionMonitor.cpp

 *

 * @author Jason Knichel

 *

 * @date 2/4/08

 */

//TODO: make this file asynchronous

#include <PositionMonitor.h>

#include <stdlib.h>

#include <vision.h>

#include <stdio.h>

#include <map>

using namespace std;

PositionMonitor::PositionMonitor() {

  //TODO: don't hardcode this file name

  //TODO: check for error returned from init

  vision_init("/var/www/colonet.jpg");

}

PositionMonitor::~PositionMonitor() {

}

int PositionMonitor::startMonitoring() {

  //TODO: fill this in when this becomes asynchronous

  return 0;

}

int PositionMonitor::stopMonitoring() {

  //TODO: fill this in when this becomes asynchronous

  return 0;

}

int PositionMonitor::updatePositions() {

  VisionPosition * positions = NULL;

  //TODO: check for error returned

  int numPositions = vision_get_robot_positions(&positions);

  //TODO: compare current set of positions to previous set of

  // positions to match up who is who

  positionMap.clear();

  int i;

  for (i = 0; i < numPositions; i++) {

    positionMap[i] = positions[i];

  }

  //TODO: remove this debug information

  map<int, VisionPosition>::iterator iter;

  for (iter = positionMap.begin(); iter != positionMap.end(); iter++) {

    printf("%d has position (%d, %d)\n", iter->first, iter->second.x, iter->second.y);

  }

  if (positions)

    free(positions);

  return 0;

}

map<int, VisionPosition> PositionMonitor::getAllRobotPositions() {

  return positionMap;

}

int PositionMonitor::getRobotPosition(int robot_id, int* xbuf, int* ybuf) {

  //TODO: figure out what a map returns if the element doesn't exist

  if (positionMap.find(robot_id) == positionMap.end()){

    return -1;

  }

  VisionPosition pos = positionMap[robot_id];

  *xbuf = pos.x;

  *ybuf = pos.y;

  return 0;

}