root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1251
History | View | Annotate | Download (3.8 KB)
1 |
#include "test_motors.h" |
---|---|
2 |
#include "global.h" |
3 |
|
4 |
#include "comm_robot.h" |
5 |
#include "hardware_dynamo.h" |
6 |
|
7 |
|
8 |
// ******************
|
9 |
// ** Data sending **
|
10 |
// ******************
|
11 |
|
12 |
static void send_motor_data (uint8_t num, uint8_t direction, char *acceleration_string, |
13 |
uint8_t num_steps, uint8_t *data_pwm, int16_t *data_velocity) |
14 |
{ |
15 |
if (direction!=motor_direction_off)
|
16 |
{ |
17 |
usb_puts ("data motor ");
|
18 |
usb_puti (num); |
19 |
usb_puts (" ");
|
20 |
usb_puts (motor_direction_string (direction)); |
21 |
usb_puts (" ");
|
22 |
usb_puts (acceleration_string); |
23 |
usb_puts (" ");
|
24 |
|
25 |
for (uint8_t i=0; i<num_steps; ++i) |
26 |
{ |
27 |
usb_putc (' ');
|
28 |
usb_puti (data_pwm[i]); |
29 |
usb_putc ('/');
|
30 |
usb_puti (data_velocity[i]); |
31 |
} |
32 |
|
33 |
usb_puts (NL); |
34 |
} |
35 |
} |
36 |
|
37 |
|
38 |
// ************************
|
39 |
// ** Internal functions **
|
40 |
// ************************
|
41 |
|
42 |
#define velocity_steady_delay 400 |
43 |
|
44 |
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) |
45 |
{ |
46 |
// Allocate space for the data on the stack
|
47 |
uint8_t *data_pwm =malloc (num_steps*sizeof (uint8_t ));
|
48 |
int16_t *data_velocity_l =malloc (num_steps*sizeof (uint16_t));
|
49 |
int16_t *data_velocity_r =malloc (num_steps*sizeof (uint16_t));
|
50 |
|
51 |
uint8_t pwm=pwm_start; |
52 |
for (uint8_t i=0; i<num_steps; ++i) |
53 |
{ |
54 |
robot_set_motors (direction1, pwm, direction2, pwm); |
55 |
|
56 |
// Wait some fixed time for the velocity to reach steady state. This could be improved.
|
57 |
delay_ms (velocity_steady_delay); |
58 |
|
59 |
data_pwm[i]=pwm; |
60 |
|
61 |
// Reset the dynamos, wait some time, then read the dynamos.
|
62 |
// Nothing else should go between dynamos_reset and dynamos_read.
|
63 |
// Note that we are currently using busy waiting which will be totally wrong if there are any interrupts.
|
64 |
dynamos_reset (); |
65 |
delay_ms (500);
|
66 |
dynamos_read (&(data_velocity_l[i]), &(data_velocity_r[i])); |
67 |
|
68 |
// Velocity readings are dynamo ticks per second.
|
69 |
data_velocity_l[i]*=2;
|
70 |
data_velocity_r[i]*=2;
|
71 |
|
72 |
pwm+=pwm_step; |
73 |
} |
74 |
|
75 |
send_motor_data (0, direction1, acceleration_string, num_steps, data_pwm, data_velocity_l);
|
76 |
send_motor_data (1, direction2, acceleration_string, num_steps, data_pwm, data_velocity_r);
|
77 |
|
78 |
robot_set_motors (motor_direction_off, 0, motor_direction_off, 0); |
79 |
|
80 |
free (data_velocity_r); |
81 |
free (data_velocity_l); |
82 |
free (data_pwm); |
83 |
} |
84 |
|
85 |
static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
86 |
{ |
87 |
// The following equation must be true: vel_min+(vel_steps-1)*vel_inc=vel_max
|
88 |
// Note that we're setting vel_max twice, once going up and once going down. It's not important for vel_max, but
|
89 |
// vel_min is also reached twice, once at the beginning of accelerating and once at the end of decelerating, and
|
90 |
// this one is important.
|
91 |
|
92 |
#define vel_inc 10 |
93 |
#define vel_steps 12 |
94 |
#define vel_min 140 |
95 |
#define vel_max 250 |
96 |
|
97 |
test_motors_direction_acceleration (direction1, direction2, vel_min, vel_steps, vel_inc, "increasing");
|
98 |
test_motors_direction_acceleration (direction1, direction2, vel_max, vel_steps, -vel_inc, "decreasing");
|
99 |
} |
100 |
|
101 |
|
102 |
// **********************
|
103 |
// ** Public functions **
|
104 |
// **********************
|
105 |
|
106 |
void test_motor_all (void) |
107 |
{ |
108 |
usb_puts("# Testing motors" NL);
|
109 |
|
110 |
test_motors_direction (motor_direction_forward, motor_direction_backward); |
111 |
test_motors_direction (motor_direction_backward, motor_direction_forward); |
112 |
|
113 |
robot_set_motors_off (); |
114 |
|
115 |
usb_puts("# Testing motors finished" NL);
|
116 |
} |
117 |
|
118 |
void test_motor (uint8_t num)
|
119 |
{ |
120 |
if (num==1) |
121 |
{ |
122 |
test_motors_direction (motor_direction_forward, motor_direction_off); |
123 |
test_motors_direction (motor_direction_backward, motor_direction_off); |
124 |
} |
125 |
else if (num==2) |
126 |
{ |
127 |
test_motors_direction (motor_direction_off, motor_direction_forward); |
128 |
test_motors_direction (motor_direction_off, motor_direction_backward); |
129 |
} |
130 |
} |