Statistics
| Revision:

## root / branches / encoders / code / behaviors / encoder_test / main.c @ 771

 1 ```/* encoders_test: this behavior uses the encoders to move a given ``` ``` * distance, specified by GO where GO/1024 is the number of wheel ``` ``` * rotations. ``` ``` * It first calibrates the motors by finding the minimum reading ``` ``` * it can use to move them. ``` ``` */ ``` ```#include ``` ```//#define usb_puti(x) x ``` ```#define TARGET_V 25 ``` ```#define GO 1024 ``` ```#define K 1 ``` ```#define ABS(x) ((x>0)?x:-x) ``` ```int main(void) ``` ```{ ``` ``` int l=0,r=0, motorl=0, motorr=0; ``` ``` short int lmin=0,rmin=0; ``` ``` ``` ``` dragonfly_init(ALL_ON); ``` ``` encoders_init(); ``` ``` ``` ``` /* ``` ``` motor1_set(FORWARD,200); ``` ``` motor2_set(FORWARD,200); ``` ``` ``` ``` while(1){ ``` ``` l=encoder_get_v(LEFT); ``` ``` r=encoder_get_v(RIGHT); ``` ``` ``` ``` usb_puti(l);usb_puts(" "); ``` ``` usb_puti(r);usb_puts("\r\n"); ``` ```}*/ ``` ``` ``` ``` //calibration code: ``` ``` //TODO: this only find the min for stop->go, which is different than go->stop ``` ``` while(l<10){ ``` ``` l=encoder_get_dx(LEFT); ``` ``` motor1_set(FORWARD,++lmin); ``` ``` encoder_wait(1); ``` ``` usb_puts("."); ``` ``` } ``` ``` motor1_set(FORWARD,0); ``` ``` while(r<10){ ``` ``` r=encoder_get_dx(RIGHT); ``` ``` motor2_set(FORWARD,++rmin); ``` ``` encoder_wait(1); ``` ``` usb_puts("|"); ``` ``` } ``` ``` motor2_set(FORWARD,0); ``` ``` usb_puts("lmin: ");usb_puti(lmin); ``` ``` usb_puts(" rmin: ");usb_puti(rmin);usb_puts("\r\n"); ``` ``` ``` ``` ``` ``` //straight line forever code: ``` ``` ``` ``` motorl=lmin; ``` ``` motorr=rmin; ``` ``` ``` ``` while(1){ ``` ``` l=encoder_get_v(LEFT); ``` ``` r=encoder_get_v(RIGHT); ``` ``` ``` ``` motorl += (TARGET_V - l)*2; ``` ``` motorr += (TARGET_V - r)*2; ``` ``` ``` ``` usb_puti(motorl);usb_puts(":");usb_puti(l);usb_puts(" "); ``` ``` usb_puti(motorr);usb_puts(":");usb_puti(r);usb_puts("\r\n"); ``` ``` ``` ``` if(motorl<0) ``` ``` motorl=0; ``` ``` if(motorl>255) ``` ``` motorl=255; ``` ``` ``` ``` if(motorr<0) ``` ``` motorr=0; ``` ``` if(motorr>255) ``` ``` motorr=255; ``` ``` ``` ``` motor1_set(FORWARD,motorl); ``` ``` motor2_set(FORWARD,motorr); ``` ``` ``` ``` encoder_wait(1); ``` ``` ``` ``` } ``` ``` ``` ``` ``` ``` //go exact distance code: ``` ``` ``` ``` while(1){ ``` ``` l=encoder_get_dx(LEFT); ``` ``` r=encoder_get_dx(RIGHT); ``` ``` l = (GO-l)/K; ``` ``` r = (GO-r)/K; ``` ``` if(l>255) ``` ``` l=255; ``` ``` if(l<0) ``` ``` l=0; ``` ``` if(r>255) ``` ``` r=255; ``` ``` if(r<0) ``` ``` r=0; ``` ``` usb_puti(l);usb_puts(":");usb_puti(encoder_get_v(LEFT));usb_puts(" "); ``` ``` usb_puti(r);usb_puts(":");usb_puti(encoder_get_v(RIGHT));usb_puts("\r\n"); ``` ``` motor1_set(FORWARD,(l