root / trunk / code / projects / colonet / robot / joystick / keyboard_slave / slave.c @ 1390
History | View | Annotate | Download (2.9 KB)
1 | 896 | cmar | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | #include "../../../common/colonet_defs.h" |
||
4 | |||
5 | #define WL_CNTL_GROUP 4 |
||
6 | |||
7 | #define FORWARD 0 |
||
8 | #define BACK 1 |
||
9 | #define LEFT 2 |
||
10 | #define RIGHT 3 |
||
11 | #define LEFT_CURVE 4 |
||
12 | #define RIGHT_CURVE 5 |
||
13 | #define STOP 6 |
||
14 | #define VEL_UP 7 |
||
15 | #define VEL_DOWN 8 |
||
16 | |||
17 | int state;
|
||
18 | int velocity;
|
||
19 | char buf[100]; |
||
20 | |||
21 | void packet_receive (char type, int source, unsigned char* packet, int length); |
||
22 | void set_motion (void); |
||
23 | |||
24 | PacketGroupHandler packetHandler = {WL_CNTL_GROUP, NULL, NULL, &packet_receive, NULL}; |
||
25 | |||
26 | int main (void) { |
||
27 | dragonfly_init(ALL_ON); |
||
28 | wl_init(); |
||
29 | wl_set_channel(0xF);
|
||
30 | wl_register_packet_group(&packetHandler); |
||
31 | orb_enable(); |
||
32 | //orb_disable();
|
||
33 | //orb_set(255,255,255);
|
||
34 | orb_set_color(PURPLE); |
||
35 | //delay_ms(1000);
|
||
36 | //while(1);
|
||
37 | |||
38 | int xb_addr = xbee_get_address();
|
||
39 | |||
40 | sprintf(buf, "beginning: xbee addr=%d\n", xb_addr);
|
||
41 | usb_puts(buf); |
||
42 | |||
43 | velocity = 160;
|
||
44 | state = STOP; |
||
45 | while (1) { |
||
46 | wl_do(); |
||
47 | } |
||
48 | |||
49 | return 0; |
||
50 | } |
||
51 | |||
52 | void packet_receive (char type, int source, unsigned char* packet, int length) { |
||
53 | sprintf(buf, "received packet: %d\n", type);
|
||
54 | usb_puts(buf); |
||
55 | switch (type) {
|
||
56 | case FORWARD:
|
||
57 | state = FORWARD; |
||
58 | break;
|
||
59 | case BACK:
|
||
60 | state = BACK; |
||
61 | break;
|
||
62 | case LEFT:
|
||
63 | state = LEFT; |
||
64 | break;
|
||
65 | case RIGHT:
|
||
66 | state = RIGHT; |
||
67 | break;
|
||
68 | case LEFT_CURVE:
|
||
69 | state = LEFT_CURVE; |
||
70 | break;
|
||
71 | case RIGHT_CURVE:
|
||
72 | state = RIGHT_CURVE; |
||
73 | break;
|
||
74 | case STOP:
|
||
75 | state = STOP; |
||
76 | break;
|
||
77 | case VEL_UP:
|
||
78 | if (velocity > 250) |
||
79 | velocity = 250;
|
||
80 | else
|
||
81 | velocity += 5;
|
||
82 | break;
|
||
83 | case VEL_DOWN:
|
||
84 | velocity -= 5;
|
||
85 | if (velocity < 150) |
||
86 | velocity = 150;
|
||
87 | break;
|
||
88 | default:
|
||
89 | state = STOP; |
||
90 | break;
|
||
91 | } |
||
92 | set_motion(); |
||
93 | } |
||
94 | |||
95 | void set_motion (void) { |
||
96 | switch (state) {
|
||
97 | case FORWARD:
|
||
98 | orb_set_color(GREEN); |
||
99 | move(velocity, 0);
|
||
100 | break;
|
||
101 | case BACK:
|
||
102 | orb_set_color(BLUE); |
||
103 | move(-velocity, 0);
|
||
104 | break;
|
||
105 | case LEFT:
|
||
106 | orb_set_color(ORANGE); |
||
107 | motor1_set(0, velocity);
|
||
108 | motor2_set(1, velocity);
|
||
109 | break;
|
||
110 | case RIGHT:
|
||
111 | orb_set_color(YELLOW); |
||
112 | motor1_set(1, velocity);
|
||
113 | motor2_set(0, velocity);
|
||
114 | break;
|
||
115 | case LEFT_CURVE:
|
||
116 | orb_set_color(MAGENTA); |
||
117 | motor1_set(1, 150); |
||
118 | motor2_set(1, velocity);
|
||
119 | break;
|
||
120 | case RIGHT_CURVE:
|
||
121 | orb_set_color(CYAN); |
||
122 | motor1_set(1, velocity);
|
||
123 | motor2_set(1, 150); |
||
124 | break;
|
||
125 | case STOP:
|
||
126 | orb_set_color(RED); |
||
127 | move(0, 0); |
||
128 | break;
|
||
129 | } |
||
130 | } |