Colony

#include <stdio.h>

#include <stdlib.h>

#include <math.h>

#include "motion.h"

#include "rangefinders.h"

#include "robot.h"

#define CUTOFF 120

#define TIME 1 /*sec*/

#define ROBOT_WIDTH 131 /*mm*/

#define MOTOR_CONVERSION_FACTOR 100.0

#define FUDGE 10 /* minimum rangefinder distance until collision */

/** move_robot will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.

 * (x,y) and theta will be updated by the move_robot function instead of returning a value

 * (x,y) is some kind of absolute position in the "world", (0,0) is the top left of the "world"

 * theta will an angle be between 0 and 2*Pi (0 being faces east and goes clockwise)

 * speed is between 0 and 255, there is some magical cutoff point before the motors actually starts running

 * move will return 0 if successful

 **/

int move_robot(Robot* r)

{

        Pose old_pose = r->pose;

        short speed1 = r->shared->motor1;

        short speed2 = r->shared->motor2;

        float theta = r->pose.theta;

        if (theta < 0 || theta > 2*M_PI) return 1;

        if (speed1 < -255 || speed1 > 255) return 1;

        if (speed2 < -255 || speed2 > 255) return 1;

        /* if speed is lower than the cut off, don't move */

        if (abs(speed1) < CUTOFF) {

                speed1 = 0;

        }

        if (abs(speed2) < CUTOFF) {

                speed2 = 0;

        }

        double radius;

        int divide;

        if (speed1 == speed2) {

          /* go straight */

          r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR;

          r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR;

          for (divide = 0; divide < 5; divide++) {

            /* Lets just call this a collision... */

            //printf("%d: %d\n",divide,r->shared->ranges.d[divide]);

            if (r->shared->ranges.d[divide] < FUDGE) {

                /* Restore x,y, but allow rotation */

                r->pose.x = old_pose.x;

                r->pose.y = old_pose.y;

                /* Rotated robot, need to recalculate */

                update_rangefinders(r);

                return 0;

            }

                }

          return 0;

        }

        double t;

        if (speed1 != 0)

        {

                radius = ROBOT_WIDTH * speed1 / (speed1 - speed2);

                t = speed1 / radius / MOTOR_CONVERSION_FACTOR;

        }

        else

        {

                radius = ROBOT_WIDTH * speed2 / (speed1 - speed2);

                t = speed2 / radius / MOTOR_CONVERSION_FACTOR;

        }

        double newx = (radius * sin(t)) / MOTOR_CONVERSION_FACTOR;

        double newy = (radius - radius * cos(t)) / MOTOR_CONVERSION_FACTOR;

        r->pose.x += newx * cos(theta);

        r->pose.y += newx * sin(theta);

        r->pose.x += newy * - sin(theta);

        r->pose.y += newy * cos(theta);

        divide = (t+r->pose.theta)/(2 * M_PI);

        r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide);

        if (r->pose.theta<0) r->pose.theta += 2 * M_PI;

        /* XXX: this is a terrible hack  */

        update_rangefinders(r);

        for (divide = 0; divide < 5; divide++) {

            /* Lets just call this a collision... */

            //printf("%d: %d\n",divide,r->shared->ranges.d[divide]);

            if (r->shared->ranges.d[divide] < FUDGE) {

                /* Restore x,y, but allow rotation */

                r->pose.x = old_pose.x;

                r->pose.y = old_pose.y;

                /* Rotated robot, need to recalculate */

                update_rangefinders(r);

                return 0;

            }

        }

        return 0;

}