Colony

#include "hardware_wall.h"

#include <avr/io.h>

#include <avr/interrupt.h>

volatile 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

    /* uncomment when we actually have encoders

    cli();

    EICRB |= _BV(ISC60);

    EIMSK |= _BV(INT6);

        sei();

    */

    // Enable limit switch as input

    DDRF &= ~_BV(DDF0);

    usb_puts("# Wall 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("# Wall 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)

{

    char i;

        usb_puts ("# Moving wall from position ");

        usb_puti (wall_position);

        usb_puts (NL);

        usb_puts ("# Moving wall to position ");

        usb_puti (position);

        usb_puts (NL);

    if (wall_position < position) {

        for(i = 0; i < position - wall_position; i++) {

            motor2_set(BACKWARD, 255);

            delay_ms(2700);

            motor2_set(FORWARD, 0);

        }

    } else if (wall_position > position) {

        for(i = 0; i < wall_position - position; i++) {

            motor2_set(FORWARD, 255);

            delay_ms(2700);

            motor2_set(FORWARD, 0);

            if (PINF & _BV(PINF0))

                break;

        }

    }

        wall_position=position;

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

    }

}

*/