def goVel(x, y, theta, time=1):
v1, v2, v3 = mat.getWheelVel(x, y, theta)
# print v1, v2, v3;
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
#print "readS"
r2 = readM1speed(129)
#print "Read r2"
r3 = readM2speed(129)
#print "readr3"
r1 = readM2speed(128)
#print "Read r1"
# if DEBUG:
# print r1
# print r2
# print r3
#SetM2SpeedAccel(129,int(abs(r1-s1)/time),18000)
SetM2SpeedAccel(128, int(abs(r1 - s1) / time), s1)
SetM1SpeedAccel(129, int(abs(r2 - s2) / time), s2)
SetM2SpeedAccel(129, int(abs(r3 - s3) / time), s3)
#print "SetSpeeds"
s1_prev = s1
s2_prev = s2
s3_prev = s3
def goXYOmega(x,y,omega,limit=False):
if limit:
total = math.sqrt(float(x**2+y**2))
if total > cap:
scale = cap / total
x = x * scale
y = y * scale
v1,v2,v3 = mat.getWheelVel(x,y,omega)
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
SetM2Speed(129,s1)
SetM2Speed(128,s2)
SetM1Speed(129,s3)
def goXYOmega(x,y,omega,limit=False):
if limit:
total = math.sqrt(float(x**2+y**2))
if total > cap:
scale = cap / total
x = x * scale
y = y * scale
v1,v2,v3 = mat.getWheelVel(x,y,omega)
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
SetM1Speed(128,s1)
SetM2Speed(128,s2)
SetM1Speed(129,s3)
def goXYOmega(x,y,Omega): #if limit: total = math.sqrt(float(x**2+y**2)) if total > cap: scale = cap / total #print "this is the scale",scale x = x *scale y = y *scale v1,v2,v3 = mat.getWheelVel(x,y,Omega) s1 = radianToQpps(v1) s2 = radianToQpps(v2) s3 = radianToQpps(v3) print "values for v2,v2,v3: ",v1,v2,v3 print "....." print "values for s1,s2,s2 for speed: ",s1,s2,s3 SpeedM1(128,s1) SpeedM2(128,s2) SpeedM1(129,s3)
def goXYThetaAccel(x, y, theta, time=1):
v1, v2, v3 = mat.getWheelVel(x, y, theta)
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
r1 = readM1speed(128)
r3 = readM2speed(128)
r2 = readM2speed(129)
if DEBUG:
print r1
print r2
print r3
SetM1SpeedAccel(128, int(abs(r1 - s1) / time), s1)
SetM2SpeedAccel(128, int(abs(r2 - s2) / time), s2)
SetM1SpeedAccel(129, int(abs(r3 - s3) / time), s3)
s1_prev = s1
s2_prev = s2
s3_prev = s3
def goXYThetaAccel(x,y,theta,time=1):
v1,v2,v3 = mat.getWheelVel(x,y,theta)
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
r1 = readM1speed(128)
r3 = readM2speed(128)
r2 = readM2speed(129)
if DEBUG:
print r1
print r2
print r3
SetM1SpeedAccel(128,int(abs(r1-s1)/time),s1)
SetM2SpeedAccel(128,int(abs(r2-s2)/time),s2)
SetM1SpeedAccel(129,int(abs(r3-s3)/time),s3)
s1_prev=s1
s2_prev=s2
s3_prev=s3
def goXYOmega(x, y, Omega):
#if limit:
total = math.sqrt(float(x**2 + y**2))
if total > cap:
scale = cap / total
#print "this is the scale",scale
x = x * scale
y = y * scale
v1, v2, v3 = mat.getWheelVel(x, y, Omega)
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
print "values for v2,v2,v3: ", v1, v2, v3
print "....."
print "values for s1,s2,s2 for speed: ", s1, s2, s3
SpeedM1(128, s1)
SpeedM2(128, s2)
SpeedM1(129, s3)
def goXYOmega(x, y, omega, limit=False):
if limit:
total = math.sqrt(float(x**2 + y**2))
if total > cap:
scale = cap / total
x = x * scale
y = y * scale
v1, v2, v3 = mat.getWheelVel(x, y, omega)
print("wheel vel ({}, {}, {})").format(v1, v2, v3)
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
print("qpps ({}, {}, {})\n\n").format(s1, s2, s3)
SetM1Speed(128, s1)
SetM2Speed(128, s2)
SetM1Speed(129, s3)
def goXYOmega(x, y, omega, limit=False):
if limit:
total = math.sqrt(float(x**2 + y**2))
if total > cap:
scale = cap / total
x = x * scale
y = y * scale
v1, v2, v3 = mat.getWheelVel(x, y, omega)
#print v1
#print v2
#print v3
#print "."
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
print "Speed to Motors"
print s1
print s2
print s3
print "."
SetM1Speed(128, s1)
SetM2Speed(128, s2)
SetM2Speed(129, s3)
def goXYOmega(x,y,omega,limit=False):
if limit:
total = math.sqrt(float(x**2+y**2))
if total > cap:
scale = cap / total
x = x * scale
y = y * scale
v1,v2,v3 = mat.getWheelVel(x,y,omega)
#print v1
#print v2
#print v3
#print "."
s1 = radianToQpps(v1)
s2 = radianToQpps(v2)
s3 = radianToQpps(v3)
print "Speed to Motors"
print s1
print s2
print s3
print "."
SetM1Speed(128,s1)
SetM2Speed(128,s2)
SetM2Speed(129,s3)