Revision 1214
Implemented motors test
| test_motors.c | ||
|---|---|---|
| 2 | 2 |
#include "global.h" |
| 3 | 3 |
|
| 4 | 4 |
#include "comm_robot.h" |
| 5 |
#include "hardware_encoders.h" |
|
| 5 | 6 |
|
| 6 |
// NB don't produce overflows! |
|
| 7 |
#define vel_min 100 |
|
| 8 |
#define vel_max 250 |
|
| 9 |
#define vel_inc 10 |
|
| 10 |
#define vel_delay 200 |
|
| 11 |
|
|
| 12 |
static void test_motors_direction_velocity (uint8_t direction1, uint8_t direction2, uint8_t velocity) |
|
| 7 |
static void send_motor_data (uint8_t num, uint8_t direction, char *acceleration_string, |
|
| 8 |
uint8_t num_steps, uint8_t *data_pwm, int16_t *data_velocity) |
|
| 13 | 9 |
{
|
| 14 |
robot_set_motors (direction1, velocity, direction2, velocity); |
|
| 15 |
delay_ms (vel_delay); |
|
| 10 |
if (direction!=motor_direction_off) |
|
| 11 |
{
|
|
| 12 |
usb_puts ("data motor ");
|
|
| 13 |
usb_puti (num); |
|
| 14 |
usb_puts (" ");
|
|
| 15 |
usb_puts (motor_direction_string (direction)); |
|
| 16 |
usb_puts (" ");
|
|
| 17 |
usb_puts (acceleration_string); |
|
| 18 |
usb_puts (" ");
|
|
| 19 |
|
|
| 20 |
for (uint8_t i=0; i<num_steps; ++i) |
|
| 21 |
{
|
|
| 22 |
usb_putc (' ');
|
|
| 23 |
usb_puti (data_pwm[i]); |
|
| 24 |
usb_putc ('/');
|
|
| 25 |
usb_puti (data_velocity[i]); |
|
| 26 |
} |
|
| 16 | 27 |
|
| 17 |
// wait (steady) |
|
| 18 |
// |
|
| 19 |
// reset_encoders (); |
|
| 20 |
// for (measurements) |
|
| 21 |
// {
|
|
| 22 |
// measure left/right |
|
| 23 |
// wait |
|
| 24 |
// } |
|
| 28 |
usb_puts (NL); |
|
| 29 |
} |
|
| 25 | 30 |
} |
| 26 | 31 |
|
| 32 |
static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t pwm_start, uint8_t num_steps, int8_t pwm_step, char *acceleration_string) |
|
| 33 |
{
|
|
| 34 |
// Allocate space for the data on the stack |
|
| 35 |
uint8_t *data_pwm =malloc (num_steps*sizeof (uint8_t )); |
|
| 36 |
int16_t *data_velocity_l =malloc (num_steps*sizeof (uint16_t)); |
|
| 37 |
int16_t *data_velocity_r =malloc (num_steps*sizeof (uint16_t)); |
|
| 27 | 38 |
|
| 28 |
#define acceleration_increasing 1 |
|
| 29 |
#define acceleration_decreasing 2 |
|
| 39 |
uint8_t pwm=pwm_start; |
|
| 40 |
for (uint8_t i=0; i<num_steps; ++i) |
|
| 41 |
{
|
|
| 42 |
robot_set_motors (direction1, pwm, direction2, pwm); |
|
| 43 |
// TODO better |
|
| 44 |
#define vel_delay 400 |
|
| 45 |
delay_ms (vel_delay); // TODO wait steady |
|
| 30 | 46 |
|
| 31 |
// TODO: pass number of steps instead |
|
| 32 |
static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t vel_start, uint8_t vel_end, int8_t vel_step, char *acceleration_string) |
|
| 33 |
{
|
|
| 34 |
// Use 16 bit variable for vel to avoid problems with overflow (if vel=250 and 10 is added, it would be 260-256=4 |
|
| 35 |
// which is still smaller than vel_max). There are more elegant solutions to this problem, but this one is |
|
| 36 |
// easy and performance is not an issue here anyway. |
|
| 47 |
data_pwm[i]=pwm; |
|
| 37 | 48 |
|
| 38 |
// Distinguish between start>=end/start<end because we have to use the corresponding stop condition. |
|
| 39 |
if (vel_end>=vel_start) |
|
| 40 |
for (uint16_t vel=vel_start; vel<=vel_end; vel+=vel_step) |
|
| 41 |
test_motors_direction_velocity (direction1, direction2, vel); |
|
| 42 |
else |
|
| 43 |
for (uint16_t vel=vel_start; vel>=vel_end; vel+=vel_step) |
|
| 44 |
test_motors_direction_velocity (direction1, direction2, vel); |
|
| 49 |
encoders_reset (); |
|
| 50 |
// TODO delay |
|
| 51 |
encoders_read (&(data_velocity_l[i]), &(data_velocity_r[i])); |
|
| 45 | 52 |
|
| 46 |
- // Send data |
|
| 47 |
for (uint8_t i=1; i<=2; ++i) |
|
| 48 |
{
|
|
| 49 |
uint8_t direction=(i==1)?direction1:direction2; |
|
| 50 |
|
|
| 51 |
if (direction!=motor_direction_off) |
|
| 52 |
{
|
|
| 53 |
usb_puts ("data motor ");
|
|
| 54 |
usb_puti (i); |
|
| 55 |
usb_puts (" ");
|
|
| 56 |
usb_puts (motor_direction_string (direction)); |
|
| 57 |
usb_puts (" ");
|
|
| 58 |
usb_puts (acceleration_string); |
|
| 59 |
usb_puts (" ");
|
|
| 60 |
|
|
| 61 |
usb_puts (" 160/100 170/200 180/300 190/400 200/500 210/600 220/700" NL);
|
|
| 62 |
} |
|
| 53 |
pwm+=pwm_step; |
|
| 63 | 54 |
} |
| 55 |
|
|
| 56 |
send_motor_data (0, direction1, acceleration_string, num_steps, data_pwm, data_velocity_l); |
|
| 57 |
send_motor_data (1, direction2, acceleration_string, num_steps, data_pwm, data_velocity_r); |
|
| 58 |
|
|
| 59 |
robot_set_motors (motor_direction_off, 0, motor_direction_off, 0); |
|
| 60 |
|
|
| 61 |
free (data_velocity_r); |
|
| 62 |
free (data_velocity_l); |
|
| 63 |
free (data_pwm); |
|
| 64 | 64 |
} |
| 65 | 65 |
|
| 66 | 66 |
static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
| 67 | 67 |
{
|
| 68 |
test_motors_direction_acceleration (direction1, direction2, vel_min, vel_max, vel_inc, "increasing"); |
|
| 69 |
test_motors_direction_acceleration (direction1, direction2, vel_max, vel_min, -vel_inc, "decreasing"); |
|
| 68 |
#define vel_inc 10 |
|
| 69 |
#define vel_steps 10 |
|
| 70 |
#define vel_min 140 |
|
| 71 |
#define vel_max 250 |
|
| 72 |
|
|
| 73 |
test_motors_direction_acceleration (direction1, direction2, vel_min, vel_steps, vel_inc, "increasing"); |
|
| 74 |
test_motors_direction_acceleration (direction1, direction2, vel_max, vel_steps, -vel_inc, "decreasing"); |
|
| 70 | 75 |
} |
| 71 | 76 |
|
| 72 | 77 |
|
Also available in: Unified diff