Colony

#include <stdio.h>

#include <termios.h>

#include <fcntl.h>

#include <stdlib.h>

#include <unistd.h>

#include <sys/ioctl.h>

int setupSerial(int fd, int baud)

{

        struct termios options;

    /* set baud rate, etc. correctly */

        tcgetattr(fd, &options);

        cfsetispeed(&options, baud);

        cfsetospeed(&options, baud);

        options.c_iflag &= ~ICRNL;

        options.c_oflag &= ~OCRNL;

        options.c_cflag |= (CLOCAL | CREAD);

        options.c_cflag &= ~PARENB;

        options.c_cflag &= ~CSTOPB;

        options.c_cflag &= ~CSIZE;

        options.c_cflag |= CS8;

        options.c_lflag &= ~ICANON;

        options.c_cc[VMIN] = 1;

        options.c_cc[VTIME] = 50;

        if (tcsetattr(fd, TCSANOW, &options))

        {

                printf("Error setting attributes.\n");

                return -1;

        }

    return 0;

}

int initSerial(char* portName)

{

    /* open port for I/O */

    int fd = open(portName, O_RDWR | O_NOCTTY ); 

    if (fd <0)

    {

        perror(portName);

        return -1;

    }

    return fd;

}

int writeSerial(int h, const char buf[], int length)

{

    return write(h,buf,length);

}

int writeByte(int h, char buf)

{

    return writeSerial(h, &buf, 1);

}

int getAvailBytes(int h)

{

    int bytes;

    ioctl(h, FIONREAD, &bytes);

    return bytes;

}

int readSerial(int h, char buf[], int length)

{

    return read(h,buf,length);

}

int readByte(int h)

{

    char buf;

    if (readSerial(h, &buf, 1)<1)

        return -1;

    return buf;

}

void closeSerial(int h)

{

    close(h);

}

int wait_for_ok(int h)

{

    const char* s = "OK\r";

    const char* curr = s;

    int count = 0;

    while (curr - s < 3)

    {

        int c;

        while(!getAvailBytes(h))

        {

            if (count++ > 150) return 0;

            usleep(10000);

        }

        c = readByte(h);

        if (c>=0)

        {

            if (c == *curr) {

                curr++;

            } else {

                curr = s;

            }

        }

    }

    return 1;

}

int xbee_setup(int h, int robotNum)

{

    int _init = 0;

    char idstr[9] = "ATIDFF";

    setupSerial(h, B9600);

    writeSerial(h, "+++", 3);

    if (wait_for_ok(h))

    {

        printf("Initializing XBee\n");

        writeSerial(h, "ATCH0C\r", 7);

        wait_for_ok(h);

        idstr[6] = (robotNum / 10) + 0x30;

        idstr[7] = robotNum % 10 + 0x30;

        idstr[8] = '\r';

        writeSerial(h, idstr, 9);

        wait_for_ok(h);

        writeSerial(h, "ATDH0\r", 6);

        wait_for_ok(h);

        writeSerial(h, "ATDLFFFF\r", 9);

        wait_for_ok(h);

        writeSerial(h, "ATAP0\r", 6);

        wait_for_ok(h);

        writeSerial(h, "ATBD6\r", 6);

        wait_for_ok(h);

        writeSerial(h, "ATCN\r", 5);

        wait_for_ok(h);

        _init = 1;

    }

    setupSerial(h, B57600);

    if (!_init)

    {

        printf("Configuring XBee\n");

        writeSerial(h, "+++", 3);

        wait_for_ok(h);

        idstr[6] = (robotNum / 10) + 0x30;

        idstr[7] = robotNum % 10 + 0x30;

        idstr[8] = '\r';

        writeSerial(h, idstr, 9);

        wait_for_ok(h);

        writeSerial(h, "ATCN\r", 5);

        wait_for_ok(h);

    }

    return 0;

}

/* Take in an intel hex file and load it into a large array

 * This allows us to read sequential data from the large array without having to

 * worry about weird character line-feed breaks in the text/HEX file.

 * 

 * Takes: file name?

 * Returns: Big array

 * 

 * For ATmega168, we have to load 128 bytes at a time (program full page). Document: AVR095

 */

#define MEM_SIZE 131072

unsigned char intel_hex_array[MEM_SIZE];

unsigned int Hex_Convert(char str[3])

{

    unsigned int hex_num;

    char hstr[5] = { '0', 'x', 0, 0, 0 };

    hstr[2] = str[0];

    hstr[3] = str[1];

    sscanf(hstr, "%x", &hex_num);

    return hex_num;

}

int parse_file(char* file)

