Colony

#include <dragonfly_lib.h>

/* init_dragonfly - Initializes functions based on configuration parameters

   examples:

   init_dragonfly (0, 0, 0); - just initialize the digital IO (buttons, potentiometer)

   init_dragonfly (ANALOG | SERIAL | BUZZER, C0_C1_ANALOG, BAUD115200); 

   Initialize ADC and set C0 and C1 as analog inputs.  Initialize serial and set baud rate

   to 115200 bps.  Initialize the buzzer.

   init_dragonfly (MOTORS | ORB, 0, 0);

   Initialize motor driving and the color fading abilities of the ORB. */

void dragonfly_init(int config) 

{

        // Set directionality of various IO pins

        DDRG &= ~(_BV(PING0)|_BV(PING1));

        PORTG |= _BV(PING0)|_BV(PING1);

        if(config & ANALOG) {

                analog_init();

        }

        if(config & COMM) {

    //Defaults to 115200. Check serial.h for more information.

    sei();

    usb_init();

    xbee_init();

        }

        if(config & BUZZER) {

    sei();

                buzzer_init();

        }

        if(config & ORB) {

    sei();

                orb_init();

        }

        if(config & MOTORS) {

                motors_init();

        }

        if(config & SERVOS) {

                sei();

    //Servos not yet implemented

    //servo_init();

        }

        if(config & LCD) {

                lcd_init();

        }

        if(config & (SERVOS | ORB)) {

    sei();

    orb_init();

    //Servos not yet implemented

        }

        // delay a bit for stability

        _delay_ms(1);

}

void adc_gain_test(void);

void analog_test(void);

void batt_test(void);

void button_test(void);

void buzzer_test(void);

void dio_test(void);

void general_test(void);

void i2c_test(void);

void lcd_test(void);

void motor_test(void);

void orb_test(void);

void pullup_test(void);

void ranger_test(void);

void reset_test(void);

void ring_buffer_tester(void);

void rtc_test(void);

void rtc_test_pulse(void);

void serial_test(void);

void servo_orb_test(void);

void wl_receive_test(void);

void wl_send_test(void);

void wl_send_packet_test(void);

void move_test(void);

void move_test(void){

  range_init();

  motors_init();

  analog_init();

  while(1){

    move_avoid(200,0,50 );

  }

}  

void general_test(){

        int i;

        dragonfly_init(MOTORS|BUZZER|ORB|USB);

        while(1){

                if(button1_read()){

                        for(i=0;i<17;i++){

                                buzzer_set_val(i);

                                delay_ms(100);

                        }

                        buzzer_off();

                        motor1_set(1, 255);

                        motor2_set(1, 255);

                        delay_ms(3000);

                        motor1_set(1, 200);

                        delay_ms(1000);

                        motor1_set(0, 255);

                        motor2_set(0, 255);

                        delay_ms(3000);

                        motors_off();

                }

                if(button2_read()){

                        orb_enable();

                        orb1_set_color(RED);

                        delay_ms(500);

                        orb2_set_color(GREEN);

                        delay_ms(500);

                        orb1_set_color(YELLOW);

                        delay_ms(500);

                        orb2_set_color(CYAN);

                        delay_ms(500);

                        orb1_set_color(WHITE);

                        delay_ms(500);

                        orb2_set_color(WHITE);

                        delay_ms(500);

                        orb_disable();

                }

        }

}

void reset_test() {

    dragonfly_init(ORB|SERIAL);

    delay_ms(1000);

    usb_puti((int)DDRE);

    orb_set_color(RED);

    delay_ms(1000);

    orb_set_color(GREEN);

    delay_ms(1000);

    DDRE |= 0x80;

    reset();

    /*

    alternate implelmentation using external interrupts

    maintains register values on reset

    sei();

    EIMSK |= 0x80;

    EICRB |= 0xC0;

    DDRE |= 0x80;

    PORTE &= 0x7F;

    PORTE |= 0x80;

    */

    /*

    should not reach here

    */

    orb_set_color(BLUE);

    delay_ms(1000);

}

void ring_buffer_tester (void) {

        dragonfly_init(ALL_ON & ~ORB);

        orb_disable();

        RING_BUFFER_NEW(ring_buffer, 12, int, buffer);

        RING_BUFFER_INIT(buffer, 12);

        RING_BUFFER_CLEAR(buffer);

        for(int i=0;i<10;i++)

                RING_BUFFER_ADD(buffer, 0);

        int x;

        for(int i = 0; i < 100; i++) {

                RING_BUFFER_REMOVE(buffer, x);

                RING_BUFFER_ADD(buffer, i);

                usb_puti(i-x);

                usb_putc(' ');

        }

        usb_puts("\n\rfull? " );

        usb_puti(RING_BUFFER_FULL(buffer));

        while(!RING_BUFFER_FULL(buffer))

                RING_BUFFER_ADD(buffer, 0);

        usb_puts("\n\rfilling... full? " );

        usb_puti(RING_BUFFER_FULL(buffer));

        usb_puts("\n\rempty? " );

        usb_puti(RING_BUFFER_EMPTY(buffer));

        usb_puts("\n\rclearing... empty? ");

        RING_BUFFER_CLEAR(buffer);

        usb_puti(RING_BUFFER_EMPTY(buffer));

        usb_puts("\n\r");

}

