Statistics
| Revision:

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

History | View | Annotate | Download (3.32 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, uint8_t num_measurements)
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, uint16_t on_delay, uint16_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
                //usb_puts ("# I readed teh encoders: ");
65
                //usb_puti (data_robot_l[m]);
66
                //usb_puts (" ");
67
                //usb_puti (data_robot_r[m]);
68
                //usb_puts (NL);
69

    
70
        }
71

    
72
        send_encoder_data (0, direction1, data_station_l, data_robot_l, num_measurements);
73
        send_encoder_data (1, direction2, data_station_r, data_robot_r, num_measurements);
74
        
75
        free (data_robot_r);
76
        free (data_robot_l);
77
        free (data_station_r);
78
        free (data_station_l);
79
}
80

    
81

    
82
// **********************
83
// ** Public functions **
84
// **********************
85

    
86
// Encoder readings seem to be unreliable with these values (on too long?)
87
//#define NUM 4
88
//#define VEL 200
89
//#define ON_DELAY 500
90
//#define OFF_DELAY 800
91

    
92
// Testing values
93
#define NUM 8
94
#define VEL 200
95
#define ON_DELAY 150
96
#define OFF_DELAY 400
97

    
98
void test_encoder_all (void)
99
{
100
        usb_puts("# Testing encoders" NL);
101

    
102
        test_encoders_direction (motor_direction_forward , motor_direction_backward, NUM, VEL, ON_DELAY, OFF_DELAY);
103
        test_encoders_direction (motor_direction_backward, motor_direction_forward , NUM, VEL, ON_DELAY, OFF_DELAY);
104

    
105
        usb_puts("# Testing encoders finished" NL);
106
}
107

    
108
void test_encoder (uint8_t num)
109
{
110
        if (num==1)
111
        {
112
                test_encoders_direction (motor_direction_forward , motor_direction_off, NUM, VEL, ON_DELAY, OFF_DELAY);
113
                test_encoders_direction (motor_direction_backward, motor_direction_off, NUM, VEL, ON_DELAY, OFF_DELAY);
114
        }
115
        else if (num==2)
116
        {
117
                test_encoders_direction (motor_direction_off, motor_direction_forward , NUM, VEL, ON_DELAY, OFF_DELAY);
118
                test_encoders_direction (motor_direction_off, motor_direction_backward, NUM, VEL, ON_DELAY, OFF_DELAY);
119
        }
120
}