Colony

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <encoders.h>

int delta = 16;

int divisor = 7;

int epsilon = 8;

/*

  speed initial motor speed.

  distance in encoder_get_x unit.

  Has trouble when the distance difference is greater than epsilon * divisor

  better re initiate with encoders_init() since it might overflow. or with initial difference.

  @param speed

  @param distance 30000 is roughly <10 meter

  @param time

      not yet implemented.

 */

void encoder_censored_forward(int speed, int distance, int time){

        encoders_init();

        int dist_l = encoder_get_x(LEFT);

        usb_puts("dist_l: ");

        usb_puti(dist_l);

        usb_puts("\r");

        int dist_r = encoder_get_x(RIGHT);

        usb_puts("dist_l: ");

        usb_puti(dist_l);

        usb_puts("\r");

        motor_l_set(FORWARD, speed);

        motor_r_set(FORWARD, speed);

        int spd_l = speed;

        int spd_r = speed;

        int dist_diff = 0;

        // int dist_diff_new = 0;  // was used for ensure the new speed is helping

        int count = 0; // count for time

        while (dist_l<distance && dist_r<distance){

          delay_ms(40);

          count++;

          dist_l = encoder_get_x(LEFT);

          dist_r = encoder_get_x(RIGHT);

          dist_diff = dist_l-dist_r;

          if (dist_diff< -delta) {

            spd_l += -dist_diff/divisor;

            spd_l = (spd_l >speed+epsilon)? speed+epsilon : spd_l;

            spd_r -= -dist_diff/divisor;

            spd_r = (spd_r <speed-epsilon)? speed-epsilon : spd_r;

          }else if (dist_diff > delta){

            spd_l -= dist_diff/divisor;

            spd_l = (spd_l <speed-epsilon)? speed-epsilon : spd_l;

            spd_r += dist_diff/divisor;

            spd_r = (spd_r >speed+epsilon)? speed+epsilon : spd_r;

          }

          motor_l_set(FORWARD, spd_l);

          motor_r_set(FORWARD, spd_r);

          usb_puts( "dist_l and dist_r: ");

          usb_puti(dist_l);

          usb_puts("   ");

          usb_puti(dist_r);

          usb_puts("\r");

        }

        motor_l_set(FORWARD, 0);

        motor_r_set(FORWARD, 0);

        usb_puts("time :");

        usb_puti(count *40);

}

void test_encoder(void){

          encoders_init();

        int dist_l = encoder_get_x(LEFT);

        usb_puts("dist_l: ");

        usb_puti(dist_l);

        usb_puts("\r");

        int dist_r = encoder_get_x(RIGHT);

        usb_puts("dist_r: ");

        usb_puti(dist_r);

        usb_puts("\r\r\r");

        for (int i=180;i<256; i++){

          motor_l_set(FORWARD, i);

          motor_r_set(FORWARD, i);

          delay_ms(200);

          usb_puts("\rspeed is: ");

          usb_puti(i);

          usb_puts("  speed_l  ");

          usb_puti(encoder_get_dx(LEFT));

          usb_puts("  speed_r  ");

          usb_puti(encoder_get_dx(RIGHT));

        }

}

int main(void)

{

        /* Initialize dragonfly board */

            dragonfly_init(ALL_ON);

            /* Initialize the basic wireless library */

            wl_basic_init_default();

            /* Set the XBee channel to 24 - must be standard among robots */

        wl_set_channel(24);

        encoder_censored_forward(225, 30000, 3000);

        //        test_encoder();

        while(1); /* END HERE, just in case something happened.  This way we can see the red orb. */

}