Revision 1219
Added reset_encoders command
Fixed bugs
| trunk/code/projects/diagnostic_station/station/comm_robot.c | ||
|---|---|---|
| 156 | 156 |
send_command_packet (station_robot_set_bom, data, 2); |
| 157 | 157 |
} |
| 158 | 158 |
|
| 159 |
void robot_reset_encoders (void) |
|
| 160 |
{
|
|
| 161 |
send_command_packet (station_robot_reset_encoders, NULL, 0); |
|
| 162 |
} |
|
| 159 | 163 |
|
| 164 |
|
|
| 160 | 165 |
// ************************* |
| 161 | 166 |
// ** Individual requests ** |
| 162 | 167 |
// ************************* |
| trunk/code/projects/diagnostic_station/station/test_encoders.c | ||
|---|---|---|
| 50 | 50 |
int16_t *data_robot_l =malloc (num_measurements*sizeof (uint16_t)); |
| 51 | 51 |
int16_t *data_robot_r =malloc (num_measurements*sizeof (uint16_t)); |
| 52 | 52 |
- |
| 53 |
usb_puts ("# Resetting encoders");
|
|
| 53 | 54 |
encoders_reset (); |
| 55 |
robot_reset_encoders (); |
|
| 54 | 56 |
|
| 55 | 57 |
for (uint8_t m=0; m<num_measurements; ++m) |
| 56 | 58 |
{
|
| ... | ... | |
| 67 | 68 |
// TODO instead of waiting a fixed time, wait until the wheels |
| 68 | 69 |
// are not moving any more, using the station's encoders. |
| 69 | 70 |
|
| 71 |
// Read the station encoders |
|
| 70 | 72 |
encoders_read (&(data_station_l[m]), &(data_station_r[m])); |
| 71 |
//TODO robot_read_encoders () |
|
| 72 |
data_robot_l[m]=23;
|
|
| 73 |
data_robot_r[m]=42;
|
|
| 73 |
|
|
| 74 |
// Read the robot encoders
|
|
| 75 |
robot_read_encoders (&(data_robot_l[m]), &(data_robot_r[m]));
|
|
| 74 | 76 |
} |
| 75 | 77 |
|
| 76 | 78 |
send_encoder_data (0, direction1, data_station_l, data_robot_l); |
| trunk/code/projects/diagnostic_station/station/comm_robot.h | ||
|---|---|---|
| 11 | 11 |
void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2); |
| 12 | 12 |
void robot_set_motors_off (void); |
| 13 | 13 |
void robot_set_bom (uint16_t bitmask); |
| 14 |
void robot_reset_encoders (void); |
|
| 14 | 15 |
|
| 15 | 16 |
bool robot_read_encoders (int16_t *left, int16_t *right); |
| 16 | 17 |
uint16_t robot_read_bom (uint8_t num); |
| trunk/code/projects/diagnostic_station/robot/main.c | ||
|---|---|---|
| 58 | 58 |
} |
| 59 | 59 |
} |
| 60 | 60 |
|
| 61 |
static void message_reset_encoders (int length, uint8_t *data) |
|
| 62 |
{
|
|
| 63 |
SYNC |
|
| 64 |
{
|
|
| 65 |
encoder_rst_dx (LEFT); |
|
| 66 |
encoder_rst_dx (RIGHT); |
|
| 67 |
} |
|
| 68 |
} |
|
| 61 | 69 |
|
| 62 | 70 |
// ********************** |
| 63 | 71 |
// ** Request handlers ** |
| ... | ... | |
| 69 | 77 |
|
| 70 | 78 |
SYNC |
| 71 | 79 |
{
|
| 72 |
left = encoder_read (LEFT);
|
|
| 73 |
right = encoder_read (RIGHT);
|
|
| 80 |
left = encoder_get_dx (LEFT);
|
|
| 81 |
right = encoder_get_dx (RIGHT);
|
|
| 74 | 82 |
} |
| 75 | 83 |
|
| 76 | 84 |
char senddata[4]; |
| ... | ... | |
| 93 | 101 |
// TODO why does it keep receiving 0 messages? |
| 94 | 102 |
//if (type==0) return; |
| 95 | 103 |
|
| 96 |
// Dump the raw packet |
|
| 97 |
usb_puts ("Recvd [");
|
|
| 98 |
for (uint8_t i=0; i<length; ++i) |
|
| 99 |
{
|
|
| 100 |
if (i!=0) usb_putc (' ');
|
|
| 101 |
usb_puti (packet[i]); |
|
| 102 |
} |
|
| 103 |
usb_puts ("]\r\n");
|
|
| 104 |
|
|
| 105 |
|
|
| 106 |
|
|
| 107 | 104 |
orb_set (0, 0, 255); |
| 108 | 105 |
|
| 109 | 106 |
usb_puts ("Received a message: ");
|
| ... | ... | |
| 118 | 115 |
case station_robot_set_bom: message_set_bom (length, packet); break; |
| 119 | 116 |
case station_robot_set_motors_off: message_set_motors_off (length, packet); break; |
| 120 | 117 |
case station_robot_set_motors_time: message_set_motors_time (length, packet); break; |
| 118 |
case station_robot_reset_encoders: message_reset_encoders (length, packet); break; |
|
| 121 | 119 |
|
| 122 | 120 |
// Request messages |
| 123 | 121 |
case station_robot_read_encoders: message_send_encoders (length, packet); break; |
| ... | ... | |
| 131 | 129 |
// ********** |
| 132 | 130 |
|
| 133 | 131 |
int main(void) {
|
| 134 |
dragonfly_init(0);
|
|
| 132 |
dragonfly_init(ALL_ON); // TODO initialize only required components
|
|
| 135 | 133 |
|
| 136 | 134 |
encoders_init (); |
| 137 | 135 |
|
| trunk/code/projects/diagnostic_station/common/comm_station_robot.h | ||
|---|---|---|
| 16 | 16 |
#define station_robot_set_motors_off 4 |
| 17 | 17 |
#define station_robot_set_motors_time 5 |
| 18 | 18 |
#define station_robot_read_encoders 6 |
| 19 |
#define station_robot_reset_encoders 7 |
|
| 19 | 20 |
|
| 20 | 21 |
#define robot_station_group 2 |
| 21 | 22 |
|
Also available in: Unified diff