root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1281
History | View | Annotate | Download (3.8 KB)
1 | 1151 | deffi | #include "test_motors.h" |
---|---|---|---|
2 | #include "global.h" |
||
3 | |||
4 | 1182 | deffi | #include "comm_robot.h" |
5 | 1257 | bneuman | #include "hardware_dynamos.h" |
6 | 1159 | deffi | |
7 | 1215 | deffi | |
8 | // ******************
|
||
9 | // ** Data sending **
|
||
10 | // ******************
|
||
11 | |||
12 | 1214 | deffi | 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 | 1159 | deffi | { |
15 | 1214 | deffi | 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 | 1159 | deffi | |
33 | 1214 | deffi | usb_puts (NL); |
34 | } |
||
35 | 1159 | deffi | } |
36 | |||
37 | 1215 | deffi | |
38 | // ************************
|
||
39 | // ** Internal functions **
|
||
40 | // ************************
|
||
41 | |||
42 | 1250 | deffi | #define velocity_steady_delay 400 |
43 | |||
44 | 1214 | deffi | 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 | 1206 | deffi | |
51 | 1214 | deffi | 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 | 1206 | deffi | |
56 | 1250 | deffi | // Wait some fixed time for the velocity to reach steady state. This could be improved.
|
57 | delay_ms (velocity_steady_delay); |
||
58 | |||
59 | 1214 | deffi | data_pwm[i]=pwm; |
60 | 1206 | deffi | |
61 | 1251 | deffi | // Reset the dynamos, wait some time, then read the dynamos.
|
62 | // Nothing else should go between dynamos_reset and dynamos_read.
|
||
63 | 1250 | deffi | // Note that we are currently using busy waiting which will be totally wrong if there are any interrupts.
|
64 | 1251 | deffi | dynamos_reset (); |
65 | 1250 | deffi | delay_ms (500);
|
66 | 1251 | deffi | dynamos_read (&(data_velocity_l[i]), &(data_velocity_r[i])); |
67 | 1250 | deffi | |
68 | 1251 | deffi | // Velocity readings are dynamo ticks per second.
|
69 | 1250 | deffi | data_velocity_l[i]*=2;
|
70 | data_velocity_r[i]*=2;
|
||
71 | 1159 | deffi | |
72 | 1214 | deffi | pwm+=pwm_step; |
73 | 1203 | deffi | } |
74 | 1214 | deffi | |
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 | 1206 | deffi | } |
84 | 1203 | deffi | |
85 | 1206 | deffi | static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
86 | { |
||
87 | 1250 | deffi | // 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 | 1214 | deffi | #define vel_inc 10 |
93 | 1250 | deffi | #define vel_steps 12 |
94 | 1214 | deffi | #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 | 1159 | deffi | } |
100 | |||
101 | 1204 | deffi | |
102 | 1215 | deffi | // **********************
|
103 | // ** Public functions **
|
||
104 | // **********************
|
||
105 | |||
106 | 1202 | deffi | void test_motor_all (void) |
107 | 1151 | deffi | { |
108 | 1166 | deffi | usb_puts("# Testing motors" NL);
|
109 | 1151 | deffi | |
110 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_backward); |
111 | test_motors_direction (motor_direction_backward, motor_direction_forward); |
||
112 | 1159 | deffi | |
113 | 1164 | deffi | robot_set_motors_off (); |
114 | 1159 | deffi | |
115 | 1166 | deffi | usb_puts("# Testing motors finished" NL);
|
116 | 1151 | deffi | } |
117 | 1202 | deffi | |
118 | void test_motor (uint8_t num)
|
||
119 | { |
||
120 | 1204 | deffi | if (num==1) |
121 | { |
||
122 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_off); |
123 | test_motors_direction (motor_direction_backward, motor_direction_off); |
||
124 | 1204 | deffi | } |
125 | else if (num==2) |
||
126 | { |
||
127 | 1205 | deffi | test_motors_direction (motor_direction_off, motor_direction_forward); |
128 | test_motors_direction (motor_direction_off, motor_direction_backward); |
||
129 | 1204 | deffi | } |
130 | 1202 | deffi | } |