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;

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

  }

}

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

  }

}

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;

}