Exemple #1
0
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
Exemple #2
0
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)
Exemple #4
0
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)
Exemple #5
0
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
Exemple #6
0
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
Exemple #7
0
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)
Exemple #8
0
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)
Exemple #9
0
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)
Exemple #10
0
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)