Revision 1217
spline slave
| slave.c | ||
|---|---|---|
| 4 | 4 |
#define GROUP 1 |
| 5 | 5 |
#define TYPE 0 |
| 6 | 6 |
|
| 7 |
#define LS 0 |
|
| 8 |
#define LD 1 |
|
| 9 |
#define RS 2 |
|
| 10 |
#define RD 3 |
|
| 11 |
|
|
| 12 |
#define H0 0 |
|
| 13 |
#define L0 1 |
|
| 14 |
#define H1 2 |
|
| 15 |
#define L1 3 |
|
| 16 |
|
|
| 7 | 17 |
void packet_receive (char type, int source, unsigned char* packet, int length); |
| 8 | 18 |
|
| 9 | 19 |
PacketGroupHandler packetHandler = {GROUP, NULL, NULL, &packet_receive, NULL};
|
| 10 | 20 |
|
| 21 |
/* receive motor speed from master: |
|
| 22 |
* left_speed left_dir right_speed right_dir |
|
| 23 |
* then sent raw encoder values back to master: |
|
| 24 |
* high_left low_left high_right low_right |
|
| 25 |
*/ |
|
| 11 | 26 |
void packet_receive (char type, int source, unsigned char* packet, int length) {
|
| 12 |
int lv, ls, rv, rs;
|
|
| 27 |
orb_set_color(WHITE);
|
|
| 13 | 28 |
char buf[32]; |
| 14 | 29 |
sprintf(buf, "%d %d %d %d\n", packet[0], packet[1], packet[2], packet[3]); |
| 15 | 30 |
usb_puts(buf); |
| 16 |
lv = packet[0]; |
|
| 17 |
ls = packet[1]; |
|
| 18 |
rv = packet[2]; |
|
| 19 |
rs = packet[3]; |
|
| 20 |
motor1_set(ls,lv); |
|
| 21 |
motor2_set(rs,rv); |
|
| 31 |
motor1_set(packet[LD],packet[LS]); |
|
| 32 |
motor2_set(packet[RD],packet[RS]); |
|
| 33 |
|
|
| 34 |
int encoder_left = encoder_read(LEFT); |
|
| 35 |
int encoder_right = encoder_read(RIGHT); |
|
| 36 |
sprintf(buf, "%d %d\n", encoder_left, encoder_right); |
|
| 37 |
usb_puts(buf); |
|
| 38 |
unsigned char encoder[4]; |
|
| 39 |
encoder[H0] = (encoder_left >> 8) & 0xFF; |
|
| 40 |
encoder[L0] = (encoder_left) & 0xFF; |
|
| 41 |
encoder[H1] = (encoder_right >> 8) & 0xFF; |
|
| 42 |
encoder[L1] = (encoder_right) & 0xFF; |
|
| 43 |
wl_send_global_packet(GROUP, TYPE, encoder, 4, 0); |
|
| 44 |
orb_set_color(GREEN); |
|
| 22 | 45 |
} |
| 23 | 46 |
|
| 24 | 47 |
int main (void) {
|
| 25 |
dragonfly_init(SERIAL | USB | COMM | ORB | ANALOG | MOTORS | BUZZER); |
|
| 26 |
ADCSRA = _BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0); |
|
| 27 |
usb_puts("xbee id ");
|
|
| 28 |
usb_putc(wl_get_xbee_id()); |
|
| 48 |
dragonfly_init(ALL_ON); |
|
| 49 |
usb_init(); |
|
| 50 |
encoders_init(); |
|
| 51 |
//usb_puts("xbee id ");
|
|
| 52 |
//usb_putc(wl_get_xbee_id()); |
|
| 29 | 53 |
wl_init(); |
| 30 |
|
|
| 54 |
|
|
| 31 | 55 |
wl_register_packet_group(&packetHandler); |
| 32 |
orb_set_color(PURPLE); |
|
| 33 |
|
|
| 34 |
while (1) {
|
|
| 35 |
wl_do(); |
|
| 36 |
} |
|
| 37 |
|
|
| 56 |
orb_set_color(RED); |
|
| 57 |
|
|
| 58 |
while (1) {
|
|
| 59 |
wl_do(); |
|
| 60 |
} |
|
| 38 | 61 |
} |
| 39 | 62 |
|
Also available in: Unified diff