Project

General

Profile

Revision 71

Worked on computer_main and robot_main,
Somewhat completed robot_main. Will need to clean up packet creation.
computer_main is nowhere near complete. Working on pulling in data from the robots.

View differences:

trunk/code/projects/slam/.dep/robot_main.o.d
1
robot_main.o: robot_main.c \
2
  ../../../code/lib/include/libdragonfly/dragonfly_lib.h \
3
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/inttypes.h \
4
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdint.h \
5
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdio.h \
6
  /opt/local/lib/gcc/avr/4.0.2/include/stdarg.h \
7
  /opt/local/lib/gcc/avr/4.0.2/include/stddef.h \
8
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdlib.h \
9
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/io.h \
10
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/sfr_defs.h \
11
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/iom128.h \
12
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/portpins.h \
13
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/version.h \
14
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/interrupt.h \
15
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/util/delay.h \
16
  /opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/util/twi.h \
17
  ../../../code/lib/include/libdragonfly/analog.h \
18
  ../../../code/lib/include/libdragonfly/dio.h \
19
  ../../../code/lib/include/libdragonfly/time.h \
20
  ../../../code/lib/include/libdragonfly/lcd.h \
21
  ../../../code/lib/include/libdragonfly/lights.h \
22
  ../../../code/lib/include/libdragonfly/motor.h \
23
  ../../../code/lib/include/libdragonfly/serial.h \
24
  ../../../code/lib/include/libdragonfly/buzzer.h \
25
  ../../../code/lib/include/libdragonfly/rangefinder.h \
26
  ../../../code/lib/include/libdragonfly/bom.h \
27
  ../../../code/lib/include/libdragonfly/move.h \
28
  ../../../code/lib/include/libwireless/wireless.h \
29
  ../../../code/lib/include/libwireless/wl_token_ring.h \
30
  smart_run_around_fsm.h
31

  
32
../../../code/lib/include/libdragonfly/dragonfly_lib.h:
33

  
34
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/inttypes.h:
35

  
36
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdint.h:
37

  
38
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdio.h:
39

  
40
/opt/local/lib/gcc/avr/4.0.2/include/stdarg.h:
41

  
42
/opt/local/lib/gcc/avr/4.0.2/include/stddef.h:
43

  
44
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdlib.h:
45

  
46
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/io.h:
47

  
48
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/sfr_defs.h:
49

  
50
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/iom128.h:
51

  
52
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/portpins.h:
53

  
54
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/version.h:
55

  
56
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/interrupt.h:
57

  
58
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/util/delay.h:
59

  
60
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/util/twi.h:
61

  
62
../../../code/lib/include/libdragonfly/analog.h:
63

  
64
../../../code/lib/include/libdragonfly/dio.h:
65

  
66
../../../code/lib/include/libdragonfly/time.h:
67

  
68
../../../code/lib/include/libdragonfly/lcd.h:
69

  
70
../../../code/lib/include/libdragonfly/lights.h:
71

  
72
../../../code/lib/include/libdragonfly/motor.h:
73

  
74
../../../code/lib/include/libdragonfly/serial.h:
75

  
76
../../../code/lib/include/libdragonfly/buzzer.h:
77

  
78
../../../code/lib/include/libdragonfly/rangefinder.h:
79

  
80
../../../code/lib/include/libdragonfly/bom.h:
81

  
82
../../../code/lib/include/libdragonfly/move.h:
83

  
84
../../../code/lib/include/libwireless/wireless.h:
85

  
86
../../../code/lib/include/libwireless/wl_token_ring.h:
87

  
88
smart_run_around_fsm.h:
trunk/code/projects/slam/computer_main.c
1
#include <dragonfly_lib.h>
1
#include <stdio.h>
2
#include <wireless.h>
3
#include <unistd.h>
2 4

  
5
#define SLAM_PACKET_GROUP 11
6
#define CONTROL_PACKET_TYPE 'C'
7
#define REQUEST_PACKET_TYPE 'R'
8
#define ROBOT_RESPONSE 'I'
9

  
10
#define MAX_ROBOTS 50 
11

  
12
void handle_timeout(void);
13
void handle_response(int frame, int received);
14
void handle_receive(char type, int source, unsigned char* packet, int length);
15
void unregister(void);
16
void RobotData* getRobot(int id);
17

  
18
typedef struct bom_node{
19
  char value;
20
  short int id;
21
  BomNode* next;
22
} BomNode;
23

  
24
//If at some point we have more than 50 robots, this will fail.
25
typedef struct robot_data{
26
  int id; 
27
  unsigned char IR[5]; 
28
  RobotData* next;
29
  BomNode* bom_head;
30
} RobotData;
31

  
32
RobotData* head_bot;
33
RobotData* current_bot;
34

  
3 35
int main(void)
4 36
{
5
	dragonfly_init(ALL_ON);
37
  PacketGroupHandler* pgh = malloc(sizeof(PacketGroupHandler)); 
38
  pgh->groupCode = SLAM_PACKET_GROUP;
39
  pgh->timeout_handler = handle_timeout;
40
  pgh->handle_response = handle_response;
41
  pgh->handle_receive = handle_receive;
42
  pgh->unregister = unregister;
43

  
44
  
45
  wl_init();
46
  wl_register_packet_group(pgh);
47
  wl_token_ring_register();
48
  wl_token_ring_join();
49
  wl_token_ring_set_bom_functions(NULL,NULL,NULL);
50

  
51
  while(1){
52
    wl_do();
53
    wl_token_iterator_begin() 
54
    
55
    //Iterate through the robots in the token ring. 
56
    while(wl_token_iterator_has_next()){
57
      wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP,REQUEST_PACKET_TYPE,
58
        NULL,0,wl_token_iterator_next(),0);   
59
      usleep(50000); 
60
    }
61
  }
