Colony » Colony Scout » Colony Scout OS

set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp src/behaviors/sim_line.cpp src/behaviors/wl_test.cpp)

set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/WirelessReceiver.cpp src/EncodersControl.cpp src/LinesensorControl.cpp)

    wl_sender = new WirelessSender(node);

    wl_receiver = new WirelessReceiver(node);

#include "WirelessSender.h"

#include "WirelessReceiver.h"

        WirelessSender * wl_sender;

        WirelessReceiver * wl_receiver;

  behavior_list.push_back((Behavior*)new wl_test(scoutname));

#include "behaviors/wl_test.h"

static double last_ret;

/** 

 *  @brief Returns the average readings of the line following sensors. 

 */

double LinesensorControl::readline()

{

    unsigned int total_read = 0;

    unsigned int weighted_total = 0;

    vector<uint32_t> readings = query();

    for (int i = 0; i < 8; i++)

    {

        total_read += readings[i];

        weighted_total += i * readings[i];

    }

    if (total_read == 0)

    {

        return last_ret;

    }

    double ret_val = last_ret = ((double) weighted_total / total_read) - 4;

    return ret_val;

}

bool LinesensorControl::fullline()

{

    return false; 

}

void LinesensorControl::turnLeft()

{

    vector<uint32_t> readings = query();

}

void LinesensorControl::turnRight()

{

    vector<uint32_t> readings = query();

}

        /** Get line position */

        double readline();

        /** Implement turn functions */

        void turnLeft();

        void turnRight();

        /** Check if we are at an intersecetion */

        bool fullline();

 * @param libscout_node Node handle of the ROS node used by libscout.

 **/

WirelessReceiver::WirelessReceiver(const ros::NodeHandle& libscout_node)

    : node(libscout_node)

{

    sub = node.subscribe("/wireless/receive", QUEUE_SIZE,

          &WirelessReceiver::receive, this);

}

/**

 * @brief Registers a function to be called upon receipt of a packet.

 *

void WirelessReceiver::register_callback(WirelessCallback new_callback)

    callback = new_callback;

    if (callback == NULL)

    {

        ROS_WARN("Warning: Ignored wireless packet.");

    }

    else

    {

        (*callback)(packet->data);

    }

typedef void (*WirelessCallback)(std::vector<uint8_t>);

    public:

        /** Set up the class and prepare to communicate over ROS */

        WirelessReceiver(const ros::NodeHandle& libscout_node);

        /** Register a callback to be called every time a packet is received */

        void register_callback(WirelessCallback new_callback);

    private:

        ros::Subscriber sub;

        ros::NodeHandle node;

        WirelessCallback callback;

        void receive(const ::messages::WirelessPacketConstPtr&);

WirelessSender::WirelessSender(const ros::NodeHandle& libscout_node,

                               uint16_t type, uint16_t pan, uint16_t channel)

    : node(libscout_node)

    pub = node.advertise< ::messages::WirelessPacket>("/wireless/send",

                                                      QUEUE_SIZE);

void WirelessSender::send(uint8_t data[], int length, uint16_t dest)

    packet.dest = dest;

    pub.publish(packet);

void WirelessSender::send(vector<uint8_t>& data, uint16_t dest)

    packet.dest = dest;

 * Uses the dest field of the wireless packet to specify which

 * scout the message is intended for, or SCOUT_ALL to send to

 * all scouts

 *

/// A macro to allow sending to all the scouts.

#define SCOUT_ALL (messages::WirelessPacket::DEST_BROADCAST)

        WirelessSender(const ros::NodeHandle& libscout_node,

                uint16_t type = 0,

        void send(uint8_t data[], int length, uint16_t dest = SCOUT_ALL);

        void send(std::vector<uint8_t>& data, uint16_t dest = SCOUT_ALL);

        ros::NodeHandle node;

  Duration five(5);

  Duration three(3);

  Duration ten(10);

	Order a(1,0,1,t+ten,p,end);

  Order b(2,0,1,t+five,p,end);

  Order c(3,0,9,t+three,p,end);

  int best_index;

      best_index = i;

  Order ret;

  ret = unassignedOrders->remove(best_index);

  assignedOrders.push_back(ret);

  return ret;

    ROS_INFO("SCHEDULER: got get task message");

    spinOnce();

		while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())

    {

      spinOnce();

    }

    spinOnce();

  Path new_path = nav_map->shortest_path(curr_task->get_dest());

  curr_task->set_path(new_path);

  for(int i=0; i<new_path.len; i++)

  {

    Turn t = new_path.path[i];

    ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());

    nav_map->update_state(t);

    Duration time = nav_map->get_time_remaining();

    while(time.sec > 0)

      time = nav_map->get_time_remaining();

  }

  ROS_INFO("Path complete at state %d", (int)nav_map->get_state());

  if(error < 6) //Fail with 1/2 probability

        double line_loc = linesensor->readline();