Project

General

Profile

Statistics
| Revision:

root / branches / encoders / code / projects / libdragonfly / encoders.c @ 1191

History | View | Annotate | Download (7 KB)

1
#include "encoders.h"
2
#include "spi.h"
3
#include <dragonfly_lib.h>
4
#include "ring_buffer.h"
5

    
6
unsigned int left_data_buf;
7
unsigned int right_data_buf;
8
char encoder_buf_index;
9

    
10
unsigned int left_data;
11
unsigned int right_data;
12

    
13
unsigned int left_data_array[BUFFER_SIZE];
14
unsigned int right_data_array[BUFFER_SIZE];
15
int left_data_idx;
16
int right_data_idx;
17

    
18
int left_dx;
19
int right_dx;
20
long int timecount;
21

    
22
volatile short int data_ready;
23

    
24
void encoder_recv(char data);
25

    
26
//Helper Function Prototypes
27
inline void left_data_array_put(unsigned short int value);
28
inline unsigned int left_data_array_top(void);
29
inline unsigned int left_data_array_prev(void);
30
inline unsigned int left_data_array_bottom(void);
31

    
32
inline void right_data_array_put(unsigned short int value);
33
inline unsigned int right_data_array_top(void);
34
inline unsigned int right_data_array_prev(void);
35
inline unsigned int right_data_array_bottom(void);
36

    
37

    
38
void encoder_recv_complete(){
39
          encoder_buf_index = 0;
40
        data_ready++;
41
  
42
           spi_transfer(5);
43
}
44

    
45
/** 
46
* @brief Initializes encoder variables and the hardware interface.
47
*/
48
void encoders_init(void){
49
        int i;
50
        
51
        data_ready=0;
52

    
53
        spi_init(encoder_recv, encoder_recv_complete);
54
        encoder_buf_index = 0;
55
        left_data_buf = 0;
56
        right_data_buf= 0;
57
        left_data = -1;
58
        right_data = -1;
59
        
60
        //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
61
        left_data_idx = 0;
62
        right_data_idx = 0;
63
        for(i = 0; i < BUFFER_SIZE; i++) {
64
                left_data_array[i] = 0;
65
        }
66
        for(i = 0; i < BUFFER_SIZE; i++) {
67
                right_data_array[i] = 0;
68
        }
69
        spi_transfer(5);
70
}
71

    
72
/**
73
 * @brief Returns the specified encoders value
74
 *
75
 * @param encoder this is the encoder that you want to read. Valid arguments
76
 *          are LEFT and RIGHT
77
 *
78
 * @return the value of the specified encoder
79
 **/
80
int encoder_read(char encoder){
81
        
82
        if(encoder==LEFT) return left_data;
83
        else if(encoder==RIGHT) return right_data;
84
        else return -1;
85
}
86

    
87

    
88
/** 
89
* @brief Outputs encoder direction as FORWARD OR BACK
90
* A STUB! DO NOT use.
91
* 
92
* @param encoder The encoder you want the direction of.
93
* Valid arguments are right and left.
94
* 
95
* @return FORWARD or BACK (the constants)
96
*/
97
char encoder_direction(char encoder){
98
        return 0;
99
}
100

    
101
/**
102
 * Gets the total distance covered by the specified encoder (in encoder clicks)
103
 *
104
 * @param encoder the encoder that you want to read, use LEFT or RIGHT
105
 *
106
 * @return The distance covered by the specified encoder.
107
 **/
108
int encoder_get_dx(char encoder) {
109
        
110
        if(encoder==LEFT) return left_dx;
111
        else if(encoder==RIGHT) return right_dx;
112
        else return -1;
113
}
114

    
115
/**
116
 * Resets the distance accumulator for the specified
117
 *  encoder.
118
 *
119
 * @param encoder the encoder that you want to reset distance for
120
 **/
