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))
|
|
| 138 |
if(buf_index==0) {
|
|
| 139 |
if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF)){
|
|
| 128 | 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