Revision 1285
Added shitty position control. Needs encoders to not suck.
| trunk/code/projects/diagnostic_station/station/hardware_wall.c | ||
|---|---|---|
| 2 | 2 |
#include <avr/io.h> |
| 3 | 3 |
#include <avr/interrupt.h> |
| 4 | 4 |
|
| 5 |
uint16_t wall_position; // Wall position in encoder ticks |
|
| 5 |
volatile uint16_t wall_position; // Wall position in encoder ticks
|
|
| 6 | 6 |
|
| 7 | 7 |
void wall_init () |
| 8 | 8 |
{
|
| ... | ... | |
| 15 | 15 |
PORTB &= ~_BV(PB4); |
| 16 | 16 |
|
| 17 | 17 |
// Turn on the interrupts for the A quadrature |
| 18 |
/* uncomment when we actually have encoders |
|
| 18 | 19 |
cli(); |
| 19 | 20 |
EICRB |= _BV(ISC60); |
| 20 | 21 |
EIMSK |= _BV(INT6); |
| 21 | 22 |
sei(); |
| 22 |
|
|
| 23 |
*/ |
|
| 23 | 24 |
// Enable limit switch as input |
| 24 | 25 |
DDRF &= ~_BV(DDF0); |
| 25 | 26 |
|
| ... | ... | |
| 40 | 41 |
|
| 41 | 42 |
usb_puts("Done with wall init\r\n");
|
| 42 | 43 |
|
| 43 |
wall_position=0; |
|
| 44 |
|
|
| 44 |
wall_position = 0; |
|
| 45 | 45 |
} |
| 46 | 46 |
|
| 47 | 47 |
|
| 48 | 48 |
void wall_set_position (uint16_t position) |
| 49 | 49 |
{
|
| 50 |
char i; |
|
| 51 |
usb_puts ("# Moving wall from position ");
|
|
| 52 |
usb_puti (wall_position); |
|
| 53 |
usb_puts (NL); |
|
| 50 | 54 |
usb_puts ("# Moving wall to position ");
|
| 51 | 55 |
usb_puti (position); |
| 52 | 56 |
usb_puts (NL); |
| 53 | 57 |
|
| 54 |
// FIXME implement |
|
| 55 |
|
|
| 56 |
//wall_position=position+1; |
|
| 57 |
|
|
| 58 |
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; |
|
| 58 | 76 |
usb_puts ("# Wall position reached" NL);
|
| 59 | 77 |
} |
| 60 | 78 |
|
| ... | ... | |
| 64 | 82 |
{
|
| 65 | 83 |
return wall_position; |
| 66 | 84 |
} |
| 67 |
|
|
| 85 |
/* |
|
| 68 | 86 |
// Read encoder when it changes and determine our position |
| 69 | 87 |
ISR(INT6_vect) {
|
| 70 | 88 |
uint8_t tmp = PINE; |
| ... | ... | |
| 75 | 93 |
wall_position++; |
| 76 | 94 |
} |
| 77 | 95 |
} |
| 96 |
*/ |
|
| 97 |
|
|
| 98 |
|
|
| trunk/code/projects/diagnostic_station/station/main.c | ||
|---|---|---|
| 138 | 138 |
usb_puts("# Diagnostic station version " version_string " starting" NL);
|
| 139 | 139 |
|
| 140 | 140 |
wall_init(); |
| 141 |
|
|
| 142 |
wall_set_position(1); |
|
| 143 |
wall_set_position(2); |
|
| 144 |
wall_set_position(3); |
|
| 145 |
wall_set_position(4); |
|
| 146 |
wall_set_position(5); |
|
| 147 |
|
|
| 141 | 148 |
while (1) {
|
| 149 |
//usb_puti(wall_get_position()); |
|
| 150 |
//usb_puts("\r\n");
|
|
| 142 | 151 |
if (PINF & _BV(PINF0)) {
|
| 143 | 152 |
orbs_set(0,0,255,0,0,0); |
| 144 | 153 |
} |
Also available in: Unified diff