6 62
	return 0;
7 63
}
8 64

  
65
void handle_timeout(void){}
66
void handle_response(int frame, int received){}
67

  
68
void handle_receive(char type, int source, unsigned char* packet, int length){
69
  if(type == ROBOT_RESPONSE){
70
    int i; 
71
    current_bot = getRobot(source);
72
    if(current_bot == NULL){
73
      //Add the new robot to the front of the list.
74
      current_bot = malloc(sizeof(RobotData));
75
      current_bot->next = head_bot;
76
      head_bot = current_bot;
77
    }
78
    for(i=0;i<5;i++)
79
      current_bot->IR[i] = packet[i];
80
    i++; //i=5 
81
   
82
    //Bom Handling.
83
    //Clear the current information.
84
    BomNode* head = current_bot->bom_head;
85
    if(head!=NULL){
86
      BomNode* next = head-next;
87
      while(next!=NULL){
88
        free(head);
89
        head = next;
90
        next = head->next;
91
      }
92
      free(head);
93
    }
94
    //All of the memory allocated for the BomList is free.
95

  
96
    //The remainder of the packet is bom data. 
97
    //TODO Finish this shit.
98
    BomNode* bom_node = malloc(sizeof(BomNode));
99
    current_bot->bom_head = bom_node;
100
    while(i<length){
101
      bom_node->id = (short int)packet[i];
102
      i+=2;
103
      bom_node->value = packet[i];
104
      i++;
105
    }
106
  }
107
}
108

  
109
RobotData* getRobot(int id){
110
  RobotData* next = head_bot; 
111
  while(next->id != id && next->next!=NULL);
112
  
113
  if(next->id != id) return NULL;
114
  
115
  return next;
116
}
117

  
trunk/code/projects/slam/robot_main.c
27 27
void receive_handle(char type, int source, unsigned char* packet, int length);
28 28
void unregister(void);
29 29

  
30
char* make_request_packet(char* packet);
30
int  make_request_packet(char* packet);
31 31

  
32 32
int IR[5];
33 33

  
......
72 72
    bom_off();
73 73
}
74 74

  
75
//These can probably be NULL..
76

  
75 77
void timeout_handle(void){
76
    wl_token_ring_join();
78

  
77 79
}
78 80

  
79 81
void response_handle(int frame, int received){
......
83 85
void receive_handle(char type, int source, unsigned char* packet, int length){
84 86
  int i; 
85 87
  if(type==REQUEST_PACKET_TYPE){
86
    char packet[REQUEST_PACKET_SIZE]; 
87 88
    //Send a message back to our computer overlord.
88
    int len= make_request_packet(packet);
89
   
89
    char buf[PACKET_BUFFER_SIZE]; 
90
    int len = make_request_packet(&buf);
91
    char packet[len];
92
    for(i=0; i< len ; i++){
93
      packet[i]=buf[i];
94
    } 
95
    
90 96
    wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP, ROBOT_RESPONSE, packet, len, source , 0);
91 97
  }
92 98
  else if(type == CONTROL_PACKET_TYPE){
93 99
    for(i=0;i<length;i++){
94

  
100
      
95 101
    }
96 102
  }
97 103
}
98 104

  
99
int make_request_packet(char* packet){
100
  int size = wl_token_get_num_robots();
105

  
106
/**
107

  
108
Packets are structured {IR1, IR2, IR3, IR4, IR5,
109
BomID1, ... , BomValue1, BomID2, ... , BomValue2, etc.} 
110
Total size is 5 + 3*numRobots. So Xbee should support
111
up to 31 robots before we need to restructure the 
112
communication between the computer and the robot.
113

  
114
*/
115
int make_request_packet(char** packet){
101 116
  int i,j;
102 117
  for(i=0;i<5;i++){
103
      packet[i] = IR[i];
118
      (*packet)[i] = IR[i];
104 119
  } 
105
  for(i=0;i<size;i++){
106
      for(j=0;j<size;j++){
107
       // wl_token_get_sensor_reading(int source, int dest);
108
      
109
      }
120
  i=5; 
121
  wl_token_iterator_begin();
122
  int id = wl_get_xbee_id();  
123
  
124
  while(wl_token_iterator_has_next()){
125
    int robot_in_view = wl_token_iterator_next(); 
126
    
127
    //Robot int's are two bytes. 
128
    (*packet)[i] = robot_in_view; 
129
    i+=2;
130
    
131
    int bom_number = wl_token_get_sensor_reading(id,robot_in_view);
132
    //Bom reading of 255 denotes an unseen node. 
133
    if(bom_number<0) bom_number = 255; 
134
    bom_number %= 256;
135
    char bom_value = (char) bom_number;
136
    (*packet)[i] = bom_value;
137
    i++;
110 138
  }
111
  return size;
139
  //Size is one bom byte towards every robot, and 5 IR bytes. 
140
  return wl_token_get_num_robots()+5;
112 141
}
113 142

  
trunk/code/projects/slam/Makefile
3 3
#
4 4

  
5 5
# Relative path to the root directory (containing lib directory)
6
#ifndef COLONYROOT
6
ifndef COLONYROOT
7 7
COLONYROOT = ../../..
8
#endif
8
endif
9 9

  
10 10
# Target file name (without extension).
11 11
TARGET = robot_main 

Also available in: Unified diff