root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1305
History | View | Annotate | Download (4.02 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 | 1305 | deffi | // One way to do it
|
43 | // #define vel_inc 10
|
||
44 | // #define vel_steps 12
|
||
45 | // #define vel_min 140
|
||
46 | // #define vel_max 250
|
||
47 | // #define velocity_steady_delay 400
|
||
48 | 1250 | deffi | |
49 | 1305 | deffi | // More steps at smaller intervals. We don't need so much delay here.
|
50 | #define vel_inc 2 |
||
51 | #define vel_steps 56 |
||
52 | #define vel_min 140 |
||
53 | #define vel_max 250 |
||
54 | #define velocity_steady_delay 50 |
||
55 | |||
56 | |||
57 | 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) |
58 | { |
||
59 | // Allocate space for the data on the stack
|
||
60 | uint8_t *data_pwm =malloc (num_steps*sizeof (uint8_t ));
|
||
61 | int16_t *data_velocity_l =malloc (num_steps*sizeof (uint16_t));
|
||
62 | int16_t *data_velocity_r =malloc (num_steps*sizeof (uint16_t));
|
||
63 | 1206 | deffi | |
64 | 1214 | deffi | uint8_t pwm=pwm_start; |
65 | for (uint8_t i=0; i<num_steps; ++i) |
||
66 | { |
||
67 | robot_set_motors (direction1, pwm, direction2, pwm); |
||
68 | 1206 | deffi | |
69 | 1250 | deffi | // Wait some fixed time for the velocity to reach steady state. This could be improved.
|
70 | delay_ms (velocity_steady_delay); |
||
71 | |||
72 | 1214 | deffi | data_pwm[i]=pwm; |
73 | 1206 | deffi | |
74 | 1251 | deffi | // Reset the dynamos, wait some time, then read the dynamos.
|
75 | // Nothing else should go between dynamos_reset and dynamos_read.
|
||
76 | 1250 | deffi | // Note that we are currently using busy waiting which will be totally wrong if there are any interrupts.
|
77 | 1251 | deffi | dynamos_reset (); |
78 | 1250 | deffi | delay_ms (500);
|
79 | 1251 | deffi | dynamos_read (&(data_velocity_l[i]), &(data_velocity_r[i])); |
80 | 1250 | deffi | |
81 | 1251 | deffi | // Velocity readings are dynamo ticks per second.
|
82 | 1250 | deffi | data_velocity_l[i]*=2;
|
83 | data_velocity_r[i]*=2;
|
||
84 | 1159 | deffi | |
85 | 1214 | deffi | pwm+=pwm_step; |
86 | 1203 | deffi | } |
87 | 1214 | deffi | |
88 | send_motor_data (0, direction1, acceleration_string, num_steps, data_pwm, data_velocity_l);
|
||
89 | send_motor_data (1, direction2, acceleration_string, num_steps, data_pwm, data_velocity_r);
|
||
90 | |||
91 | robot_set_motors (motor_direction_off, 0, motor_direction_off, 0); |
||
92 | |||
93 | free (data_velocity_r); |
||
94 | free (data_velocity_l); |
||
95 | free (data_pwm); |
||
96 | 1206 | deffi | } |
97 | 1203 | deffi | |
98 | 1206 | deffi | static void test_motors_direction (uint8_t direction1, uint8_t direction2) |
99 | { |
||
100 | 1250 | deffi | // The following equation must be true: vel_min+(vel_steps-1)*vel_inc=vel_max
|
101 | // Note that we're setting vel_max twice, once going up and once going down. It's not important for vel_max, but
|
||
102 | // vel_min is also reached twice, once at the beginning of accelerating and once at the end of decelerating, and
|
||
103 | // this one is important.
|
||
104 | |||
105 | 1214 | deffi | test_motors_direction_acceleration (direction1, direction2, vel_min, vel_steps, vel_inc, "increasing");
|
106 | test_motors_direction_acceleration (direction1, direction2, vel_max, vel_steps, -vel_inc, "decreasing");
|
||
107 | 1159 | deffi | } |
108 | |||
109 | 1204 | deffi | |
110 | 1215 | deffi | // **********************
|
111 | // ** Public functions **
|
||
112 | // **********************
|
||
113 | |||
114 | 1202 | deffi | void test_motor_all (void) |
115 | 1151 | deffi | { |
116 | 1166 | deffi | usb_puts("# Testing motors" NL);
|
117 | 1151 | deffi | |
118 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_backward); |
119 | test_motors_direction (motor_direction_backward, motor_direction_forward); |
||
120 | 1159 | deffi | |
121 | 1164 | deffi | robot_set_motors_off (); |
122 | 1159 | deffi | |
123 | 1166 | deffi | usb_puts("# Testing motors finished" NL);
|
124 | 1151 | deffi | } |
125 | 1202 | deffi | |
126 | void test_motor (uint8_t num)
|
||
127 | { |
||
128 | 1204 | deffi | if (num==1) |
129 | { |
||
130 | 1205 | deffi | test_motors_direction (motor_direction_forward, motor_direction_off); |
131 | test_motors_direction (motor_direction_backward, motor_direction_off); |
||
132 | 1204 | deffi | } |
133 | else if (num==2) |
||
134 | { |
||
135 | 1205 | deffi | test_motors_direction (motor_direction_off, motor_direction_forward); |
136 | test_motors_direction (motor_direction_off, motor_direction_backward); |
||
137 | 1204 | deffi | } |
138 | 1202 | deffi | } |