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>

#define PG 150

#define DG 100

#define IG 10

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 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] = {1,1,1,1};

        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 = 190;

        int dx, left_v;

        double Ul, Ur;

        while(1)

        {

                printf("SENDING DATA\n");

        velocityControl(&Ul, &Ur);

            if(Ul > 0)

                        encoders[LD] = FORWARD;

                else

                        encoders[LD] = BACKWARD;

                encoders[LS] = (unsigned char)abs(Ul);

        if(Ur > 0)

                        encoders[RD] = FORWARD;

                else

                        encoders[RD] = BACKWARD;

                encoders[RS] = (unsigned char)abs(Ur);

                int temp = 0;

                int count = 0;

                do

                {

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

                        if(temp < 0)

                                perror("Write Error");

                        count += temp;

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

                        usleep(200);

                }while(count < 4);

                count = 0;

                printf("READING DATA\n");

                do

                {

                        count += read(serialFileIn, encoders, 4);

                }while(count < 4);

                robot.vl = (encoders[0] << 8) | encoders[1];

                robot.vr = (encoders[2] << 8) | encoders[3];

                //printf("%d %d\n", robot.vl, robot.vr);

                updatePosition();

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

        }

}

void scaleInput(double* input)

{

          if(*input > 0)

        {

                (*input) += 170;

                if(*input > 210)

                        *input = 210;

        }

        else

        {

                (*input) -= 170;

                if(*input < -210)

                        *input = -210;

        }

}

void velocityControl(double* Ul, double* Ur)

{

        double vel_l_e = robot.vl_ref - robot.vl;

        double vel_r_e = robot.vr_ref - robot.vr;

        vel_l_i = vel_l_i + vel_l_e;

        vel_r_i = vel_r_i + vel_r_e;

        (*Ul) = PG * vel_l_e + DG * (vel_l_e - vel_l_old) + IG * vel_l_i;

        (*Ur) = PG * vel_r_e + DG * (vel_r_e - vel_r_old) + IG * vel_r_i;

        scaleInput(Ul);

        scaleInput(Ur);

        vel_l_old = vel_l_e;

        vel_l_old = vel_r_e;

}

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;

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

        }

}

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

void pos_control_curv(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;

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

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

}

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;

        }

    }

    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;

            break;

        }

    }

    return goal_idx;

}