Anyone written a Python "translatePos(distance)" method?

GoPiGo
1

Anyone written a Python “translatePosition(distance, technique)” method for moving a little left or right?

I’m thinking of three techniques:

  • “Forward_S” turn plus back step
  • “Backward_S” turn plus forward step
  • “Right_angle” turn, fwd distance, opposite right angle turn

where the S turns use the orbit() function with an s_angle=arc-cos(distance / wheel_base) and the step move = sin(s_angle)*wheel_base. (Have to double check the calcs…)

2

Here is the first one - “Forward_S”

# TRANSLATE Sideways in millimeters
#     executes two forward S-turns and then drives back to starting line
#     1-3mm requires about an inch front clearance and 1.5 inches side clearance
#     1 inch requires about 3 inches front clearance and about 4 inches side clearance
#     3 inches requires about 4.5 inches front clearance and 6 inches side clearance
def translateMm(egpg=None,dist_mm=1.0,debug=False):
    if (abs(dist_mm) > egpg.WHEEL_BASE_WIDTH):
        translateMm(egpg, dist_mm / 2.0)
        translateMm(egpg, dist_mm / 2.0)
    ORBIT_SPEED = 120
    egpg.set_speed(ORBIT_SPEED)
    cosTheta = (egpg.WHEEL_BASE_WIDTH - abs(dist_mm)) / egpg.WHEEL_BASE_WIDTH
    orbitAngle = math.degrees( math.acos(cosTheta) )
    orbitRadius_cm = egpg.WHEEL_BASE_WIDTH / 20
    if debug: print("orbit {:.0f} deg, {:.1f} cm radius".format(orbitAngle, orbitRadius_cm))
    egpg.orbit(degrees= np.sign(dist_mm) * orbitAngle, radius_cm=orbitRadius_cm, blocking=True)
    sleep(0.5)
    egpg.orbit(degrees= -np.sign(dist_mm) * orbitAngle, radius_cm=orbitRadius_cm, blocking=True)
    sleep(0.5)
    backup_cm = (egpg.WHEEL_BASE_WIDTH * -math.sin( math.radians(orbitAngle) )) / 10.0
    if debug: print("backing {:.1f} cm {:.1f} inches".format(backup_cm, backup_cm/2.54))
    egpg.drive_cm(dist=backup_cm,blocking=True)

(With special thanks to @cleoqc for the EasyGoPiGo3.orbit() method - made this much easier!)

1 Like