Colony

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file move.c

 * @brief Functions for moving

 *

 * Implementation of functions for moving the robot.

 *

 * @author Colony Project, CMU Robotics Club

 **/

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

}