Colony

#include "dragonfly_lib.h"

#include "data_response.h"

#include "wl_token_ring.h"

#include "wireless.h"

#include "serial.h"

PacketGroupHandler pgh;

void handle_receive(char type, int source, unsigned char* packet, int length);

int generate_bom_packet(char** buf);

int generate_IR_packet(char** buf);

/** 

* @brief The only function that should be called by user code.

  Prepares the robot to receive and respond to data requests.

*/

void data_response_init(void){

  usb_init();

  usb_puts("USB initialized!\n\r");

  range_init();

  pgh.groupCode=DATA_REQUEST_GROUP; 

  pgh.timeout_handler = NULL;

  pgh.handle_response = NULL;

  pgh.handle_receive = handle_receive;

  pgh.unregister = NULL;

  wl_register_packet_group(&pgh);

  usb_puts("Packet Group Registered!\n\r");

}

/**

* Sorry for the lack of modularity, I didn't want to deal with memory issues for too long, plus it'll be faster!

*/

void handle_receive(char type, int source, unsigned char* packet, int length){

  char buf[MAX_PACKET_LENGTH]; 

  int size, i;

  usb_puts("Got a packet!\n\r");

  switch(type){

  case BOM_TYPE:

    short int i=0, robot_id; 

    wl_token_iterator_begin();

    while(wl_token_iterator_has_next() && i<=96){

      robot_id = wl_token_iterator_next();

      (*buf)[i++] = (char)((robot_id>>8) & 255);

      (*buf)[i++] = (char)(robot_id & 255);

      //TODO Ensure that wl_token_get.... returns the expected char. 

      (*buf)[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));

    }

    //i is one greater than the index of the last byte... the size.

    wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,BOM_TYPE,buf,i,source,0);

  case IR_TYPE:

    usb_puts("Generating an IR packet.\n");

    char ranges[] = {IR1,IR2,IR3,IR4,IR5}; 

    int i;

    short ir_value; 

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

      ir_value = (short)range_read_distance(ranges[i]);

      buf[2*i] = (char)((ir_value>>8)&255);   

      buf[2*i+1] = (ir_value&255);   

      usb_puts("Distance :\t");

      usb_puti(ir_value);

      usb_puts("\n");

    }

    size = 10;

    usb_puts("Packet Contents: ");

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

        usb_puti(buf[i]);

        usb_putc(' ');

    }

    wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,IR_TYPE,buf,size,source,0);

  case ENCODER_TYPE:

    break; 

    //Not yet supported.

  }

}

/** 

* @brief  Generates a BOM Data packet. **NO LONGER USED** Delete once this code is used often.

*/

int generate_bom_packet(char** buf){

  short int i=0, robot_id; 

  wl_token_iterator_begin();

  while(wl_token_iterator_has_next() && i<=96){

    robot_id = wl_token_iterator_next();

    (*buf)[i++] = (char)((robot_id>>8) & 0xFF);

    (*buf)[i++] = (char)(robot_id & 0xFF);

    (*buf)[i++] = (char)(wl_token_get_my_sensor_reading(robot_id));

  }

  //i is one greater than the index of the last byte... the size.

  return i;

}

/** 

* @brief Generates an IR Packet **NO LONGER USED** Delete once this code is used often.

* 

*/

int generate_IR_packet(char** buf){

  usb_puts("Generating an IR packet.\n");

  char ranges[] = {IR1,IR2,IR3,IR4,IR5}; 

  int i;

  short ir_value; 

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

    ir_value = (short)range_read_distance(ranges[i]);

    (*buf)[2*i] = (char)((ir_value>>8)&255);   

    (*buf)[2*i+1] = (ir_value&255);   

    usb_puts("Distance :\t");

    usb_puti(ir_value);

    usb_puts("\n");

  }

  return 10;

}

#ifndef _DATA_RESPONSE_H

#define _DATA_REQUESTS_H

#define DATA_REQUEST_GROUP 7

#define BOM_TYPE 0

#define IR_TYPE 1

#define ENCODER_TYPE 2

#define MAX_PACKET_LENGTH 100

/* This requires the wireless library

and the token ring to already be initialized.*/

void data_response_init(void);

#endif

#include "data_requests.h"

#include <stdlib.h>

#include <stdio.h>

#include <pthread.h>

#include "../../../libwireless/lib/wireless.h"

#include <string.h>

PacketGroupHandler pgh;

pthread_t* dr_thread;

void receive_handle(char type, int source, unsigned char* packet, int length);

void (*bom_handle)(BomNode** head);

