Revision 1281
Finished wall init. Calibrates it to the 0 point correctly.
| hardware_wall.c | ||
|---|---|---|
| 1 | 1 |
#include "hardware_wall.h" |
| 2 |
#include <avr/io.h> |
|
| 3 |
#include <avr/interrupt.h> |
|
| 2 | 4 |
|
| 3 |
uint16_t simulated_wall_position; // TODO remove and replace with real wall
|
|
| 5 |
uint16_t wall_position; // Wall position in encoder ticks
|
|
| 4 | 6 |
|
| 5 | 7 |
void wall_init () |
| 6 | 8 |
{
|
| 7 |
// FIXME implement |
|
| 8 |
|
|
| 9 |
simulated_wall_position=0; |
|
| 9 |
// Start the motors |
|
| 10 |
motors_init(); |
|
| 11 |
motor2_set(FORWARD, 0); |
|
| 12 |
|
|
| 13 |
// Enable 5V switching regulator to power dynamos |
|
| 14 |
DDRB |= _BV(PB4); |
|
| 15 |
PORTB &= ~_BV(PB4); |
|
| 16 |
|
|
| 17 |
// Turn on the interrupts for the A quadrature |
|
| 18 |
cli(); |
|
| 19 |
EICRB |= _BV(ISC60); |
|
| 20 |
EIMSK |= _BV(INT6); |
|
| 21 |
sei(); |
|
| 22 |
|
|
| 23 |
// Enable limit switch as input |
|
| 24 |
DDRF &= ~_BV(DDF0); |
|
| 25 |
|
|
| 26 |
usb_puts("Moving back...\r\n");
|
|
| 27 |
delay_ms(1000); |
|
| 28 |
// Move wall back until the switch is triggered |
|
| 29 |
motor2_set(FORWARD, 255); |
|
| 30 |
while (!(PINF & _BV(PINF0))); |
|
| 31 |
motor2_set(FORWARD, 0); |
|
| 32 |
|
|
| 33 |
delay_ms(1000); |
|
| 34 |
usb_puts("Moving forward...\r\n");
|
|
| 35 |
|
|
| 36 |
// Move wall forward slowly until the switch is untriggered |
|
| 37 |
motor2_set(BACKWARD, 255); |
|
| 38 |
while (PINF & _BV(PINF0)); |
|
| 39 |
motor2_set(FORWARD, 0); |
|
| 40 |
|
|
| 41 |
usb_puts("Done with wall init\r\n");
|
|
| 42 |
|
|
| 43 |
wall_position=0; |
|
| 44 |
|
|
| 10 | 45 |
} |
| 11 | 46 |
|
| 12 | 47 |
|
| ... | ... | |
| 18 | 53 |
|
| 19 | 54 |
// FIXME implement |
| 20 | 55 |
|
| 21 |
simulated_wall_position=position+1;
|
|
| 56 |
//wall_position=position+1;
|
|
| 22 | 57 |
|
| 23 | 58 |
usb_puts ("# Wall position reached" NL);
|
| 24 | 59 |
} |
| 25 | 60 |
|
| 61 |
|
|
| 26 | 62 |
// We might also need this because the wall_set_position might not reach the exact position |
| 27 | 63 |
uint16_t wall_get_position () |
| 28 | 64 |
{
|
| 29 |
// FIXME implement |
|
| 30 |
//return 0; |
|
| 31 |
|
|
| 32 |
return simulated_wall_position; |
|
| 65 |
return wall_position; |
|
| 33 | 66 |
} |
| 67 |
|
|
| 68 |
// Read encoder when it changes and determine our position |
|
| 69 |
ISR(INT6_vect) {
|
|
| 70 |
uint8_t tmp = PINE; |
|
| 71 |
|
|
| 72 |
if (((tmp & _BV(PE6)) << 1) ^ (tmp & _BV(PE7))) {
|
|
| 73 |
wall_position--; |
|
| 74 |
} else {
|
|
| 75 |
wall_position++; |
|
| 76 |
} |
|
| 77 |
} |
|
Also available in: Unified diff