Revision 752
fixed lost of bugs in the libraries for spi and encoder
| branches/encoders/code/behaviors/encoder_test/main.c | ||
|---|---|---|
| 1 | 1 |
#include <dragonfly_lib.h> |
| 2 |
#define usb_puti(x) x |
|
| 2 |
//#define usb_puti(x) x
|
|
| 3 | 3 |
|
| 4 | 4 |
int main(void) |
| 5 | 5 |
{
|
| 6 | 6 |
dragonfly_init(ALL_ON); |
| 7 | 7 |
|
| 8 | 8 |
encoders_init(); |
| 9 |
//spi_init(usb_putc); |
|
| 10 |
//usb_puts("Init done.\n\r");
|
|
| 11 |
move(130,0); |
|
| 12 |
//while(1){spi_transfer(5);}
|
|
| 9 |
|
|
| 10 |
move(200,0); |
|
| 13 | 11 |
|
| 14 | 12 |
while(1){
|
| 15 | 13 |
usb_puti(encoder_get_dx(LEFT)); |
| 16 |
// usb_puts(" ");
|
|
| 14 |
usb_puts(" ");
|
|
| 17 | 15 |
usb_puti(encoder_get_dx(RIGHT)); |
| 18 |
// usb_puts("\r\n");
|
|
| 16 |
usb_puts("\r\n");
|
|
| 19 | 17 |
} |
| 20 | 18 |
|
| 21 | 19 |
} |
| branches/encoders/code/lib/src/libdragonfly/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++; |
| branches/encoders/code/lib/src/libdragonfly/spi.c | ||
|---|---|---|
| 10 | 10 |
#include <dragonfly_lib.h> |
| 11 | 11 |
#include "spi.h" |
| 12 | 12 |
|
| 13 |
|
|
| 13 | 14 |
static volatile char spi_bytes; /* number of bytes to read */ |
| 14 | 15 |
static spi_fun_recv_t spi_recv_func; /* byte handler */ |
| 15 | 16 |
static spi_fun_recv_complete_t spi_recv_complete_func; /*transmission completion handler */ |
| ... | ... | |
| 17 | 18 |
void spi_init (spi_fun_recv_t recv_func, spi_fun_recv_complete_t recv_complete_func) |
| 18 | 19 |
{
|
| 19 | 20 |
/* Enable Interrupt, Enable SPI Module, MSB First, Master Mode, Clock div = 64 */ |
| 20 |
SPCR = _BV(SPE) | _BV(SPIE) | _BV(DORD) | _BV(MSTR) | _BV(SPR1) | _BV(SPR0);
|
|
| 21 |
SPCR = _BV(SPE) | _BV(SPIE) /*| _BV(DORD)*/ | _BV(MSTR) | _BV(SPR1) | _BV(SPR0);
|
|
| 21 | 22 |
SPSR = _BV(SPI2X); |
| 22 | 23 |
|
| 23 | 24 |
/* Set SCLK, SS, MOSI as outputs. MISO as input */ |
| 24 |
DDRB = MOSI | SCLK | SS; |
|
| 25 |
DDRB |= MOSI | SCLK | SS;
|
|
| 25 | 26 |
DDRB &= ~MISO; |
| 26 | 27 |
|
| 27 | 28 |
/* Keep SS high until transmit */ |
| ... | ... | |
| 47 | 48 |
/* only handle intterupt when we are expecting data */ |
| 48 | 49 |
if(spi_bytes > 0){
|
| 49 | 50 |
/* process byte */ |
| 50 |
spi_recv_func(SPDR);
|
|
| 51 |
spi_recv_func(SPDR);
|
|
| 51 | 52 |
/* if we've read all the bytes, set SS high to end transmission, |
| 52 | 53 |
* otherwise get the next byte */ |
| 53 | 54 |
if(--spi_bytes == 0){
|
| 54 | 55 |
//usb_puts("Read all bytes\r\n");
|
| 55 | 56 |
PORTB |= SS; |
| 56 |
spi_recv_complete_func(); |
|
| 57 |
if(spi_recv_complete_func) |
|
| 58 |
spi_recv_complete_func(); |
|
| 57 | 59 |
}else {
|
| 58 | 60 |
//usb_puts("There are this many bytes left: "); usb_puti(spi_bytes);usb_puts("\r\n");
|
| 59 | 61 |
SPDR = 0xff; |
| branches/encoders/code/projects/libdragonfly/spi.c | ||
|---|---|---|
| 10 | 10 |
#include <dragonfly_lib.h> |
| 11 | 11 |
#include "spi.h" |
| 12 | 12 |
|
| 13 |
|
|
| 13 | 14 |
static volatile char spi_bytes; /* number of bytes to read */ |
| 14 | 15 |
static spi_fun_recv_t spi_recv_func; /* byte handler */ |
| 15 | 16 |
static spi_fun_recv_complete_t spi_recv_complete_func; /*transmission completion handler */ |
| ... | ... | |
| 17 | 18 |
void spi_init (spi_fun_recv_t recv_func, spi_fun_recv_complete_t recv_complete_func) |
| 18 | 19 |
{
|
| 19 | 20 |
/* Enable Interrupt, Enable SPI Module, MSB First, Master Mode, Clock div = 64 */ |
| 20 |
SPCR = _BV(SPE) | _BV(SPIE) | _BV(DORD) | _BV(MSTR) | _BV(SPR1) | _BV(SPR0);
|
|
| 21 |
SPCR = _BV(SPE) | _BV(SPIE) /*| _BV(DORD)*/ | _BV(MSTR) | _BV(SPR1) | _BV(SPR0);
|
|
| 21 | 22 |
SPSR = _BV(SPI2X); |
| 22 | 23 |
|
| 23 | 24 |
/* Set SCLK, SS, MOSI as outputs. MISO as input */ |
| 24 |
DDRB = MOSI | SCLK | SS; |
|
| 25 |
DDRB |= MOSI | SCLK | SS;
|
|
| 25 | 26 |
DDRB &= ~MISO; |
| 26 | 27 |
|
| 27 | 28 |
/* Keep SS high until transmit */ |
| ... | ... | |
| 47 | 48 |
/* only handle intterupt when we are expecting data */ |
| 48 | 49 |
if(spi_bytes > 0){
|
| 49 | 50 |
/* process byte */ |
| 50 |
spi_recv_func(SPDR);
|
|
| 51 |
spi_recv_func(SPDR);
|
|
| 51 | 52 |
/* if we've read all the bytes, set SS high to end transmission, |
| 52 | 53 |
* otherwise get the next byte */ |
| 53 | 54 |
if(--spi_bytes == 0){
|
| 54 | 55 |
//usb_puts("Read all bytes\r\n");
|
| 55 | 56 |
PORTB |= SS; |
| 56 |
spi_recv_complete_func(); |
|
| 57 |
if(spi_recv_complete_func) |
|
| 58 |
spi_recv_complete_func(); |
|
| 57 | 59 |
}else {
|
| 58 | 60 |
//usb_puts("There are this many bytes left: "); usb_puti(spi_bytes);usb_puts("\r\n");
|
| 59 | 61 |
SPDR = 0xff; |
| branches/encoders/code/projects/libdragonfly/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