Revision 1215
Implemented encoders test
| test_encoders.c | ||
|---|---|---|
| 2 | 2 |
#include "global.h" |
| 3 | 3 |
#include "comm_robot.h" |
| 4 | 4 |
|
| 5 |
#include "hardware.h" |
|
| 6 |
|
|
| 7 |
// TODO as parameters |
|
| 5 | 8 |
#define num_measurements 4 |
| 6 | 9 |
#define velocity 200 |
| 7 | 10 |
#define on_delay 500 |
| 8 | 11 |
#define off_delay 800 |
| 9 | 12 |
|
| 13 |
// ****************** |
|
| 14 |
// ** Data sending ** |
|
| 15 |
// ****************** |
|
| 16 |
|
|
| 17 |
static void send_encoder_data (uint8_t num, uint8_t direction, |
|
| 18 |
int16_t *data_station, int16_t *data_robot) |
|
| 19 |
{
|
|
| 20 |
if (direction!=motor_direction_off) |
|
| 21 |
{
|
|
| 22 |
usb_puts ("data encoder ");
|
|
| 23 |
usb_puti (num); |
|
| 24 |
usb_puts (" ");
|
|
| 25 |
usb_puts (motor_direction_string (direction)); |
|
| 26 |
usb_puts (" ");
|
|
| 27 |
|
|
| 28 |
for (uint8_t i=0; i<num_measurements; ++i) |
|
| 29 |
{
|
|
| 30 |
usb_putc (' ');
|
|
| 31 |
usb_puti (data_station[i]); |
|
| 32 |
usb_putc ('/');
|
|
| 33 |
usb_puti (data_robot[i]); |
|
| 34 |
} |
|
| 35 |
|
|
| 36 |
usb_puts (NL); |
|
| 37 |
} |
|
| 38 |
} |
|
| 39 |
|
|
| 40 |
|
|
| 41 |
// ************************ |
|
| 42 |
// ** Internal functions ** |
|
| 43 |
// ************************ |
|
| 44 |
|
|
| 10 | 45 |
static void test_encoders_direction (uint8_t direction1, uint8_t direction2) |
| 11 | 46 |
{
|
| 12 |
// reset_encoders (); |
|
| 47 |
// Allocate space for the data on the stack |
|
| 48 |
int16_t *data_station_l =malloc (num_measurements*sizeof (uint16_t)); |
|
| 49 |
int16_t *data_station_r =malloc (num_measurements*sizeof (uint16_t)); |
|
| 50 |
int16_t *data_robot_l =malloc (num_measurements*sizeof (uint16_t)); |
|
| 51 |
int16_t *data_robot_r =malloc (num_measurements*sizeof (uint16_t)); |
|
| 52 |
+ |
|
| 53 |
encoders_reset (); |
|
| 54 |
|
|
| 13 | 55 |
for (uint8_t m=0; m<num_measurements; ++m) |
| 14 | 56 |
{
|
| 15 | 57 |
robot_set_motors (direction1, velocity, direction2, velocity); |
| ... | ... | |
| 23 | 66 |
|
| 24 | 67 |
// TODO instead of waiting a fixed time, wait until the wheels |
| 25 | 68 |
// are not moving any more, using the station's encoders. |
| 26 |
|
|
| 27 |
// send (turn motor on for time) |
|
| 28 |
// wait (done) |
|
| 29 |
// wait_stopped |
|
| 30 |
// send (read encoders) |
|
| 31 |
// receive (data) |
|
| 32 |
// read encoders |
|
| 33 |
|
|
| 69 |
|
|
| 70 |
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; |
|
| 34 | 74 |
} |
| 35 | 75 |
|
| 36 |
// Send data |
|
| 37 |
for (uint8_t i=1; i<=2; ++i) |
|
| 38 |
{
|
|
| 39 |
uint8_t direction=(i==1)?direction1:direction2; |
|
| 40 |
|
|
| 41 |
if (direction!=motor_direction_off) |
|
| 42 |
{
|
|
| 43 |
usb_puts ("data encoder ");
|
|
| 44 |
usb_puti (i); |
|
| 45 |
usb_puts (" ");
|
|
| 46 |
usb_puts (motor_direction_string (direction)); |
|
| 47 |
usb_puts (" 100/100 200/200 300/300 400/400 500/500 600/600 700/700" NL);
|
|
| 48 |
} |
|
| 49 |
} |
|
| 76 |
send_encoder_data (0, direction1, data_station_l, data_robot_l); |
|
| 77 |
send_encoder_data (1, direction2, data_station_r, data_robot_r); |
|
| 78 |
|
|
| 79 |
free (data_robot_r); |
|
| 80 |
free (data_robot_l); |
|
| 81 |
free (data_station_r); |
|
| 82 |
free (data_station_l); |
|
| 50 | 83 |
} |
| 51 | 84 |
|
| 85 |
|
|
| 86 |
// ********************** |
|
| 87 |
// ** Public functions ** |
|
| 88 |
// ********************** |
|
| 89 |
|
|
| 52 | 90 |
void test_encoder_all (void) |
| 53 | 91 |
{
|
| 54 | 92 |
usb_puts("# Testing encoders" NL);
|
Also available in: Unified diff