xymotors.h

unsigned char x_cycle, y_cycle;

// move the x rod forwards
void x_forw(int time_ms){
	switch(x_cycle)
	{
		case 0: PORTB = 0x80 | (PORTB & 0x0f); break;
		case 1: PORTB = 0x40 | (PORTB & 0x0f); break;
		case 2: PORTB = 0x20 | (PORTB & 0x0f); break;
		case 3: PORTB = 0x10 | (PORTB & 0x0f); break;
		default: x_cycle = x_cycle%4; break;  // default: if half cycle used,
	}	                                          //     x_cycle may not be in 0-3 range
	x_cycle = (x_cycle + 1)% 4;
	delay_ms(time_ms);
}

// move the x rod backwards
void x_backw(int time_ms){
	switch(x_cycle)
	{
		case 0: PORTB=0x20 | (PORTB & 0x0f);
				x_cycle=3;
				break;
		case 1: PORTB=0x10 | (PORTB & 0x0f);
				x_cycle=0;
				break;
		case 2: PORTB = 0x80 | (PORTB & 0x0f);
				x_cycle=1;
				break;
		case 3: PORTB = 0x40 | (PORTB & 0x0f);
				x_cycle=2;
				break;
		default: x_cycle = x_cycle%4; break;
	}
	delay_ms(time_ms);
}

// move the y rod forwards
void y_forw(int time_ms){
	switch(y_cycle)
	{
		case 0: PORTB= 0x08 | (PORTB & 0xf0); break;
		case 1: PORTB= 0x04 | (PORTB & 0xf0); break;
		case 2: PORTB= 0x02 | (PORTB & 0xf0); break;
		case 3: PORTB= 0x01 | (PORTB & 0xf0); break;
		default: y_cycle = y_cycle%4; break;
	}
	y_cycle = (y_cycle + 1)%4;
	delay_ms(time_ms);
}

// move the y rod backwards
void y_backw(int time_ms){
	switch(y_cycle)
	{
		case 0: PORTB=0x02 | (PORTB & 0xf0);
				y_cycle=3;
				break;
		case 1: PORTB=0x01 | (PORTB & 0xf0);
				y_cycle=0;
				break;
		case 2: PORTB = 0x08 | (PORTB & 0xf0);
				y_cycle=1;
				break;
		case 3: PORTB = 0x04 | (PORTB & 0xf0);
				y_cycle=2;
				break;
		default: y_cycle = y_cycle%4; break;
	}
	delay_ms(time_ms);
}

// move x-rod half-step forward
void x_forw_half(int time_ms){
	switch (x_cycle)
	{
		case 0: PORTB = 0x80 | (PORTB & 0x0f); break;
		case 1: PORTB = 0xc0 | (PORTB & 0x0f); break;
		case 2: PORTB = 0x40 | (PORTB & 0x0f); break;
		case 3: PORTB = 0x60 | (PORTB & 0x0f); break;
		case 4: PORTB = 0x20 | (PORTB & 0x0f); break;
		case 5: PORTB = 0x30 | (PORTB & 0x0f); break;
		case 6: PORTB = 0x10 | (PORTB & 0x0f); break;
		case 7: PORTB = 0x90 | (PORTB & 0x0f); break;
	}
	x_cycle = (x_cycle + 1)% 8;
	delay_ms(time_ms);
}

// move x rod half-step backwards
void x_backw_half(int time_ms){
	switch (x_cycle)
	{
		case 0: PORTB = 0x20 | (PORTB & 0x0f);
				x_cycle = 7;
				break;
		case 1: PORTB = 0x30 | (PORTB & 0x0f);
				x_cycle=0;
				break;
		case 2: PORTB = 0x10 | (PORTB & 0x0f);
				x_cycle=1;
				break;
		case 3: PORTB = 0x90 | (PORTB & 0x0f);
				x_cycle=2;
				break;
		case 4: PORTB = 0x80 | (PORTB & 0x0f);
				x_cycle=3;
				break;
		case 5: PORTB = 0xc0 | (PORTB & 0x0f);
				x_cycle=4;
				break;
		case 6: PORTB = 0x40 | (PORTB & 0x0f);
				x_cycle=5;
				break;
		case 7: PORTB = 0x60 | (PORTB & 0x0f);
				x_cycle=6;
				break;
	}
	delay_ms(time_ms);
}

// move y rod half-step forward
void y_forw_half(int time_ms){
	switch (y_cycle)
	{
		case 0: PORTB = 0x08 | (PORTB & 0xf0); break;
		case 1: PORTB = 0x0c | (PORTB & 0xf0); break;
		case 2: PORTB = 0x04 | (PORTB & 0xf0); break;
		case 3: PORTB = 0x06 | (PORTB & 0xf0); break;
		case 4: PORTB = 0x02 | (PORTB & 0xf0); break;
		case 5: PORTB = 0x03 | (PORTB & 0xf0); break;
		case 6: PORTB = 0x01 | (PORTB & 0xf0); break;
		case 7: PORTB = 0x09 | (PORTB & 0xf0); break;
	}
	y_cycle = (y_cycle + 1)% 8;
	delay_ms(time_ms);
}

// move y rod half-step backwards
void y_backw_half(int time_ms){
	switch (y_cycle)
	{
		case 0: PORTB = 0x02 | (PORTB & 0xf0);
				y_cycle = 7;
				break;
		case 1: PORTB = 0x03 | (PORTB & 0xf0);
				y_cycle=0;
				break;
		case 2: PORTB = 0x01 | (PORTB & 0xf0);
				y_cycle=1;
				break;
		case 3: PORTB = 0x09 | (PORTB & 0xf0);
				y_cycle=2;
				break;
		case 4: PORTB = 0x08 | (PORTB & 0xf0);
				y_cycle=3;
				break;
		case 5: PORTB = 0x0c | (PORTB & 0xf0);
				y_cycle=4;
				break;
		case 6: PORTB = 0x04 | (PORTB & 0xf0);
				y_cycle=5;
				break;
		case 7: PORTB = 0x06 | (PORTB & 0xf0);
				y_cycle=6;
				break;
	}
	delay_ms(time_ms);
}