root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1206
History | View | Annotate | Download (2.83 KB)
1 |
#include "test_motors.h" |
---|---|
2 |
#include "global.h" |
3 |
|
4 |
#include "comm_robot.h" |
5 |
|
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) |
13 |
{ |
14 |
robot_set_motors (direction1, velocity, direction2, velocity); |
15 |
delay_ms (vel_delay); |
16 |
|
17 |
// wait (steady)
|
18 |
//
|
19 |
// reset_encoders ();
|
20 |
// for (measurements)
|
21 |
// {
|
22 |
// measure left/right
|
23 |
// wait
|
24 |
// }
|
25 |
} |
26 |
|
27 |
|
28 |
#define acceleration_increasing 1 |
29 |
#define acceleration_decreasing 2 |
30 |
|
31 |
static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t vel_start, uint8_t vel_end, int8_t vel_step) |
32 |
{ |
33 |
// Use 16 bit variable for vel to avoid problems with overflow (if vel=250 and 10 is added, it would be 260-256=4
|
34 |
// which is still smaller than vel_max). There are more elegant solutions to this problem, but this one is
|
35 |
// easy and performance is not an issue here anyway.
|
36 |
|
37 |
// Distinguish between start>=end/start<end because we have to use the corresponding stop condition.
|
38 |
if (vel_end>=vel_start)
|
39 |
for (uint16_t vel=vel_start; vel<=vel_end; vel+=vel_step)
|
40 |
test_motors_direction_velocity (direction1, direction2, vel); |
41 |
else
|
42 |
for (uint16_t vel=vel_start; vel>=vel_end; vel+=vel_step)
|
43 |
test_motors_direction_velocity (direction1, direction2, vel); |
44 |
|
45 |
|
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 |
|
58 |
if (vel_end>vel_start)
|
59 |
usb_puts (" increasing ");
|
60 |
else if (vel_end<vel_start) |
61 |
usb_puts (" decreasing ");
|
62 |
else
|
63 |
usb_puts (" constant ");
|
64 |
|
65 |
usb_puts (" 160/100 170/200 180/300 190/400 200/500 210/600 220/700" NL);
|
66 |
} |
67 |
} |
68 |
} |
69 |
|
70 |
static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
71 |
{ |
72 |
test_motors_direction_acceleration (direction1, direction2, vel_min, vel_max, vel_inc); |
73 |
test_motors_direction_acceleration (direction1, direction2, vel_max, vel_min, -vel_inc); |
74 |
} |
75 |
|
76 |
|
77 |
void test_motor_all (void) |
78 |
{ |
79 |
usb_puts("# Testing motors" NL);
|
80 |
|
81 |
test_motors_direction (motor_direction_forward, motor_direction_backward); |
82 |
test_motors_direction (motor_direction_backward, motor_direction_forward); |
83 |
|
84 |
robot_set_motors_off (); |
85 |
|
86 |
usb_puts("# Testing motors finished" NL);
|
87 |
} |
88 |
|
89 |
void test_motor (uint8_t num)
|
90 |
{ |
91 |
if (num==1) |
92 |
{ |
93 |
test_motors_direction (motor_direction_forward, motor_direction_off); |
94 |
test_motors_direction (motor_direction_backward, motor_direction_off); |
95 |
} |
96 |
else if (num==2) |
97 |
{ |
98 |
test_motors_direction (motor_direction_off, motor_direction_forward); |
99 |
test_motors_direction (motor_direction_off, motor_direction_backward); |
100 |
} |
101 |
} |