void (*IR_handle)(short** data);

void (*encoder_handle)(unsigned char** data);

void data_requests_init(void(*bom_data_handler)(BomNode** head),

                        void(*IR_data_handler)(short** data),

                        void(*encoder_data_handler)(unsigned char** data)){

  bom_handle = bom_data_handler;

  IR_handle = IR_data_handler;

  encoder_handle = encoder_data_handler;

  pgh.groupCode = DATA_REQUEST_GROUP;

  pgh.timeout_handler = NULL;

  pgh.handle_response = NULL;

  pgh.handle_receive = receive_handle;

  pgh.unregister = NULL;

  wl_register_packet_group(&pgh);

}

void request_bom_data(int robot_id){

  wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,BOM_TYPE,NULL,0,robot_id,0);

}

void request_IR_data(int robot_id){

  wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,IR_TYPE,NULL,0,robot_id,0);

}

void request_encoder_data(int robot_id){

  wl_send_robot_to_robot_global_packet(DATA_REQUEST_GROUP,ENCODER_TYPE,NULL,0,robot_id,0);

}

/*void* pthread_handle(void* arg){

  RawData* data = (RawData*)arg;

  unsigned char* packet = data->packet;

  int length = data->length;

  char type = data->type;

  int i=0;

  BomNode* head;

  switch(type){

    case  BOM_TYPE:

      //BomNode* head;

      head = malloc(sizeof(BomNode));

      BomNode* iter = head;

      while(i<length){

        iter->robot_id = ((short)packet[i]<<8) + ((short)packet[i+1]); 

        i+=2;

        iter->value = packet[i++];

        if(i<length){

            iter-> next = malloc(sizeof(BomNode));

            iter = iter->next;

        }

      }

      bom_handle(head);

      break;

    case IR_TYPE:

      An IR packet consists of just the 5 bytes representing the IR

      values, in order.

      if(length != 10) break;

      short data[10];

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

        data[i] = ((short)packet[2*i]<<8)+((short)packet[2*i+1]);

      }

      IR_handle(data);

      break;

    case ENCODER_TYPE:

      /*I have no idea, unsupported.

      break;

  }

  free(data->packet);

  free(data);

  return NULL;

}*/

void receive_handle(char type, int source, unsigned char* packet, int length){

  int i=0;

  BomNode* head;

  printf("Packet contents: [ ");

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

    printf("%i ",packet[i]);

  }

  switch(type){

    case  BOM_TYPE:

      printf("I shouldn't be here during testing\n");

      head = malloc(sizeof(BomNode));

      BomNode* iter = head;

      while(i<length){

        iter->robot_id = ((short)packet[i]<<8) + ((short)packet[i+1]); 

        i+=2;

        iter->value = packet[i++];

        if(i<length){

            iter-> next = malloc(sizeof(BomNode));

            iter = iter->next;

        }

      }

      bom_handle(head);

      break;

    case IR_TYPE:

      /*An IR packet consists of just the 10 bytes representing the IR

      values as shorts, in order.*/

      if(length != 10) break;

      short* data = malloc(5*sizeof(short));

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

        data[i]=  ((((short)packet[2*i])<<8)&0xff00 ) | ( (short)(packet[2*i+1])&255);

      }

      IR_handle(&data);

      break;

    case ENCODER_TYPE:

      /*I have no idea, unsupported.*/

      break;

  }

}

/*void receive_handle(char type, int source, unsigned char* packet, int length){

  RawData* data = malloc(sizeof(RawData));

  if(!data){

    return;

  }

  unsigned char* newPacket = malloc(length);

  memcpy(newPacket, packet,length);

  data->packet=newPacket;

  data->source=source;

  data->length=length;

  data->type=type;

  dr_thread = malloc(sizeof(pthread_t));  

  pthread_create(dr_thread,NULL,pthread_handle,(void*)data);

}*/

#define DATA_REQUEST_GROUP 7

#define BOM_TYPE 0

#define IR_TYPE 1

#define ENCODER_TYPE 2

typedef struct {

  short int robot_id;

  char value;

  struct BomNode* next;

} BomNode;

typedef struct{

    unsigned char* packet;

    char type;

    int source;

    int length;

} RawData;

void data_requests_init(

                        void(*bom_data_handler)(BomNode** head),

                        void(*IR_data_handler)(short** data),

                        void(*encoder_data_handler)(unsigned char** data));

void request_bom_data(int robot_id);

void request_IR_data(int robot_id);

void request_encoder_data(int robot_id);