Fixed several bugs - appears to work in serial now