Colony

//Created 4/30/06 by James Kong

#include "dragonfly_lib.h"

//#include "pindefs_ff.h"

#include "move.h"

void move (int velocity, int omega) {

  int vl = 0;

  int vr = 0;

  translateAngulartoLinear(velocity, omega, &vl , &vr );

  //

  if (vl < 0) {

    motor1_set(BACKWARD, -vl);

  } else {

    motor1_set(FORWARD, vl);

  }

  if (vr < 0) {

    motor2_set(BACKWARD, -vr);

  } else {

    motor2_set(FORWARD, vr);

  }

}

/*@strength, the strength of the avoid behavior from 0 to 100,

255 gives full control to avoid, 0 none. defines here for now*/

void move_avoid(int velocity, int omega, int strength){

  int pControl;

  int temp;

  temp=read_distance(IR1);

  d1 = (temp == -1) ? d1 : temp;

  temp=read_distance(IR2);

  d2=(temp == -1) ? d2 : temp;

  temp=read_distance(IR3);

  d3=(temp == -1) ? d3 : temp;

  temp=read_distance(IR4);

  d4=(temp == -1) ? d4 : temp;

  temp=read_distance(IR5);

  d5=(temp == -1) ? d5 : temp;

  /* Avoid obstacles ahead

  if(d2>170)

    v*=-1;

  Naturally slow down if there is something in the way.

  if(d2>150 || d1>180 || d3>180){

    v>>=1;

  */

  pControl= (((d3-d1) + (d4-d5))*strength)/100;

  omega = (omega*(100-strength))/100 + pControl;

  move(velocity, omega);

}

void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr) {

        //omega: angle measure, positive couter-clockwise from front.

        // -180 <= omega <= 180

        //velocity: -255 <= velocity <= 255

  long int vltemp, vrtemp;

  //make sure values are in bounds

  if (velocity < -255 || velocity >255 || omega < -255 || omega > 255) return;

  //compute

        vrtemp = velocity + omega * 3;

        vltemp = velocity - omega * 3;

        //check to see if max linear velocities have been exceeded.

  if (vrtemp > 255) {

    vltemp = 255 * vltemp / vrtemp;

    vrtemp = 255;

  }

  if (vltemp > 255) {

        vrtemp = 255 * vrtemp / vltemp;

        vltemp = 255;

  }

  if (vrtemp < -255) {

    vltemp = -255 * vltemp / vrtemp;

    vrtemp = -255;

  }

  if (vltemp < -255) {

    vrtemp = -255 * vrtemp / vltemp;

    vltemp = -255;

  }

  *vr = (int)vrtemp;

  *vl = (int)vltemp;

}

/*

Move and turn functions are overloaded so they can be called with or without the speed or time parameters.

If called wihtout parameters, the defaults will be used.

*/

//Forward

void moveForward(void) {

        motor1_set(FWD, NRML_SPD);

        motor2_set(FWD, NRML_SPD);

        delay_ms(DELAY_F);

}

/*void moveForward(int speed) {

        motor1_set(FWD, speed);

        motor2_set(FWD, speed);

        delay_ms(DELAY_F);

}

void moveForward(int speed, int time) {

        motor1_set(FWD, speed);

        motor2_set(FWD, speed);

        delay_ms(time);

}

*/

//Back

void moveBack(void) {

        motor1_set(BCK, HALF_SPD);

        motor2_set(BCK, HALF_SPD);

        delay_ms(DELAY_B);

}

/*

void moveBack(int speed) {

        motor1_set(BCK, speed);

        motor2_set(BCK, speed);

        delay_ms(DELAY_B);

}

void moveBack(int speed, int time) {

        motor1_set(BCK, speed);

        motor2_set(BCK, speed);

        delay_ms(time);

}

*/

//Turn Left

void turnLeft(void) {

        motor1_set(FWD, NRML_TURN);

        motor2_set(BCK, NRML_TURN);

        delay_ms(DELAY_L);

}

/*

void turnLeft(int speed) {

        motor1_set(FWD, speed);

        motor2_set(BCK, speed);

        delay_ms(DELAY_L);

}

void turnLeft(int speed, int time) {

        motor1_set(FWD, speed);

        motor2_set(BCK, speed);

        delay_ms(time);

}

*/

//Turn Right

void turnRight(void) {

        motor1_set(BCK, NRML_TURN);

        motor2_set(FWD, NRML_TURN);

        delay_ms(DELAY_R);

}

/*

void turnRight(int speed) {

        motor1_set(BCK, speed);

        motor2_set(FWD, speed);

        delay_ms(DELAY_R);

}

void turnRight(int speed, int time) {

        motor1_set(BCK, speed);

        motor2_set(FWD, speed);

        delay_ms(time);

}*/