#define WL_BAUD BAUD115200

//serial -- XBEE

//serial1 -- USB

void wl_send_test(void) {

        int i;

        usb_init();                //WIRED

        xbee_init();        //WIRELESS

        orb_init();

        sei();

        while(1) {

                for(i = 32; i <= 126; i++) {

                        if(i%4) {

                                orb1_set(255, 0, 0);

                        }

                        else {

                                orb1_set(0,0,0);

                        }

                        usb_putc(i);                //wired

                        xbee_putc(i);                //wireless

                        delay_ms(100);

                }

        }

        return;

}

void wl_send_packet_test(void) {

        int i;

        usb_init();                //WIRED

        xbee_init();        //WIRELESS

        orb_init();

        sei();

        while(1) {

                for(i = 65; i <= 122; i++) {

                        usb_putc(i);                //wired

                        xbee_putc(i);                //wireless

      //delay_ms(100);

                }

    delay_ms(1000);

        }

        return;

}

volatile char c;

volatile char haveNew = 0;

void wl_receive_test(void) {

        usb_init();

        xbee_init();

        orb_init();

        c = '.';

  xbee_putc('r');

        UCSR1B |= _BV(RXCIE);

        UCSR0B |= _BV(RXCIE);

        sei();

        orb1_set(255, 0, 0);                //blue

        while(1)         {

                if(haveNew) {

                        xbee_putc(c);

                        haveNew = 0;

                }

        }

        orb1_set(0, 0, 255);                //green

        while(1);

}

/*

ISR(USART1_RX_vect) {

        c = UDR1;

        haveNew = 1;

}

ISR(USART0_RX_vect) {

        c = UDR0;

        haveNew = 1;

}

*/

void buzzer_test(void)

{

        sei();

        buzzer_init();

        unsigned int i = 0;

        while(1) {

                buzzer_set_val(i);

                if (i == 20) {

                        i = 0;

                        delay_ms(250);

                } else {

                        i++;

                }

                delay_ms(100);

        }

}

void adc_gain_test(void)

{

  analog_init();

  lcd_init();

  while(1)

  {

    lcd_putint(ADCH);

    delay_ms(100);

    lcd_clear_screen();

  }

}

void ranger_test( void )

{

        lcd_init();

        while(1) {

                lcd_clear_screen();

                lcd_putint(linearize_distance(read_distance(PIN_C0)));

                delay_ms(100);

        }

}

void button_test ( void) 

{

    dragonfly_init(ALL_ON);

    move(200,0);

        while(1) {

                if(button1_read()) {

                        //lcd_putchar('1');

                        usb_putc('1');

                } else if(button2_read()) {

                        //lcd_putchar('2');

                        usb_putc('2');

                }

        }

}

void motor_test (void) {

        motors_init();

        //led_init();

        //motor1_set(1, 255);

        //motor2_set(1, 255);

        move(200,0);

        delay_ms(5000);

        //motor1_set(0, 200);

        move(200,100);

        delay_ms(5000);

        //motor2_set(0, 170);

        move(200,0);

        delay_ms(5000);

        motors_off();

        //LED USER NO LONGER EXISTS

        //led_user(1);

        while(1);

}

void orb_test( void ) {

        int orbs[12] = { ORB_OFF, RED, ORANGE, YELLOW, LIME, GREEN, CYAN, BLUE, PINK, PURPLE, MAGENTA, WHITE };

        int orb_select = 0;

        lcd_init();

        orb_init();

        sei();

        orb2_set(254, 253, 252);

        orb1_set(251, 250, 249);

        while(1) {

                orb_set_color(orbs[orb_select]);

                /*orb_set1(251, 250, 249);

                delay_ms(2000);

                orb_set1(255, 1,2);

                delay_ms(1000);

                orb_set1(1, 255, 2);

                delay_ms(1000);

                orb_set1(1, 2, 255);

                */

                delay_ms(3000);

                orb_select++;

                if(orb_select == 13) {

                        orb_disable();

                        delay_ms(2000);

                        orb_enable();

                        orb_select = 0;

                }

        /*

                for (int i = 0; i < 256; i++) {

                        for (int j = 0; j < 256; j++) {

                                for (int k = 0; k < 256; k++) {

                                        orb_set(i, j, k);

                                }

                        }

                }

        */

        }

        return;

}

