Colony

#include <termios.h>

#include <stdio.h>

#include <stdlib.h>

#include <sys/types.h>

#include <sys/stat.h>

#include <fcntl.h>

#include "internals.h"

#include <math.h>

#include "speed_map.h"

#include <time.h>

#define PG 8

#define DG .1

#define IG .1

int VEL_S = 1;

#define TICKS_PER_CM 48.03

#define TIME_INTERVAL 1520

#define LEN 30000

state robot;

double vel_l_i;

double vel_r_i;

double vel_l_old;

double vel_r_old;

void init(double x, double y, double theta)

{

        robot.x = x;

        robot.y = y;

        robot.theta = theta;

        robot.vl = 0;

        robot.vr = 0;

        robot.vl_ref = 0;

        robot.vr_ref = 0;

}

void setAttrib(int file)

{

        struct termios attributes;

            tcgetattr(file, &attributes);

        cfsetispeed(&attributes, B115200);

        cfsetospeed(&attributes, B115200);

        attributes.c_cflag &= ~PARENB;

        attributes.c_cflag &= ~CSTOPB;

        attributes.c_cflag &= ~CSIZE;

        attributes.c_cflag |=         CS8;

        attributes.c_cflag &= ~ICRNL;

        attributes.c_cflag &= ~OCRNL;

        attributes.c_cflag |= (CLOCAL | CREAD);

        attributes.c_lflag &= ~ICANON;

        if (tcsetattr(file, TCSANOW, &attributes) < 0){

                perror("tcsetattr failed");

                exit(-1);

        }

}

//#define FORWARD

#ifdef FORWARD

#undef LEFT

#undef RIGHT

#define LEFT 0

#define RIGHT 1

#endif

#define CIRCLE

//#define SINE

//#define COSINE

//#define LINE45

//#define LINE

#define RADIUS          70

#define AMP                                70

int main(int argc, char** args)

{

#ifdef CIRCLE

    init(20, 30, 0);

#endif

#ifdef SINE

        init(0, 0, M_PI/4);

#endif

#ifdef COSINE

        init(0, AMP, 0);

#endif

#ifdef LINE45

        init(0,0,0);

#endif

#ifdef LINE

        init(0,0,0);

#endif

        double x_ref[LEN];

        double y_ref[LEN];

    int i;

        for (i = 0; i < LEN; i++)  {

#ifdef CIRCLE

              x_ref[i] = RADIUS * cos((double)(i) / LEN * 2.0 * M_PI);

        y_ref[i] = RADIUS * sin((double)(i) / LEN * 2.0 * M_PI);

#endif

#ifdef SINE

                x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100;

            y_ref[i] = AMP *sin (x_ref[i] / 50);

#endif

#ifdef COSINE

                x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100;

            y_ref[i] = AMP *cos (x_ref[i] / 50);

#endif

#ifdef LINE45

            x_ref[i] = (double)i / LEN * 1000;

            y_ref[i] = (double)i / LEN * 1000;

#endif

#ifdef LINE

            x_ref[i] = (double)i / LEN * 1000;

            y_ref[i] = 0;

#endif

        //printf("%g \t%g\n", x_ref[i], y_ref[i]);

                //getchar();

        }

        //printf("aaaaaaa\n");

        curv_t ref;

        ref.x = x_ref;

        ref.y = y_ref;

        int serialFileIn = open("/dev/ttyUSB0", O_RDWR);

        int serialFileOut = open("/dev/ttyUSB1", O_RDWR);

        if(serialFileIn < 1 || serialFileOut < 1)

        {

                printf("Error opening serial\n");

                return -1;

        }

        setAttrib(serialFileOut);

        setAttrib(serialFileIn);

        //unsigned char encoders[4] = {0,0,0,0};

        char ref_vels[2] = {0,0};

        char senderNum = SENDER;

        char receiverNum = RECEIVER;

        write(serialFileIn, &receiverNum, 1);

        sleep(1);

        write(serialFileOut, &senderNum, 1);

        //Sending velocity as LS LD RS RD

        int tempCount = 0;

        int dx;

        double Ul, Ur;

        while(1) {

        pos_control_curve(&ref, LEN);

  //          ref_vels[LEFT] = 20;

  //          ref_vels[RIGHT] = 20;

                ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref);

                ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref);

        #ifdef FORWARD

                ref_vels[LEFT] = -ref_vels[LEFT];

                ref_vels[RIGHT] = -ref_vels[RIGHT];

        #endif

        //printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);

       // printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]);

                //tempCount++;

                //printf("RECEIVED %d\n", tempCount);

                int temp = 0;

                int count = 0;

                do

                {

                        temp = write(serialFileOut, ref_vels + count, 1);

                        if(temp < 0)

                                perror("Write Error");

                        count += temp;

                        //printf("sent: %d\n", count);

                        usleep(10000);

                   // sleep(1);

                } while(count < 2);

                count = 0;

                //printf("READING DATA\n");

                do

                {

                        count += read(serialFileIn, ref_vels, 2);

                }while(count < 2);

