Colony

#include <termios.h>

#include <stdio.h>

#include <stdlib.h>

#include <sys/types.h>

#include <sys/stat.h>

#include <fcntl.h>

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

int left_dx = 0;

int right_dx = 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;

        while(1)

        {

                printf("SENDING DATA\n");

                encoders[LS] = tempCount;

                encoders[LD] = 0;

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

//                encoder_recv(leftEncoder, rightEncoder);

//                printf("LEFT V: %d \t RIGHT V: %d\n", encoder_get_v(LEFT), encoder_get_v(RIGHT));

        }

}