void servo_orb_test( void ) {

//        int orbs[13] = { ORB_OFF, RED, ORANGE, YELLOW, LIME, GREEN, CYAN, BLUE, PURPLE, PINK, PURPLE, MAGENTA, WHITE };

        int orb_select = 0;

        orb_init();

        sei();

        orb_set_angle(1,250);

        orb_set_angle(2,250);

        orb_set_angle(4,0);

        orb_set_angle(5,0);

        orb_set_angle(6,0);

        while(1) {

                //orb_set_color(orbs[orb_select]);

                orb_set_angle(0,orb_select+1);

        //        orb_set_angle(1,orb_select+1);

        //        orb_set_angle(2,orb_select+1);

                delay_ms(50);

                orb_select++;

                if(orb_select == 255) {

                        orb_select = 0;

                }

        /*

                for (int i = 0; i < 256; i++) {

                        for (int j = 0; j < 256; j++) {

                                for (int k = 0; k < 256; k++) {

                                        orb_set(i, j, k);

                                }

                        }

                }

        */

        }

        return;

}

void lcd_test(void) {

        int i, j;

        lcd_init();

        for(i = 32,j = 1; i <= 126; i++) {

                lcd_putchar(i);

                delay_ms(250);

        if(j > 50) {

            lcd_clear_screen();

            j = 0;

        }

        j++;

        }

        return;

}

void serial_test( void ) {

        int i;

        usb_init();

        for(i = 32; i <= 126; i++) {

                usb_putc(i);

                delay_ms(250);

        }

        return;

}

void pullup_test()

{

        lcd_init();

        lcd_putstr("pullup test");

        delay_ms(100);

        digital_pull_up(PIN_A4);

        digital_pull_up(PIN_A5);

        digital_pull_up(PIN_A6);

        digital_pull_up(PIN_A7);

        while(1){

                lcd_clear_screen();

                if(!digital_input(PIN_A4))

                        lcd_putchar('0');

                else

                        lcd_putchar(' ');

                if(!digital_input(PIN_A5))

                        lcd_putchar('1');

                else

                        lcd_putchar(' ');

                if(!digital_input(PIN_A6))

                        lcd_putchar('2');

                else

                        lcd_putchar(' ');

                if(!digital_input(PIN_A7))

                        lcd_putchar('3');

                else

                        lcd_putchar(' ');

                delay_ms(100);

        }

}

void batt_test()

{

        lcd_init();

  usb_init();

        analog_init();

        while(1){

    usb_puti(battery8());

    usb_putc(' ');

                delay_ms(10000);

        }

}

void analog_test(void){

  //int i;

  analog_init();

  usb_init();

  range_init();

  usb_puts(" information:\n");

  while(1){

    printAllBoms();

    /*for(i=0;i<16;i++){

      digital_output(MONK0,i&1);

      digital_output(MONK1, (i>>1)&1);

      digital_output(MONK2, (i>>2)&1);

      digital_output(MONK3, (i>>3)&1);

      usb_puts("Bom ");

      usb_puti(i);

      usb_puts(": ");

      usb_puti(analog8(MONKI));

      usb_puts("\n\r");

    }*/

    /*for(i=1;i<8;i++){

      usb_puts("Analog Ext ");

      usb_puti(i);

      usb_puts(":  ");

      usb_puti(analog8(i));

      usb_puts("\n\r");

    }

    usb_puts("IR1 val: ");

    usb_puti(read_distance(IR1));

    usb_puts("\n\rIR2 val: ");

    usb_puti(read_distance(IR2));

    usb_puts("\n\rIR3 val: ");

    usb_puti(read_distance(IR3));

    usb_puts("\n\rIR4 val: ");

    usb_puti(read_distance(IR4));

    usb_puts("\n\rIR5 val: ");

    usb_puti(read_distance(IR5));

    */

    delay_ms(2000);

  }

}

void dio_test()

{

        while(1)

        {

                digital_output(_PIN_E5,1);

                delay_ms(1000);

                digital_output(_PIN_E5,0);

                delay_ms(1000);

        }

}

void rtc_test_pulse() {

        usb_putc('s');

}

void rtc_test() {

        sei();

        usb_init();

        usb_putc('a');

        rtc_init(HALF_SECOND, &rtc_test_pulse);

}

#define RECEIVE 1

#define MY_ADDR (RECEIVE == 1 ? 0x01 : 0x02)

#define TARGET_ADDR (RECEIVE == 1 ? 0x02 : 0x01)

void i2c_test() {

  char c = 0;

  char i = 0;

  sei();

  i2c_init(MY_ADDR);

  usb_init();

  usb_putc('5');

  delay_ms(1000);

  while(1) {

    if (RECEIVE) {

      while(c != 'z') {

        if(!i2c_getc(&c)) {

          usb_putc(c);

          usb_putc(' ');

        }

      }

    } else {

    while(i < 26) {

      if(!i2c_putc(TARGET_ADDR, i + 0x61))

        i++;

      }

    }

    delay_ms(1000);

    if (!RECEIVE) {

      while(c != 'z') {

        if(!i2c_getc(&c)) {

          usb_putc(c);

          usb_putc(' ');

        }

      }

    } else {

      while(i < 26) {

        if(!i2c_putc(TARGET_ADDR, i + 0x61))

          i++;

      }

    }

    c = 0;

    i = 0;

  }

  while(1);

}