root / trunk / code / projects / diagnostic_station / station / hardware_wall.c @ 1900
History | View | Annotate | Download (2.29 KB)
1 | 1183 | deffi | #include "hardware_wall.h" |
---|---|---|---|
2 | 1281 | kwoo | #include <avr/io.h> |
3 | #include <avr/interrupt.h> |
||
4 | 1183 | deffi | |
5 | 1285 | kwoo | volatile uint16_t wall_position; // Wall position in encoder ticks |
6 | 1213 | deffi | |
7 | 1183 | deffi | void wall_init ()
|
8 | { |
||
9 | 1281 | kwoo | // 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 | 1285 | kwoo | /* uncomment when we actually have encoders
|
19 | 1281 | kwoo | cli();
|
20 | EICRB |= _BV(ISC60);
|
||
21 | EIMSK |= _BV(INT6);
|
||
22 | sei();
|
||
23 | 1285 | kwoo | */
|
24 | 1281 | kwoo | // Enable limit switch as input
|
25 | DDRF &= ~_BV(DDF0); |
||
26 | |||
27 | 1288 | deffi | usb_puts("# Wall moving back...\r\n");
|
28 | 1281 | kwoo | delay_ms(1000);
|
29 | // Move wall back until the switch is triggered
|
||
30 | motor2_set(FORWARD, 255);
|
||
31 | while (!(PINF & _BV(PINF0)));
|
||
32 | motor2_set(FORWARD, 0);
|
||
33 | |||
34 | delay_ms(1000);
|
||
35 | 1288 | deffi | usb_puts("# Wall moving forward...\r\n");
|
36 | 1281 | kwoo | |
37 | // Move wall forward slowly until the switch is untriggered
|
||
38 | motor2_set(BACKWARD, 255);
|
||
39 | while (PINF & _BV(PINF0));
|
||
40 | motor2_set(FORWARD, 0);
|
||
41 | |||
42 | 1288 | deffi | usb_puts("# Done with wall init\r\n");
|
43 | 1281 | kwoo | |
44 | 1285 | kwoo | wall_position = 0;
|
45 | 1183 | deffi | } |
46 | |||
47 | 1205 | deffi | |
48 | 1183 | deffi | void wall_set_position (uint16_t position)
|
49 | { |
||
50 | 1285 | kwoo | char i;
|
51 | usb_puts ("# Moving wall from position ");
|
||
52 | usb_puti (wall_position); |
||
53 | usb_puts (NL); |
||
54 | 1183 | deffi | usb_puts ("# Moving wall to position ");
|
55 | usb_puti (position); |
||
56 | usb_puts (NL); |
||
57 | |||
58 | 1285 | kwoo | if (wall_position < position) {
|
59 | for(i = 0; i < position - wall_position; i++) { |
||
60 | motor2_set(BACKWARD, 255);
|
||
61 | delay_ms(2700);
|
||
62 | motor2_set(FORWARD, 0);
|
||
63 | } |
||
64 | } else if (wall_position > position) { |
||
65 | for(i = 0; i < wall_position - position; i++) { |
||
66 | motor2_set(FORWARD, 255);
|
||
67 | delay_ms(2700);
|
||
68 | motor2_set(FORWARD, 0);
|
||
69 | if (PINF & _BV(PINF0))
|
||
70 | break;
|
||
71 | } |
||
72 | |||
73 | } |
||
74 | |||
75 | wall_position=position; |
||
76 | 1213 | deffi | usb_puts ("# Wall position reached" NL);
|
77 | 1183 | deffi | } |
78 | |||
79 | 1281 | kwoo | |
80 | 1183 | deffi | // We might also need this because the wall_set_position might not reach the exact position
|
81 | uint16_t wall_get_position () |
||
82 | { |
||
83 | 1281 | kwoo | return wall_position;
|
84 | 1183 | deffi | } |
85 | 1285 | kwoo | /*
|
86 | 1281 | kwoo | // Read encoder when it changes and determine our position
|
87 | ISR(INT6_vect) {
|
||
88 | uint8_t tmp = PINE;
|
||
89 | |||
90 | if (((tmp & _BV(PE6)) << 1) ^ (tmp & _BV(PE7))) {
|
||
91 | wall_position--;
|
||
92 | } else {
|
||
93 | wall_position++;
|
||
94 | }
|
||
95 | }
|
||
96 | 1285 | kwoo | */
|
97 |