Colony

#include <termios.h>

#include <stdio.h>

#include <stdlib.h>

#include <sys/types.h>

#include <sys/stat.h>

#include <fcntl.h>

#include "encoders.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);

        }

}

int main(int argc, char** args)

{

    init(50, 0, M_PI/2);

        //init(0,0,0);

        double x_ref[LEN];

        double y_ref[LEN];

    int i;

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

            x_ref[i] = 50 * cos((double)(i+1) / LEN * M_PI);

            y_ref[i] = 50 * sin((double)(i+1) / LEN * M_PI);

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

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

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

                //getchar();

        }

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

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

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

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

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

        }

        return 0;

}

void scaleInput(double* input)

{

        printf("INPUT: %g\n", *input);

          if(*input > 0)

        {

                //(*input) += 170;

                if(*input > 210)

                        *input = 210;

        }

        else if((*input) < 0)

        {

                //(*input) -= 170;

                if(*input < -210)

                        *input = -210;

        }

}

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

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

    //printf("GOAL: y: %g,  o: %g\n", goal_y, goal_rad);

    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;

    int min_idx;

    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;

        }

    }

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

        //getchar();

    goal_idx = -1;

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

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

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

            goal_idx = i;

                return goal_idx;

                }

    }

        while(1) {

                printf("eol\n");

            exit(0);

        }

    return goal_idx;

}