Colony

#include <termios.h>

#include <stdio.h>

#include <stdlib.h>

#include <sys/types.h>

#include <sys/stat.h>

#include <fcntl.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

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;

//        attributes.c_cc[VMIN] = 1;

//        attributes.c_cc[VTIME] = 50;

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

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

                }while(count < 4);

                count = 0;

                printf("READING DATA\n");

                do

                {

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

                }while(count < 4);

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

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

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

        }

}