Statistics
| Revision:

root / trunk / code / projects / diagnostic_station / station / test_motors.c @ 1305

History | View | Annotate | Download (4.02 KB)

1
#include "test_motors.h"
2
#include "global.h"
3

    
4
#include "comm_robot.h"
5
#include "hardware_dynamos.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
        // 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

    
49
        // 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
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

    
64
        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

    
69
                // Wait some fixed time for the velocity to reach steady state. This could be improved.
70
                delay_ms (velocity_steady_delay);
71

    
72
                data_pwm[i]=pwm;
73

    
74
                // Reset the dynamos, wait some time, then read the dynamos.
75
                // Nothing else should go between dynamos_reset and dynamos_read.
76
                // Note that we are currently using busy waiting which will be totally wrong if there are any interrupts.
77
                dynamos_reset ();
78
                delay_ms (500);
79
                dynamos_read (&(data_velocity_l[i]), &(data_velocity_r[i]));
80
                
81
                // Velocity readings are dynamo ticks per second.
82
                data_velocity_l[i]*=2;
83
                data_velocity_r[i]*=2;
84
        
85
                pwm+=pwm_step;
86
        }
87

    
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
}
97

    
98
static void test_motors_direction (uint8_t direction1, uint8_t direction2)
99
{
100
        // 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
        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
}
108

    
109

    
110
// **********************
111
// ** Public functions **
112
// **********************
113

    
114
void test_motor_all (void)
115
{
116
        usb_puts("# Testing motors" NL);
117

    
118
        test_motors_direction (motor_direction_forward, motor_direction_backward);
119
        test_motors_direction (motor_direction_backward, motor_direction_forward);
120

    
121
        robot_set_motors_off ();
122

    
123
        usb_puts("# Testing motors finished" NL);
124
}
125

    
126
void test_motor (uint8_t num)
127
{
128
        if (num==1)
129
        {
130
                test_motors_direction (motor_direction_forward, motor_direction_off);
131
                test_motors_direction (motor_direction_backward, motor_direction_off);
132
        }
133
        else if (num==2)
134
        {
135
                test_motors_direction (motor_direction_off, motor_direction_forward);
136
                test_motors_direction (motor_direction_off, motor_direction_backward);
137
        }
138
}