{

    int i;

    char buffer[3];

        /* Read in the HEX file - clean it up */

        FILE* fnum = fopen(file, "r");

    if (fnum == NULL)

        return 1;

        /* Pre-fill hex_array with 0xFF */

        for (i=0; i < MEM_SIZE; i++)

    {

                intel_hex_array[i] = 0xff;

        }

        /* Step through cleaned file and load file contents into large array

         * Also convert from ASCII to binary */

        for(;;)

    {

        int Record_Length, Memory_Address_High, Memory_Address_Low, Memory_Address, End_Record;

        /* Peel off first character */

        int first = fgetc(fnum);

        /* Remove empty lines + lines not starting with a ":" */

        if (first == EOF) break;

                if (first != ':') continue;

                /* Peel off record length */

                Record_Length = Hex_Convert(fgets(buffer, 3, fnum));

                /* Get the memory address of this line */

                Memory_Address_High = Hex_Convert(fgets(buffer, 3, fnum));

                Memory_Address_Low = Hex_Convert(fgets(buffer, 3, fnum));

                Memory_Address = (Memory_Address_High << 8) + Memory_Address_Low;

                /*Divide Memory address by 2

                         If (Memory_Address_High Mod 2) <> 0 Then

                             Memory_Address_Low = Memory_Address_Low + 256

                         End If

                         Memory_Address_High = Memory_Address_High \ 2

                         Memory_Address_Low = Memory_Address_Low \ 2*/

                /* Peel off and check for end of file tage */

                End_Record = Hex_Convert(fgets(buffer, 3, fnum));

        if (End_Record == 04 ) continue;

                if (End_Record == 01) break;

                for (i=0; i < Record_Length; i++)

        {

                        /* Pull out the byte and store it in the memory_address location of the intel_hex array */

                        intel_hex_array[Memory_Address + i] = Hex_Convert(fgets(buffer, 3, fnum));

                }

        }

    fclose(fnum);

        return 0;

}

int download(int h, char *file)

{

        int j;

        int Memory_Address_High;

        int Memory_Address_Low;

        int Check_Sum;

    char buf;

        int last_memory_address;

        int current_memory_address;

        int page_size = 256; /* For ATmega168, we have to load 128 bytes at a time (program full page). Document: AVR095 */

        /* Take intel hex file and store it squentially into large array */

        if (parse_file(file))

    {

                fprintf(stderr, "Whoa - problem parsing file!\n");

                return -1;

        }

        printf("Waiting for target IC to broadcast\n");

        /* "Send the reset signal"; */

        while(!getAvailBytes(h))

    {

        usleep(1500000);

        writeByte(h, 6);

    }

        printf("Load Mode Notification Received\n");

        /* Find the last spot in the large hex_array */

        last_memory_address = MEM_SIZE - 1;

        while(last_memory_address > 0)

    {

                if (intel_hex_array[last_memory_address] != 255) break;

                last_memory_address--;

        }

        /* Start at beginning of large intel array */

        current_memory_address = 0;

        while (current_memory_address < last_memory_address)

    {

        while((buf = readByte(h))==-1)

            usleep(1000);

                if (buf == 'T') /* All is well */

        {

                        /* Total_Code_Words = Total_Code_Words + (Record_Length / 2) */

        }

                else if (buf == 7)  /* Re-send line */

        {

            static int resend_count = 0;

                        current_memory_address = current_memory_address - page_size;

            if (current_memory_address<0) current_memory_address=0;

            printf("\nResending: %d\n", ++resend_count);

                        usleep(1000);

        }

                else

        {

                        fprintf(stderr, "\nError : Incorrect response from target IC: %d. Programming is incomplete and will now halt.\n", buf);

                        return -2;

                }

                /* Update progress */

                printf("\rLoaded %d code words", current_memory_address);

        fflush(stdout);

                /*=============================================*/

                /* Convert 16-bit current_memory_address into two 8-bit characters */

                Memory_Address_High = current_memory_address >> 8;

                Memory_Address_Low = current_memory_address & 0xff;

                /*=============================================*/

                /* Calculate current check_sum */

                Check_Sum = 0;

                Check_Sum += page_size >> 8;    /* page size high byte */

        Check_Sum += page_size & 0xff;  /* page size low byte */

                Check_Sum += Memory_Address_High; /* Convert high byte */

                Check_Sum += Memory_Address_Low; /* Convert low byte */

                for(j=0; j < page_size; j++)

        {

                        Check_Sum += intel_hex_array[current_memory_address + j];

                }

                /* Now reduce check_sum to 8 bits */

                Check_Sum &= 0xff;

                /* Now take 2's compliment */

                Check_Sum = 256 - Check_Sum;

                /*=============================================*/

                /* Send the start character */

        writeByte(h, ':');

                /* Send the record length */

        writeByte(h, page_size & 0xff); /* low byte */

        writeByte(h, page_size >> 8);   /* high byte */

                /* Send this block's address */

        writeByte(h, Memory_Address_Low);

        writeByte(h, Memory_Address_High);

                /* Send this block's check sum */

        writeByte(h, Check_Sum);

                /* Send the block */

                for (j=0; j < page_size; j++)

        {

                        writeByte(h, intel_hex_array[current_memory_address + j]);

                }

                /*=============================================*/

                current_memory_address += page_size;

        }

        /* Tell the target IC we are done transmitting data */

    writeByte(h, ':');

    writeByte(h, 'S');

    writeByte(h, 'S');

        printf("\nDownload Complete!\n");

    return 0;

}

int main(int argc, char* argv[])

{

    int h;

    printf("//=\\\\     _                          __               \n");

    printf("||  .    |_) _ |_  _  _  |    |_    /   _  |  _ __  \\/\n");

    printf("|| -A!   | \\(_)|_)(_)(_  | |_||_)   \\__(_) | (_)| | / \n");

    printf("                                                      \n");

    printf("                 Universal Programmer                 \n");

    printf("                                                      \n");

    switch(argc)

    {

    case 3:

        /* open port for I/O */

        h = initSerial(argv[1]);

        setupSerial(h, B57600);

    break;

    case 4:

        h = initSerial(argv[1]);

        xbee_setup(h, atoi(argv[3]));        

    break;

    default:

        printf("Usage: %s <port name> <hex file> [<robot ID>]\n", argv[0]);

        return -1;

    }

    int ret = download(h, argv[2]);

    closeSerial(h);

    return ret;

}