Colony

#include "hardware_wall.h"

#include <avr/io.h>

#include <avr/interrupt.h>

uint16_t wall_position; // Wall position in encoder ticks

void wall_init ()

{

    // Start the motors

    motors_init();

    motor2_set(FORWARD, 0);

    // Enable 5V switching regulator to power dynamos

    DDRB |= _BV(PB4);

    PORTB &= ~_BV(PB4);

    // Turn on the interrupts for the A quadrature

    cli();

    EICRB |= _BV(ISC60);

    EIMSK |= _BV(INT6);

        sei();

    // Enable limit switch as input

    DDRF &= ~_BV(DDF0);

    usb_puts("Moving back...\r\n");

    delay_ms(1000);

    // Move wall back until the switch is triggered

    motor2_set(FORWARD, 255);  

    while (!(PINF & _BV(PINF0)));

    motor2_set(FORWARD, 0);

    delay_ms(1000);

    usb_puts("Moving forward...\r\n");

    // Move wall forward slowly until the switch is untriggered

    motor2_set(BACKWARD, 255);

    while (PINF & _BV(PINF0));

    motor2_set(FORWARD, 0);

    usb_puts("Done with wall init\r\n");

        wall_position=0;

}

void wall_set_position (uint16_t position)

{

        usb_puts ("# Moving wall to position ");

        usb_puti (position);

        usb_puts (NL);

        // FIXME implement

        //wall_position=position+1;

        usb_puts ("# Wall position reached" NL);

}

// We might also need this because the wall_set_position might not reach the exact position

uint16_t wall_get_position ()

{

        return wall_position;

}

// Read encoder when it changes and determine our position

ISR(INT6_vect) {

    uint8_t tmp = PINE;

    if (((tmp & _BV(PE6)) << 1) ^ (tmp & _BV(PE7))) {

        wall_position--;

    } else {

        wall_position++;

    }

}