Statistics
| Revision:

root / trunk / code / projects / diagnostic_station / robot / main.c @ 1313

History | View | Annotate | Download (5.47 KB)

1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <xbee.h>
4

    
5
#include "../common/comm_station_robot.h"
6
#include "../../../lib/include/libdragonfly/encoders.h"
7

    
8

    
9
// **********************
10
// ** Command handlers **
11
// **********************
12

    
13
static void message_set_orbs (int length, uint8_t *data)
14
{
15
        if (length>=6)
16
        {
17
                orbs_set (data[0], data[1], data[2], data[3], data[4], data[5]);
18
        }
19
}
20

    
21
static void message_set_motors (int length, uint8_t *data)
22
{
23
        if (length>=4)
24
        {
25
                motor1_set (data[0], data[1]);
26
                motor2_set (data[2], data[3]);
27
        }
28
}
29

    
30
static void message_set_motors_time (int length, uint8_t *data)
31
{
32
        if (length>=6)
33
        {
34
                motor1_set (data[0], data[1]);
35
                motor2_set (data[2], data[3]);
36
                delay_ms (WORD(data[4], data[5]));
37
                motors_off ();
38
        }
39
}
40

    
41
static void message_set_motors_off (int length, uint8_t *data)
42
{
43
        motors_off ();
44
}
45

    
46
static void message_set_bom (int length, uint8_t *data)
47
{
48
        if (length>=2)
49
        {
50
                uint16_t bitmask=WORD(data[0], data[1]);
51
                
52
                bom_set_leds (bitmask);
53
                
54
                if (bitmask==0)
55
                        bom_off ();
56
                else
57
                        bom_on ();
58
        }
59
}
60

    
61
static void message_reset_encoders (int length, uint8_t *data)
62
{
63
        SYNC
64
        {
65
                encoder_rst_dx (LEFT);
66
                encoder_rst_dx (RIGHT);
67
        }
68
}
69

    
70
// **********************
71
// ** Request handlers **
72
// **********************
73

    
74
static void message_read_encoders (int length, uint8_t *data)
75
{
76
        int16_t left, right;
77
        
78
        SYNC
79
        {
80
                left = encoder_get_dx (LEFT);
81
                right = encoder_get_dx (RIGHT);
82
        }
83
        
84
        char senddata[4];
85
        senddata[0]=WORD_BYTE_0 (left);
86
        senddata[1]=WORD_BYTE_1 (left);
87
        senddata[2]=WORD_BYTE_0 (right);
88
        senddata[3]=WORD_BYTE_1 (right);
89

    
90
        wl_send_global_packet (robot_station_group, robot_station_data_encoders, senddata, 4, 0);
91
}
92

    
93
static void message_read_rangefinder (int length, uint8_t *data)
94
{
95
        int16_t value;
96
        int rangefinder_id = 0;
97
        
98
        switch(*data)
99
        {
100
                case 0: rangefinder_id = IR1; break;
101
                case 1: rangefinder_id = IR2; break;
102
                case 2: rangefinder_id = IR3; break;
103
                case 3: rangefinder_id = IR4; break;
104
                case 4: rangefinder_id = IR5; break;
105
        }
106
        
107
        value = range_read_distance(rangefinder_id);
108
        
109
        char senddata[2];
110
        senddata[0] = WORD_BYTE_0 (value);
111
        senddata[1] = WORD_BYTE_1 (value);
112
        
113
        wl_send_global_packet (robot_station_group, robot_station_data_rangefinder, senddata, 2, 0);
114

    
115
}
116

    
117
static void message_read_bom (int length, uint8_t *data){
118
        int16_t value;
119
        bom_refresh(BOM_ALL);
120
        value = bom_get((*data));
121
        char senddata[2];
122
        senddata[0] = WORD_BYTE_0 (value);
123
        senddata[1] = WORD_BYTE_1 (value);
124
        
125
        wl_send_global_packet (robot_station_group, robot_station_data_bom, senddata, 2, 0);
126
}
127

    
128
static void message_read_bom_all(int length, uint8_t *data){
129
        int16_t value;
130
        char senddata[32];
131

    
132
        bom_refresh(BOM_ALL);
133

    
134
        for(uint8_t i=0; i<16; ++i){
135
                value=bom_get(i);
136
                senddata[2*i  ] = WORD_BYTE_0 (value);
137
                senddata[2*i+1] = WORD_BYTE_1 (value);
138
        }
139
        
140
        wl_send_global_packet(robot_station_group, robot_station_data_bom_all, senddata, 32, 0);
141
}
142

    
143
static void message_read_eeprom(int length, uint8_t *data)
144
{
145
  char senddata[2];
146
  senddata[0] = get_robotid();
147
  senddata[1] = get_bom_type();
148
  
149
  wl_send_global_packet (robot_station_group, robot_station_data_eeprom, senddata, 2, 0);
150
}
151

    
152
// ***************************
153
// ** Message acknowledging **
154
// ***************************
155

    
156
static void send_done (void)
157
{
158
        wl_send_global_packet (robot_station_group, robot_station_done, NULL, 0, 0);
159
}
160

    
161
// ***********************
162
// ** Receive callbacks **
163
// ***********************
164

    
165
static void station_robot_receive (char type, int source, unsigned char* packet, int length)
166
{
167
        orb_set (0, 0, 255);
168

    
169
        usb_puti (type);
170
        usb_puts (" [");
171
        for (uint8_t i=0; i<length; ++i)
172
        {
173
                if (i!=0) usb_putc (' ');
174
                usb_puti (packet[i]);
175
        }
176
        usb_puts ("]\r\n");
177
        
178
        switch (type)
179
        {
180
                // Command messages
181
                case station_robot_set_orbs:        message_set_orbs        (length, packet); send_done (); break;
182
                case station_robot_set_motors:      message_set_motors      (length, packet); send_done (); break;
183
                case station_robot_set_bom:         message_set_bom         (length, packet); send_done (); break;
184
                case station_robot_set_motors_off:  message_set_motors_off  (length, packet); send_done (); break;
185
                case station_robot_set_motors_time: message_set_motors_time (length, packet); send_done (); break;
186
                case station_robot_reset_encoders:  message_reset_encoders  (length, packet); send_done (); break;
187
                
188
                // Request messages
189
                case station_robot_read_encoders:         message_read_encoders           (length, packet); break;
190
                case station_robot_read_rangefinder: message_read_rangefinder  (length, packet); break;
191
                case station_robot_read_bom:         message_read_bom                   (length, packet); break;
192
                case station_robot_read_bom_all:         message_read_bom_all           (length, packet); break;
193
    case station_robot_read_eeprom:     message_read_eeprom(length, packet); break;
194
        }
195
        
196
        orb_set (255, 127, 0);
197
}
198

    
199
// **********
200
// ** Main **
201
// **********
202

    
203
int main(void) {
204
    dragonfly_init(ALL_ON);
205
        encoders_init ();
206

    
207
        analog_init (ADC_START);
208
        //bom_init ();
209

    
210
    usb_init ();
211
    usb_puts ("Diagnostic station robot starting up\r\n");
212

    
213
        orb1_set (255, 127, 0);
214
        xbee_init ();
215
        wl_init();
216
//        xbee_set_channel(15);
217
        orb2_set (255, 127, 0);
218
        
219
        usb_puts ("PAN ID: ");  usb_puti (xbee_get_pan_id ());  usb_puts ("\r\n");        // -1
220
        usb_puts ("Channel: "); usb_puti (xbee_get_channel ()); usb_puts ("\r\n");        // 0
221
        usb_puts ("Address: "); usb_puti (xbee_get_address ()); usb_puts ("\r\n");        // 4/8
222
        
223
        PacketGroupHandler receive_handler = {station_robot_group, NULL, NULL, &station_robot_receive, NULL};
224
        wl_register_packet_group(&receive_handler);
225
        
226
        while (1)
227
        {
228
                wl_do();
229
        }
230
}