#ifndef FORWARD

                robot.vl = real_velocity(1, -ref_vels[LEFT]);

                robot.vr = real_velocity(1, -ref_vels[RIGHT]);

#else

        robot.vl = real_velocity(1, ref_vels[LEFT]);

                robot.vr = real_velocity(1, ref_vels[RIGHT]);

#endif

            //printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);

                updatePosition();

                //printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);

          printf("%g\t%g\n", robot.x, robot.y);

        }

}

//Todo: This makes no sense, 4am

void updatePosition()

{

           double wdt = (robot.vr - robot.vl)*DT/L;

           double R;

           if(robot.vr - robot.vl != 0)

                   R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);

        if(wdt != 0)

        {

                double ICC[2] = {robot.x - R * sin(robot.theta), robot.y + R*cos(robot.theta)};

                robot.x = (cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0]);

                robot.y = (sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1]);

                robot.theta = robot.theta + wdt;

        }

        else

        {

                   robot.x += cos(robot.theta)*robot.vr*DT;

                robot.y += sin(robot.theta)*robot.vr*DT;

        }

        if (robot.theta > 2*M_PI)

                robot.theta -= 2*M_PI;

        if (robot.theta < -2*M_PI)

                robot.theta += 2*M_PI;

}

static int find_goal(curv_t *ref, int len);

void pos_control_curve(curv_t *ref, int len) {

    int goal_idx = find_goal(ref, len);

    //printf("\tGOAL: x: %g,  y: %g\n", ref->x[goal_idx], ref->y[goal_idx]);

    //did a transformation here.

    double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y)

        - sin(robot.theta)*(ref->x[goal_idx]-robot.x);

    double goal_rad = 2*goal_y / LOOK_AHEAD;

    robot.vl_ref = real_velocity(1, VELOCITY)*(1-0.5*goal_rad);

    robot.vr_ref = real_velocity(1, VELOCITY)*(1+0.5*goal_rad);

    /*

        if(robot.vl_ref > 127)

                robot.vl_ref = 127;

        else if(robot.vl_ref < -128)

                robot.vl_ref = -128;

        if(robot.vr_ref > 127)

                robot.vr_ref = 127;

        else if(robot.vr_ref < -128)

                robot.vr_ref = -128;

                */

}

static int find_goal(curv_t *ref, int len) {

    int i, goal_idx;

    double min_dist = 100000000;

    double temp, err;

    int min_idx;

        int ctr = 0;

    for (i = 0; i < len; i++) {

        temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);

        if (temp < min_dist) {

            min_dist = temp;

            min_idx = i;

        }

    }

        /*

    if (min_dist > LOOK_AHEAD) {

                printf("toooooo far away \n");

                return min_idx;

        }

    */

        //printf("%d min %f\n", min_idx, min_dist);

        //getchar();

    goal_idx = -1;

        err = LOOK_AHEAD_ERR_THRES;

        while(ctr < 3) {

            for (i = min_idx; i < len; i++) {

#ifdef CIRCLE

                        if (i == len-1)

                                i = 0;

#endif

                temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);

                if (abs(temp - LOOK_AHEAD) < err) {

                    goal_idx = i;

                        return goal_idx;

                        }

            }

                err += 5;

                ctr++;

        }

        while(1) {

                printf("eol\n");

            exit(0);

        }

    return goal_idx;

}