To tell you the truth, I'd be much happier programming a simple part like that by hand. I bet you could do it in under 30 lines including the comments and you'd have a much nicer time de-bugging it.