root / trunk / code / projects / mapping / robot / robot_main.c @ 965
History | View | Annotate | Download (3.92 KB)
1 | 898 | alevkoy | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | 912 | alevkoy | #include <encoders.h> |
4 | 915 | cmar | #include "odometry.h" |
5 | 722 | justin | |
6 | 917 | cmar | #define POLLING_INTERVAL 50 //interval in ms to get sensor data |
7 | 898 | alevkoy | #define MAP 1 // packet group for sending data points |
8 | 965 | alevkoy | #define POINT_ODO 2 |
9 | 898 | alevkoy | |
10 | 900 | cmar | #define WL_CNTL_GROUP 4 |
11 | |||
12 | 912 | alevkoy | #define CNTL_FORWARD 0 |
13 | #define CNTL_BACK 1 |
||
14 | #define CNTL_LEFT 2 |
||
15 | #define CNTL_RIGHT 3 |
||
16 | #define CNTL_LEFT_CURVE 4 |
||
17 | #define CNTL_RIGHT_CURVE 5 |
||
18 | #define CNTL_STOP 6 |
||
19 | 915 | cmar | #define CNTL_VEL_UP 7 |
20 | #define CNTL_VEL_DOWN 8 |
||
21 | 900 | cmar | |
22 | int state;
|
||
23 | int velocity;
|
||
24 | char buf[100]; |
||
25 | |||
26 | void packet_receive (char type, int source, unsigned char* packet, int length); |
||
27 | void set_motion (void); |
||
28 | |||
29 | PacketGroupHandler packetHandler = {WL_CNTL_GROUP, NULL, NULL, &packet_receive, NULL}; |
||
30 | 899 | alevkoy | |
31 | 898 | alevkoy | int main(void) |
32 | { |
||
33 | /* initialize components and join the token ring */
|
||
34 | 899 | alevkoy | dragonfly_init(ALL_ON); |
35 | 915 | cmar | odometry_init(); |
36 | 965 | alevkoy | orb_enable(); |
37 | 899 | alevkoy | wl_init(); |
38 | 925 | cmar | wl_set_channel(0xE);
|
39 | 900 | cmar | wl_register_packet_group(&packetHandler); |
40 | 965 | alevkoy | |
41 | 925 | cmar | velocity = 190;
|
42 | 912 | alevkoy | state = CNTL_STOP; |
43 | 900 | cmar | |
44 | 915 | cmar | int x, y; // use when sending odometry data |
45 | double theta;
|
||
46 | 898 | alevkoy | |
47 | 915 | cmar | int store[9]; |
48 | 899 | alevkoy | char *send; // for transmission |
49 | 898 | alevkoy | |
50 | while(1) |
||
51 | { |
||
52 | 900 | cmar | wl_do(); |
53 | 898 | alevkoy | delay_ms(POLLING_INTERVAL); /* until it's time to grab map data */
|
54 | 915 | cmar | x = odometry_dx(); // use when sending odometry data
|
55 | y = odometry_dy(); |
||
56 | theta = odometry_angle(); |
||
57 | store[0] = x;
|
||
58 | 965 | alevkoy | store[1] = y;
|
59 | *((double*)(store + 2)) = theta; |
||
60 | store[4] = range_read_distance(IR1); // IR1 range |
||
61 | 915 | cmar | store[5] = range_read_distance(IR2); // IR2 range |
62 | store[6] = range_read_distance(IR3); // IR3 range |
||
63 | store[7] = range_read_distance(IR4); // IR4 range |
||
64 | store[8] = range_read_distance(IR5); // IR5 range |
||
65 | 965 | alevkoy | sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1], |
66 | *((double *)(store+2)), store[4], store[5], store[6], |
||
67 | store[7], store[8] ); |
||
68 | usb_puts(buf); |
||
69 | 898 | alevkoy | |
70 | 899 | alevkoy | send = (char*) store;
|
71 | 898 | alevkoy | |
72 | 915 | cmar | wl_send_global_packet(MAP, POINT_ODO, send, 18, 0); |
73 | 898 | alevkoy | } |
74 | |||
75 | 899 | alevkoy | wl_terminate(); |
76 | 898 | alevkoy | return 0; |
77 | 722 | justin | } |
78 | 898 | alevkoy | |
79 | 900 | cmar | void packet_receive (char type, int source, unsigned char* packet, int length) { |
80 | sprintf(buf, "received packet: %d\n", type);
|
||
81 | usb_puts(buf); |
||
82 | switch (type) {
|
||
83 | 912 | alevkoy | case CNTL_FORWARD:
|
84 | state = CNTL_FORWARD; |
||
85 | 900 | cmar | break;
|
86 | 912 | alevkoy | case CNTL_BACK:
|
87 | state = CNTL_BACK; |
||
88 | 900 | cmar | break;
|
89 | 912 | alevkoy | case CNTL_LEFT:
|
90 | state = CNTL_LEFT; |
||
91 | 900 | cmar | break;
|
92 | 912 | alevkoy | case CNTL_RIGHT:
|
93 | state = CNTL_RIGHT; |
||
94 | 900 | cmar | break;
|
95 | 912 | alevkoy | case CNTL_LEFT_CURVE:
|
96 | state = CNTL_LEFT_CURVE; |
||
97 | 900 | cmar | break;
|
98 | 912 | alevkoy | case CNTL_RIGHT_CURVE:
|
99 | state = CNTL_RIGHT_CURVE; |
||
100 | 900 | cmar | break;
|
101 | 912 | alevkoy | case CNTL_STOP:
|
102 | state = CNTL_STOP; |
||
103 | 900 | cmar | break;
|
104 | 915 | cmar | case CNTL_VEL_UP:
|
105 | 900 | cmar | if (velocity > 250) |
106 | velocity = 250;
|
||
107 | else
|
||
108 | velocity += 5;
|
||
109 | break;
|
||
110 | 915 | cmar | case CNTL_VEL_DOWN:
|
111 | 900 | cmar | velocity -= 5;
|
112 | if (velocity < 150) |
||
113 | velocity = 150;
|
||
114 | break;
|
||
115 | default:
|
||
116 | 912 | alevkoy | state = CNTL_STOP; |
117 | 900 | cmar | break;
|
118 | } |
||
119 | set_motion(); |
||
120 | } |
||
121 | |||
122 | void set_motion (void) { |
||
123 | switch (state) {
|
||
124 | 912 | alevkoy | case CNTL_FORWARD:
|
125 | 900 | cmar | orb_set_color(GREEN); |
126 | move(velocity, 0);
|
||
127 | break;
|
||
128 | 912 | alevkoy | case CNTL_BACK:
|
129 | 900 | cmar | orb_set_color(BLUE); |
130 | move(-velocity, 0);
|
||
131 | break;
|
||
132 | 912 | alevkoy | case CNTL_LEFT:
|
133 | 900 | cmar | orb_set_color(ORANGE); |
134 | motor1_set(0, velocity);
|
||
135 | motor2_set(1, velocity);
|
||
136 | break;
|
||
137 | 912 | alevkoy | case CNTL_RIGHT:
|
138 | 900 | cmar | orb_set_color(YELLOW); |
139 | motor1_set(1, velocity);
|
||
140 | motor2_set(0, velocity);
|
||
141 | break;
|
||
142 | 912 | alevkoy | case CNTL_LEFT_CURVE:
|
143 | 900 | cmar | orb_set_color(MAGENTA); |
144 | motor1_set(1, 150); |
||
145 | motor2_set(1, velocity);
|
||
146 | break;
|
||
147 | 912 | alevkoy | case CNTL_RIGHT_CURVE:
|
148 | 900 | cmar | orb_set_color(CYAN); |
149 | motor1_set(1, velocity);
|
||
150 | motor2_set(1, 150); |
||
151 | break;
|
||
152 | 912 | alevkoy | case CNTL_STOP:
|
153 | 900 | cmar | orb_set_color(RED); |
154 | move(0, 0); |
||
155 | break;
|
||
156 | } |
||
157 | } |