121
void encoder_rst_dx(char encoder) {
122
        
123
        if(encoder==LEFT) left_dx = 0;
124
        else if(encoder==RIGHT) right_dx = 0;
125
}
126

    
127
/** 
128
* @brief Returns the number of encoder reads that have occurred.
129
* 
130
* @return The time count.
131
*/
132
int encoder_get_tc(void) {
133
        return timecount;
134
}
135

    
136
/** 
137
* @brief Resets the encoder read counter.
138
*/
139
void encoder_rst_tc(void) {
140
        timecount = 0;
141
}
142

    
143

    
144
/** 
145
* @brief Returns the approximated instantaneous velocity of the robot
146
* in terms of encoder clicks.
147
* 
148
* @param encoder RIGHT or LEFT - the wheel you want the velocity for.
149
* 
150
* @return The instantaneous velocity for the given wheel.
151
*/
152
int encoder_get_v(char encoder){
153
        if (encoder == LEFT)
154
                return left_data_array_bottom() - left_data_array_top();
155

    
156
        if (encoder == RIGHT)
157
                return right_data_array_top() - right_data_array_bottom();
158

    
159
        return -1; /* TODO: velocity could be -1, use another value for error */
160
}
161

    
162

    
163
/** 
164
* @brief Waits until n encoder reads have occurred.
165
* Counter is reset on functions exit.
166
* 
167
* @param n 
168
*/
169
void encoder_wait(int n){
170
        while(data_ready<n);
171
        data_ready=0;
172
}
173

    
174

    
175
//Full reads occur every 40 microseconds. This function should be called
176
//every 8 microseconds.
177
void encoder_recv(char data){
178
        short int dx;
179
        
180
        //Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
181
        // second is offset by 1 bit.
182
        switch(encoder_buf_index){        
183
        case 0: 
184
                right_data_buf |= ((short)data)<<8 & 0xff00;
185
                break;
186
        case 1:
187
                right_data_buf |= ((short)data) & 0xff;
188
                break;
189
        case 2:
190
                left_data_buf |= (((short)data) << 9) & (0x7F << 9);
191
                break;
192
        case 3:
193
                left_data_buf |= (((short)data) << 1) & (0xFF<<1);
194
                break;
195
        case 4: left_data_buf |= (((short)data)>>7) & 0x1;
196
        }        
197
                
198
        encoder_buf_index = (encoder_buf_index + 1) % 5;
199

    
200
        if(encoder_buf_index==0) {
201
                
202
                /*Error handling for the left encoder*/ 
203
                if(!(left_data_buf & OCF)) 
204
                        left_data = ENCODER_DATA_NOT_READY; 
205
                if(left_data_buf & (COF | LIN))  
206
                        left_data = ENCODER_MISALIGNED;
207
                else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
208
                        left_data = ENCODER_MAGNET_FAILURE;
209
                else left_data = (left_data_buf>>5) & 1023;
210
                        
211
                /*Error handling for the right encoder*/ 
212
                 if(!(right_data_buf & OCF))
213
                        right_data = ENCODER_DATA_NOT_READY;        
214
                if(right_data_buf & (COF | LIN)) 
215
                        right_data = ENCODER_MISALIGNED;
216
                else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
217
                        right_data = ENCODER_MAGNET_FAILURE;
218
                else right_data = (right_data_buf>>5) & 1023;
219
                        
220
                  left_data_buf = 0;
221
                right_data_buf = 0;
222

    
223
                /*Above 1023 is invalid data*/        
224
                if(!(left_data > 1023)) {
225
                        left_data_array_put(left_data);
226

    
227
                        //Adjust left accumulator
228
                        dx = - left_data + left_data_array_prev();
229
                        
230
                        if(left_data_array_prev()==0)  dx=0;
231

    
232
                        if(dx > 512) left_dx += dx - 1023; //Underflow
233
                        else if(dx < -512) left_dx += dx + 1023; //Overflow
234
                        else left_dx += dx;
235
                }
236

    
237
                /*Above 1023 is invalid data*/        
238
                if(!(right_data > 1023)) {
239
                        right_data_array_put(right_data);
240

    
241
                        //Adjust right accumulator
242
                        dx = right_data - right_data_array_prev();
243

    
244
                        if(right_data_array_prev()==0) dx=0;
245
                          
246
                        if(dx > 512) right_dx += dx - 1023; //underflow
247
                        else if(dx < -512) right_dx += dx + 1023; //overflow 
248
                        else right_dx += dx;
249
                }
250
        }
251

    
252
        //Increment timecount accumulator
253
        timecount++;
254
}
255

    
256

    
257
//Helper Functions
258
inline void left_data_array_put(unsigned short int value) {
259
        if(left_data_idx == BUFFER_SIZE-1)
260
                left_data_idx = 0;
261
        else
262
                left_data_idx++;
263
        left_data_array[left_data_idx] = value;
264
}
265

    
266
inline unsigned int left_data_array_top(void) {
267
        return left_data_array[left_data_idx];
268
}
269

    
270
inline unsigned int left_data_array_prev(void) {
271
        if(left_data_idx == 0)
272
                return left_data_array[BUFFER_SIZE-1];
273
        else
274
                return left_data_array[left_data_idx - 1];
275
}
276

    
277
inline unsigned int left_data_array_bottom(void) {
278
        if(left_data_idx == BUFFER_SIZE-1)
279
                return left_data_array[0];
280
        else
281
                return left_data_array[left_data_idx + 1];
282
}
283

    
284
inline void right_data_array_put(unsigned short int value) {
285
        if(right_data_idx == BUFFER_SIZE-1)
286
                right_data_idx = 0;
287
        else
288
                right_data_idx++;
289
        right_data_array[right_data_idx] = value;
290
}
291

    
292
inline unsigned int right_data_array_top(void) {
293
        return right_data_array[right_data_idx];
294
}
295

    
296
inline unsigned int right_data_array_prev(void) {
297
        if(right_data_idx == 0)
298
                return right_data_array[BUFFER_SIZE-1];
299
        else
300
                return right_data_array[right_data_idx - 1];
301
}
302

    
303
inline unsigned int right_data_array_bottom(void) {
304
        if(right_data_idx == BUFFER_SIZE-1)
305
                return right_data_array[0];
306
        else
307
                return right_data_array[right_data_idx + 1];
308
}
309