Colony

#include "dragonfly_lib.h"

#include "move.h"

#include "rangefinder.h"

// Internal prototypes

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

// global varaibles for move_avoid

int d1, d2, d3, d4, d5;

/**

 * @defgroup move Movement

 * @brief Functions fo controlling robot motion

 * Higher level functions to control the movement of robots.

 * 

 * @{

 **/

/**

 * Causes the robot to move with the given translation and rotational velocities.

 * motors_init must be called before this function can be used.

 *

 * @param velocity the translational velocity of the robot, in the range -255 to 255.

 * A positive value indicates forward motion, while a negative value indicates

 * backwards motion.

 * 

 * @param omega the rotational velocity of the robot, in the range -255 to 255.

 * A positive value indicates a counterclockwise velocity, while a negative

 * value indicates a clockwise velocity.

 *

 * @see motors_init, motor1_set, motor2_set

 **/

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

  }

}

/**

 * Moves the robot with the given translational and angular velocities

 * while avoiding obstacles. To be effective, this function must be

 * called repeatedly throughout the motion. It relies on the IR

 * rangefinders to detect obstacles. Before calling this function,

 * motors_init and range_init must be called.

 *

 * @param velocity the translational velocity of the robot, in the

 * range -255 to 255. A positive value indicates forward motion.

 *

 * @param omega the rotational velocity of the robot, in the range

 * -255 to 255. A positive value indicates a counterclockwise velocity.

 *

 * @param strength the strength of the avoid behavior, in the range

 * 0 to 100.

 *

 * @see motors_init, range_init, move

 **/

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

  int pControl;

  int vl = 0;

  int vr = 0;

  int temp;

  temp=range_read_distance(IR1);

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

  temp=range_read_distance(IR2);

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

  temp=range_read_distance(IR3);

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

  temp=range_read_distance(IR4);

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

  temp=range_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;

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

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

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

  }

}

/**@}**///end the motion group

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;

}