import time
from roboclaw import Roboclaw

#Windows comport name
#rc = Roboclaw("COM3",115200)
#Linux comport name
rc = Roboclaw("/dev/ttyACM0",115200)

def displayspeed():
	enc1 = rc.ReadEncM1(address)
	enc2 = rc.ReadEncM2(address)
	speed1 = rc.ReadSpeedM1(address)
	speed2 = rc.ReadSpeedM2(address)

	print("Encoder1:"),
	if(enc1[0]==1):
		print enc1[1],
		print format(enc1[2],'02x'),
	else:
		print "failed",
	print "Encoder2:",
	if(enc2[0]==1):
		print enc2[1],
		print format(enc2[2],'02x'),
	else:
		print "failed " ,
	print "Speed1:",
	if(speed1[0]):
		print speed1[1],
	else:
		print "failed",
	print("Speed2:"),
	if(speed2[0]):
		print speed2[1]
	else:
		print "failed "

rc.Open()
address = 0x80

version = rc.ReadVersion(address)
if version[0]==False:
	print "GETVERSION Failed"
else:
	print repr(version[1])
oneinch = 421.176471;
twoinch = oneinch*2;
threeinch = oneinch*3;
fourinch = oneinch*4;
fiveinch = oneinch*5;
sixinch = oneinch*6;
seveninch = oneinch*7;
eightinch = oneinch*8;
nineinch = oneinch*9;
teninch = oneinch*10;

rc.SpeedAccelDistanceM1(address,6000,6000,421*6,1);
rc.SpeedAccelDistanceM2(address,6000,6000,421*6,1);
buffers = (0,0,0)
while(buffers[1]!=0x80 and buffers[2]!=0x80):	#Loop until distance command has completed
	displayspeed();
	buffers = rc.ReadBuffers(address);

time.sleep(1)
