Statistics
| Revision:

root / trunk / code / projects / diagnostic_station / station / test_encoders.c @ 1295

History | View | Annotate | Download (2.93 KB)

1
#include "test_encoders.h"
2
#include "global.h"
3
#include "comm_robot.h"
4

    
5
#include "hardware.h"
6

    
7
// ******************
8
// ** Data sending **
9
// ******************
10

    
11
static void send_encoder_data (uint8_t num, uint8_t direction, 
12
        int16_t *data_station, int16_t *data_robot)
13
{
14
        if (direction!=motor_direction_off)
15
        {
16
                usb_puts ("data encoder ");
17
                usb_puti (num);
18
                usb_puts (" ");
19
                usb_puts (motor_direction_string (direction));
20
                usb_puts (" ");
21
                        
22
                for (uint8_t i=0; i<num_measurements; ++i)
23
                {
24
                        usb_putc (' ');
25
                        usb_puti (data_station[i]);
26
                        usb_putc ('/');
27
                        usb_puti (data_robot[i]);
28
                }
29

    
30
                usb_puts (NL);
31
        }
32
}
33

    
34

    
35
// ************************
36
// ** Internal functions **
37
// ************************
38

    
39
static void test_encoders_direction (uint8_t direction1, uint8_t direction2, uint8_t num_measurements, uint8_t velocity, uint8_t on_delay, uint8_t off_delay)
40
{
41
        // Allocate space for the data on the stack
42
        int16_t *data_station_l  =malloc (num_measurements*sizeof (uint16_t));
43
        int16_t *data_station_r  =malloc (num_measurements*sizeof (uint16_t));
44
        int16_t *data_robot_l    =malloc (num_measurements*sizeof (uint16_t));
45
        int16_t *data_robot_r    =malloc (num_measurements*sizeof (uint16_t));
46

    
47
        usb_puts ("# Resetting encoders" NL);
48
        dynamos_reset ();
49
        robot_reset_encoders ();
50
        
51
        for (uint8_t m=0; m<num_measurements; ++m)
52
        {
53
                robot_set_motors (direction1, velocity, direction2, velocity);
54
                delay_ms (on_delay);
55
                robot_set_motors_off ();
56
                delay_ms (off_delay);
57

    
58
                // Read the station dynamos
59
                dynamos_read (&(data_station_l[m]), &(data_station_r[m]));
60
                
61
                // Read the robot encoders
62
                robot_read_encoders (&(data_robot_l[m]), &(data_robot_r[m]));
63
        }
64

    
65
        send_encoder_data (0, direction1, data_station_l, data_robot_l);
66
        send_encoder_data (1, direction2, data_station_r, data_robot_r);
67
        
68
        free (data_robot_r);
69
        free (data_robot_l);
70
        free (data_station_r);
71
        free (data_station_l);
72
}
73

    
74

    
75
// **********************
76
// ** Public functions **
77
// **********************
78

    
79
#define NUM 4
80
#define VEL 200
81
#define ON_DELAY 500
82
#define OFF_DELAY 800
83

    
84
void test_encoder_all (void)
85
{
86
        usb_puts("# Testing encoders" NL);
87

    
88
        test_encoders_direction (motor_direction_forward , motor_direction_backward, NUM, VEL, ON_DLEAY, OFF_DELAY);
89
        test_encoders_direction (motor_direction_backward, motor_direction_forward , NUM, VEL, ON_DLEAY, OFF_DELAY);
90

    
91
        usb_puts("# Testing encoders finished" NL);
92
}
93

    
94
void test_encoder (uint8_t num)
95
{
96
        if (num==1)
97
        {
98
                test_encoders_direction (motor_direction_forward , motor_direction_off, NUM, VEL, ON_DLEAY, OFF_DELAY);
99
                test_encoders_direction (motor_direction_backward, motor_direction_off, NUM, VEL, ON_DLEAY, OFF_DELAY);
100
        }
101
        else if (num==2)
102
        {
103
                test_encoders_direction (motor_direction_off, motor_direction_forward , NUM, VEL, ON_DLEAY, OFF_DELAY);
104
                test_encoders_direction (motor_direction_off, motor_direction_backward, NUM, VEL, ON_DLEAY, OFF_DELAY);
105
        }
106
}