Revision 979
minor changes to remote control / mapping code...wireless control works with libwireless reverted to rev. 887. code in odometry interrupt conflicts with motors, not allowing the robot to move motors in response to commands.
| robot_main.c | ||
|---|---|---|
| 3 | 3 |
#include <encoders.h> |
| 4 | 4 |
#include "odometry.h" |
| 5 | 5 |
|
| 6 |
#define CHAN 0xE |
|
| 7 |
|
|
| 6 | 8 |
#define POLLING_INTERVAL 50 //interval in ms to get sensor data |
| 7 | 9 |
#define MAP 1 // packet group for sending data points |
| 8 | 10 |
#define POINT_ODO 2 |
| ... | ... | |
| 32 | 34 |
{
|
| 33 | 35 |
/* initialize components and join the token ring */ |
| 34 | 36 |
dragonfly_init(ALL_ON); |
| 35 |
odometry_init(); |
|
| 37 |
odometry_init(); // breaks motors (interrupt-related?)
|
|
| 36 | 38 |
orb_enable(); |
| 37 | 39 |
wl_init(); |
| 38 |
wl_set_channel(0xE);
|
|
| 40 |
wl_set_channel(CHAN);
|
|
| 39 | 41 |
wl_register_packet_group(&packetHandler); |
| 40 |
|
|
| 42 |
|
|
| 43 |
// initial control values |
|
| 41 | 44 |
velocity = 190; |
| 42 | 45 |
state = CNTL_STOP; |
| 43 |
|
|
| 44 |
int x, y; // use when sending odometry data |
|
| 46 |
|
|
| 47 |
// use when sending odometry data |
|
| 48 |
int x, y; |
|
| 45 | 49 |
double theta; |
| 46 | 50 |
|
| 47 | 51 |
int store[9]; |
| ... | ... | |
| 49 | 53 |
|
| 50 | 54 |
while(1) |
| 51 | 55 |
{
|
| 52 |
wl_do();
|
|
| 53 |
delay_ms(POLLING_INTERVAL); /* until it's time to grab map data */
|
|
| 54 |
x = odometry_dx(); // use when sending odometry data
|
|
| 55 |
y = odometry_dy();
|
|
| 56 |
theta = odometry_angle();
|
|
| 57 |
store[0] = x;
|
|
| 58 |
store[1] = y;
|
|
| 59 |
*((double*)(store + 2)) = theta;
|
|
| 60 |
store[4] = range_read_distance(IR1); // IR1 range
|
|
| 61 |
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 |
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);
|
|
| 56 |
wl_do();
|
|
| 57 |
delay_ms(POLLING_INTERVAL); // until it's time to grab map data
|
|
| 58 |
x = odometry_dx(); // use when sending odometry data
|
|
| 59 |
y = odometry_dy();
|
|
| 60 |
theta = odometry_angle();
|
|
| 61 |
store[0] = x;
|
|
| 62 |
store[1] = y;
|
|
| 63 |
*((double*)(store + 2)) = theta;
|
|
| 64 |
store[4] = range_read_distance(IR1); // IR1 range
|
|
| 65 |
store[5] = range_read_distance(IR2); // IR2 range
|
|
| 66 |
store[6] = range_read_distance(IR3); // IR3 range
|
|
| 67 |
store[7] = range_read_distance(IR4); // IR4 range
|
|
| 68 |
store[8] = range_read_distance(IR5); // IR5 range
|
|
| 69 |
sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
|
|
| 70 |
*((double *)(store+2)), store[4], store[5], store[6],
|
|
| 71 |
store[7], store[8] );
|
|
| 72 |
usb_puts(buf);
|
|
| 69 | 73 |
|
| 70 |
send = (char*) store;
|
|
| 74 |
send = (char*) store;
|
|
| 71 | 75 |
|
| 72 |
wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
|
|
| 76 |
wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
|
|
| 73 | 77 |
} |
| 74 | 78 |
|
| 75 | 79 |
wl_terminate(); |
Also available in: Unified diff