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); }