root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1204
History | View | Annotate | Download (2.77 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 |
uint8_t velocity1=velocity, velocity2=velocity; |
15 |
|
16 |
robot_set_motors (direction1, velocity, direction2, velocity); |
17 |
delay_ms (vel_delay); |
18 |
|
19 |
// wait (steady)
|
20 |
//
|
21 |
// reset_encoders ();
|
22 |
// for (measurements)
|
23 |
// {
|
24 |
// measure left/right
|
25 |
// wait
|
26 |
// }
|
27 |
} |
28 |
|
29 |
static char *direction_string (uint8_t direction) |
30 |
{ |
31 |
if (direction==direction_forward)
|
32 |
return "forward"; |
33 |
else if (direction==direction_backward) |
34 |
return "backward"; |
35 |
else if (direction==direction_off) |
36 |
return "off"; |
37 |
else
|
38 |
return "?"; |
39 |
} |
40 |
|
41 |
static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
42 |
{ |
43 |
// Use 16 bit variable for vel to avoid problems with overflow (if vel=250 and 10 is added, it would be 260-256=4
|
44 |
// which is still smaller than vel_max). There are more elegant solutions to this problem, but this one is
|
45 |
// easy and performance is not an issue here anyway.
|
46 |
|
47 |
for (uint16_t vel=vel_min; vel<=vel_max; vel+=vel_inc)
|
48 |
test_motors_direction_velocity (direction1, direction2, vel); |
49 |
|
50 |
// Send data
|
51 |
for (uint8_t i=1; i<=2; ++i) |
52 |
{ |
53 |
uint8_t direction=(i==1)?direction1:direction2;
|
54 |
|
55 |
if (direction!=direction_off)
|
56 |
{ |
57 |
usb_puts ("data motor ");
|
58 |
usb_puti (i); |
59 |
usb_puts (" ");
|
60 |
usb_puts (direction_string (direction)); |
61 |
usb_puts (" increasing ");
|
62 |
usb_puts (" 160/100 170/200 180/300 190/400 200/500 210/600 220/700" NL);
|
63 |
} |
64 |
} |
65 |
|
66 |
// TODO code duplication
|
67 |
|
68 |
for (uint16_t vel=vel_max; vel>=vel_min; vel-=vel_inc)
|
69 |
test_motors_direction_velocity (direction1, direction2, vel); |
70 |
|
71 |
// Send data
|
72 |
for (uint8_t i=1; i<=2; ++i) |
73 |
{ |
74 |
uint8_t direction=(i==1)?direction1:direction2;
|
75 |
|
76 |
if (direction!=direction_off)
|
77 |
{ |
78 |
usb_puts ("data motor ");
|
79 |
usb_puti (i); |
80 |
usb_puts (" ");
|
81 |
usb_puts (direction_string (direction)); |
82 |
usb_puts (" decreasing ");
|
83 |
usb_puts (" 220/700 210/600 200/500 190/400 180/300 170/200 160/100" NL);
|
84 |
} |
85 |
} |
86 |
|
87 |
|
88 |
} |
89 |
|
90 |
|
91 |
void test_motor_all (void) |
92 |
{ |
93 |
usb_puts("# Testing motors" NL);
|
94 |
|
95 |
test_motors_direction (direction_forward, direction_backward); |
96 |
test_motors_direction (direction_backward, direction_forward); |
97 |
|
98 |
robot_set_motors_off (); |
99 |
|
100 |
usb_puts("# Testing motors finished" NL);
|
101 |
} |
102 |
|
103 |
void test_motor (uint8_t num)
|
104 |
{ |
105 |
if (num==1) |
106 |
{ |
107 |
test_motors_direction (direction_forward, direction_off); |
108 |
test_motors_direction (direction_backward, direction_off); |
109 |
} |
110 |
else if (num==2) |
111 |
{ |
112 |
test_motors_direction (direction_off, direction_forward); |
113 |
test_motors_direction (direction_off, direction_backward); |
114 |
} |
115 |
} |