root / branches / encoders / code / behaviors / spline / slave / slave.c @ 1344
History | View | Annotate | Download (2.95 KB)
1 | 1190 | chihsiuh | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | |||
4 | #define GROUP 1 |
||
5 | 1223 | chihsiuh | #define MOTOR 0 |
6 | 1344 | ayeager | #define ENCODER 132 |
7 | 1190 | chihsiuh | |
8 | 1344 | ayeager | void packet_receive (char type, int source, char* packet, int length); |
9 | void ctrl(void); |
||
10 | 1217 | chihsiuh | |
11 | 1190 | chihsiuh | PacketGroupHandler packetHandler = {GROUP, NULL, NULL, &packet_receive, NULL}; |
12 | |||
13 | 1221 | chihsiuh | char buf[32]; |
14 | 1344 | ayeager | long unsigned int count = 0; |
15 | volatile int left_dir = FORWARD; |
||
16 | volatile int right_dir = FORWARD; |
||
17 | 1221 | chihsiuh | |
18 | 1344 | ayeager | volatile int left_ref; |
19 | volatile int right_ref; |
||
20 | |||
21 | volatile int left_old; |
||
22 | volatile int right_old; |
||
23 | |||
24 | volatile int left_i = 0; |
||
25 | volatile int right_i = 0; |
||
26 | |||
27 | volatile int left_u; |
||
28 | volatile int right_u; |
||
29 | |||
30 | volatile int left_vel = 0; |
||
31 | volatile int right_vel = 0; |
||
32 | |||
33 | #define PL 1 |
||
34 | #define DL 0.1 |
||
35 | #define IL 0.1 |
||
36 | |||
37 | #define PR 1.5 |
||
38 | #define DR 0.2 |
||
39 | #define IR 0.1 |
||
40 | |||
41 | |||
42 | #define INTERVAL 50 |
||
43 | |||
44 | 1217 | chihsiuh | /* receive motor speed from master:
|
45 | * left_speed left_dir right_speed right_dir
|
||
46 | * then sent raw encoder values back to master:
|
||
47 | * high_left low_left high_right low_right
|
||
48 | */
|
||
49 | 1344 | ayeager | void packet_receive (char type, int source, char* packet, int length) { |
50 | 1223 | chihsiuh | if (type != MOTOR) return; |
51 | 1344 | ayeager | |
52 | left_ref = packet[0];
|
||
53 | right_ref = packet[1];
|
||
54 | |||
55 | sprintf(buf, "received: %d %d\n", left_ref, right_ref);
|
||
56 | 1190 | chihsiuh | usb_puts(buf); |
57 | 1221 | chihsiuh | //orb_set_color(GREEN);
|
58 | 1190 | chihsiuh | } |
59 | |||
60 | 1344 | ayeager | void ctrl() {
|
61 | int left_err = left_ref - left_vel;
|
||
62 | int right_err = right_ref - right_vel;
|
||
63 | |||
64 | left_err = left_vel == 2048 ? left_old : left_err;
|
||
65 | right_err = right_vel == 2048 ? right_old : right_err;
|
||
66 | |||
67 | left_i += left_err; |
||
68 | left_u = (int)(PL*(float)left_err + DL*(float)(left_err - left_old) + IL*(float)left_i); |
||
69 | left_old = left_err; |
||
70 | |||
71 | right_i += right_err; |
||
72 | right_u = (int)(PR*(float)right_err + DR*(float)(right_err - right_old) + IR*(float)right_i); |
||
73 | right_old = right_err; |
||
74 | |||
75 | if (left_u < 0) { |
||
76 | left_dir = 0;
|
||
77 | left_u = -left_u; |
||
78 | } |
||
79 | else
|
||
80 | left_dir = 1;
|
||
81 | |||
82 | if (right_u < 0) { |
||
83 | right_u = -right_u; |
||
84 | right_dir = 0;
|
||
85 | } |
||
86 | else
|
||
87 | right_dir = 1;
|
||
88 | |||
89 | left_u += 160;
|
||
90 | right_u += 160;
|
||
91 | |||
92 | left_u = left_u > 210 ? 210 : left_u; |
||
93 | right_u = right_u > 210 ? 210 : right_u; |
||
94 | } |
||
95 | |||
96 | 1190 | chihsiuh | int main (void) { |
97 | 1344 | ayeager | char encoder[2]; |
98 | 1217 | chihsiuh | dragonfly_init(ALL_ON); |
99 | usb_init(); |
||
100 | encoders_init(); |
||
101 | //usb_puts("xbee id ");
|
||
102 | //usb_putc(wl_get_xbee_id());
|
||
103 | 1190 | chihsiuh | wl_init(); |
104 | 1344 | ayeager | left_ref = 20;
|
105 | right_ref = 20;
|
||
106 | 1190 | chihsiuh | wl_register_packet_group(&packetHandler); |
107 | 1221 | chihsiuh | //orb_set_color(RED);
|
108 | 1240 | chihsiuh | //motor1_set(FORWARD,230);
|
109 | //motor2_set(FORWARD,230);
|
||
110 | 1344 | ayeager | |
111 | 1217 | chihsiuh | while (1) { |
112 | 1344 | ayeager | usb_puts("We Win\n");
|
113 | //left_vel = encoder_get_v(0);
|
||
114 | //right_vel = encoder_get_v(1);
|
||
115 | delay_ms(1000);
|
||
116 | //if (!(count % INTERVAL)) {
|
||
117 | // wl_send_global_packet(GROUP, ENCODER, encoder, 2, 0);
|
||
118 | encoder[0] = left_vel;
|
||
119 | encoder[1] = right_vel;
|
||
120 | |||
121 | // usb_puti(left_vel);
|
||
122 | // sprintf(buf, "sent (%d): %d %d %d %d\n", count, left_vel, right_vel, encoder[0], encoder[1]);
|
||
123 | // usb_puts(buf);
|
||
124 | // usb_puti(encoder[0]);
|
||
125 | // }
|
||
126 | |||
127 | 1221 | chihsiuh | |
128 | 1344 | ayeager | //ctrl();
|
129 | |||
130 | // if (!(count % INTERVAL))
|
||
131 | // wl_do();
|
||
132 | |||
133 | // sprintf(buf, "motor (%d): %d %d %d %d\n", count, left_dir, left_ref, right_dir, right_ref);
|
||
134 | // usb_puts(buf);
|
||
135 | |||
136 | //motor1_set(left_dir, left_u);
|
||
137 | //motor2_set(right_dir, right_u);
|
||
138 | |||
139 | count++; |
||
140 | delay_ms(1000);
|
||
141 | 1217 | chihsiuh | } |
142 | 1190 | chihsiuh | } |