Revision 752
fixed lost of bugs in the libraries for spi and encoder
encoders.c | ||
---|---|---|
37 | 37 |
void encoder_recv_complete(void); |
38 | 38 |
|
39 | 39 |
void encoder_recv_complete(){ |
40 |
// usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n"); |
|
41 |
// usb_puts("\r\n"); |
|
40 | 42 |
spi_transfer(5); |
41 | 43 |
} |
42 | 44 |
|
45 |
void put_bin(char data){ |
|
46 |
int i; |
|
43 | 47 |
|
48 |
for(i=7;i>=0;i--) |
|
49 |
usb_puti((data>>i)&1); |
|
50 |
usb_puts(" "); |
|
51 |
} |
|
52 |
|
|
44 | 53 |
void encoders_init(void){ |
45 | 54 |
int i; |
46 | 55 |
|
47 |
spi_init(encoder_recv, encoder_recv_complete); |
|
56 |
spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
|
|
48 | 57 |
buf_index = 0; |
49 | 58 |
left_data_buf = 0; |
50 | 59 |
right_data_buf= 0; |
... | ... | |
108 | 117 |
//usb_puti(buf_index); |
109 | 118 |
short int dx; |
110 | 119 |
|
111 |
if(buf_index < 2) |
|
112 |
right_data_buf |= (((short)data) << ((1-buf_index)*8)) & (0xFF<<(1-buf_index)); |
|
120 |
if(buf_index == 0) |
|
121 |
right_data_buf |= ((short)data)<<8 & 0xff00; |
|
122 |
|
|
123 |
else if (buf_index == 1) |
|
124 |
right_data_buf |= ((short)data) & 0xff; |
|
113 | 125 |
|
114 | 126 |
else if (buf_index == 2) |
115 | 127 |
left_data_buf |= (((short)data) << 9) & (0x7F << 9); |
... | ... | |
123 | 135 |
|
124 | 136 |
buf_index = (buf_index + 1) % 5; |
125 | 137 |
|
126 |
if(buf_index==0){ |
|
127 |
//if(left_data_buf & (OCF | COF | LIN)) |
|
128 |
// left_data = INVALID; |
|
138 |
if(buf_index==0) { |
|
139 |
if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF)){ |
|
140 |
left_data = INVALID; |
|
141 |
} |
|
129 | 142 |
|
130 |
/*else*/ if(((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)) |
|
131 |
left_data = MAGNET_FAILURE; |
|
143 |
else if(((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)){ |
|
144 |
left_data = MAGNET_FAILURE; |
|
145 |
} |
|
146 |
else{ |
|
147 |
left_data = (left_data_buf>>5) & 1023; |
|
148 |
} |
|
132 | 149 |
|
133 |
else |
|
134 |
left_data = (left_data_buf>>5) & 1023; |
|
135 |
|
|
136 |
if(right_data_buf & (OCF | COF | LIN)) |
|
137 |
right_data = INVALID; |
|
138 |
|
|
139 |
else if ( ((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)) |
|
140 |
left_data = MAGNET_FAILURE; |
|
141 |
|
|
142 |
else right_data = (right_data_buf>>5) & 1023; |
|
150 |
if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)){ |
|
151 |
right_data = INVALID; |
|
152 |
} |
|
153 |
else if ( ((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)){ |
|
154 |
left_data = MAGNET_FAILURE; |
|
155 |
} |
|
156 |
else{ |
|
157 |
right_data = (right_data_buf>>5) & 1023; |
|
158 |
} |
|
143 | 159 |
|
144 |
usb_puts("[");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("]\r\n"); |
|
160 |
|
|
161 |
//if(left_data!=INVALID || right_data!=INVALID){ |
|
162 |
// usb_puts("{");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("}\r\n"); |
|
163 |
//} |
|
145 | 164 |
|
146 | 165 |
left_data_buf = 0; |
147 | 166 |
right_data_buf = 0; |
148 |
|
|
149 |
} |
|
150 | 167 |
|
151 | 168 |
|
152 | 169 |
if(left_data < INVALID) { |
... | ... | |
154 | 171 |
left_data_array_put(left_data); |
155 | 172 |
|
156 | 173 |
//Adjust left accumulator |
157 |
dx = left_data - left_data_array_prev(); |
|
174 |
//dx = left_data - left_data_array_prev(); |
|
175 |
dx = left_data_array_prev() - left_data; |
|
176 |
|
|
177 |
/*left dx was negative on robot 7, could just be that |
|
178 |
the encoder is backwards, so this may need to change |
|
179 |
back |
|
180 |
*/ |
|
181 |
|
|
182 |
|
|
183 |
|
|
184 |
if(left_data_array_prev()==0) |
|
185 |
dx=0; |
|
186 |
|
|
158 | 187 |
if(dx > 5) { //underflow |
159 | 188 |
left_dx += dx - 1023; |
160 | 189 |
} |
... | ... | |
172 | 201 |
|
173 | 202 |
//Adjust right accumulator |
174 | 203 |
dx = right_data - right_data_array_prev(); |
204 |
|
|
205 |
if(right_data_array_prev()==0) |
|
206 |
dx=0; |
|
207 |
|
|
175 | 208 |
if(dx > 5) { //underflow |
176 | 209 |
right_dx += dx - 1023; |
177 | 210 |
} |
... | ... | |
182 | 215 |
right_dx += dx; |
183 | 216 |
} |
184 | 217 |
} |
218 |
} |
|
185 | 219 |
|
186 | 220 |
//Increment timecount accumulator |
187 | 221 |
timecount++; |
Also available in: Unified diff