Colony

/* 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 <dragonfly_lib.h>

//#define usb_puti(x) x

#define GO 2048

#define K 1

#define ABS(x) ((x>0)?x:-x)

int main(void)

{

  int l=0,r=0;

  short int lmin=0,rmin=0;

        dragonfly_init(ALL_ON);

        encoders_init();

        while(l<10){

          l=encoder_get_dx(LEFT);

          motor1_set(FORWARD,++lmin);

          delay_ms(1);

        }

        motor1_set(FORWARD,0);

        while(r<10){

          r=encoder_get_dx(RIGHT);

          motor2_set(FORWARD,++rmin);

          delay_ms(1);

        }

        motor2_set(FORWARD,0);

        usb_puts("lmin: ");usb_puti(lmin);

        usb_puts(" rmin: ");usb_puti(rmin);usb_puts("\r\n");

        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(r);usb_puts("\r\n");

                motor1_set(FORWARD,(l<lmin)?(lmin+5):l);

                motor2_set(FORWARD,(r<rmin)?(rmin+5):r);

                if(ABS(r)<2 && ABS(l)<2){

                  move(0,0);

                  usb_puts("done!\r\n");

                  while(1);

                }

        }

}