def printState(): r1 = readM2speed(129) r2 = readM2speed(128) r3 = readM1speed(129) v1 = qppsToRadian(r1) v2 = qppsToRadian(r2) v3 = qppsToRadian(r3) x,y,omega = mat.getXYOmega(v1,v2,v3) print "X=%f" % x print "Y=%f" % y print "O=%f" % omega
def printState(): r1 = readM1speed(128) r3 = readM2speed(128) r2 = readM2speed(129) v1 = qppsToRadian(r1) v2 = qppsToRadian(r2) v3 = qppsToRadian(r3) x,y,omega = mat.getXYOmega(v1,v2,v3) print "X=%f" % x print "Y=%f" % y print "O=%f" % omega