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 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<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);

                }

        }

}