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 <math.h>

#define LS  0

#define LD  1

#define RS  2

#define RD  3

#define FORWARD 1

#define BACKWARD 0

#define SENDER 1

#define RECEIVER 0  

#define INFINITE -1

#define L 10.0

double position[3] = {0,0,0};

void updatePosition(int vl, int vr, double dt);

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;

        while(1)

        {

                printf("SENDING DATA\n");

                encoders[LS] = tempCount;

                encoders[LD] = 1;

                encoders[RS] = tempCount;

                encoders[RD] = 0;

                //tempCount += 50;

                int temp = 0;

                int count = 0;

                do

                {

//                        tcflush(serialFile, TCIOFLUSH);

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

                short leftEncoder = (encoders[0] << 8) | encoders[1];

                short rightEncoder = (encoders[2] << 8) | encoders[3];

                printf("%d %d\n", leftEncoder, rightEncoder);

                updatePosition(leftEncoder, rightEncoder, .1);

                printf("Location is x: %g \t y: %g \t o: %g\n", position[0], position[1], position[2]);

        }

}

void updatePosition(int vl, int vr, double dt)

{

           double wdt = (vr - vl)*dt/L;

           double R;           

           if(vr - vl > 0)

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

        if(wdt != 0)

        {

                double ICC[2] = {position[0] - R * sin(position[2]), position[1] + R*cos(position[2])};

                position[0] = cos(wdt)*(position[0]-ICC[0]) - sin(wdt)*(position[1]-ICC[1]) + ICC[0];

                position[1] = sin(wdt)*(position[0]-ICC[0]) + cos(wdt)*(position[1]-ICC[1]) + ICC[1];

                position[2] = position[2] + wdt;  

        }

        else

        {

                   position[0] += cos(position[2])*vr;

                position[1] += sin(position[2])*vr;

        }

}