Revision 1221
spline slave
| slave.c | ||
|---|---|---|
| 18 | 18 |
|
| 19 | 19 |
PacketGroupHandler packetHandler = {GROUP, NULL, NULL, &packet_receive, NULL};
|
| 20 | 20 |
|
| 21 |
char buf[32]; |
|
| 22 |
int count = 0; |
|
| 23 |
|
|
| 21 | 24 |
/* receive motor speed from master: |
| 22 | 25 |
* left_speed left_dir right_speed right_dir |
| 23 | 26 |
* then sent raw encoder values back to master: |
| 24 | 27 |
* high_left low_left high_right low_right |
| 25 | 28 |
*/ |
| 26 | 29 |
void packet_receive (char type, int source, unsigned char* packet, int length) {
|
| 27 |
orb_set_color(WHITE); |
|
| 28 |
char buf[32];
|
|
| 29 |
sprintf(buf, "%d %d %d %d\n", packet[0], packet[1], packet[2], packet[3]);
|
|
| 30 |
//orb_set_color(WHITE);
|
|
| 31 |
usb_puts("receiving\n");
|
|
| 32 |
sprintf(buf, "%d %d %d %d\n", packet[LS], packet[LD], packet[RS], packet[RD]);
|
|
| 30 | 33 |
usb_puts(buf); |
| 31 | 34 |
motor1_set(packet[LD],packet[LS]); |
| 32 | 35 |
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); |
|
| 36 |
//orb_set_color(GREEN); |
|
| 45 | 37 |
} |
| 46 | 38 |
|
| 47 | 39 |
int main (void) {
|
| ... | ... | |
| 53 | 45 |
wl_init(); |
| 54 | 46 |
|
| 55 | 47 |
wl_register_packet_group(&packetHandler); |
| 56 |
orb_set_color(RED); |
|
| 48 |
//orb_set_color(RED);
|
|
| 57 | 49 |
|
| 58 | 50 |
while (1) {
|
| 59 | 51 |
wl_do(); |
| 52 |
|
|
| 53 |
int encoder_left = encoder_read(LEFT); |
|
| 54 |
int encoder_right = encoder_read(RIGHT); |
|
| 55 |
sprintf(buf, "%d %d\n", encoder_left, encoder_right); |
|
| 56 |
usb_puts(buf); |
|
| 57 |
unsigned char encoder[4]; |
|
| 58 |
encoder[H0] = (encoder_left >> 8) & 0xFF; |
|
| 59 |
encoder[L0] = (encoder_left) & 0xFF; |
|
| 60 |
encoder[H1] = (encoder_right >> 8) & 0xFF; |
|
| 61 |
encoder[L1] = (encoder_right) & 0xFF; |
|
| 62 |
wl_send_global_packet(GROUP, TYPE, encoder, 4, 0); |
|
| 63 |
usb_puts("Sent ");
|
|
| 64 |
usb_puti(count++); |
|
| 65 |
usb_puts("\n");
|
|
| 66 |
|
|
| 67 |
delay_ms(200); |
|
| 60 | 68 |
} |
| 61 | 69 |
} |
| 62 | 70 |
|
Also available in: Unified diff