def turn_in_place(degrees_to_turn): # Turning amount should not be more than 360 degrees degrees_to_turn = degrees_to_turn % FULL_REVOLUTION_DEGREES if degrees_to_turn == 0: return # Make turning efficient so that robot does not turn more than half turn if abs(degrees_to_turn) > HALF_REVOLUTION_DEGREES: if degrees_to_turn > 0: degrees_to_turn = -1 * (FULL_REVOLUTION_DEGREES - degrees_to_turn) else: degrees_to_turn = FULL_REVOLUTION_DEGREES + degrees_to_turn #Compute the number of encoder steps needed encoder_steps_needed = int(ENCODER_STEPS_FOR_ABOUT_TURN * abs(degrees_to_turn) / FULL_REVOLUTION_DEGREES) #If encoder steps needed are zero, due to truncation, do nothing if encoder_steps_needed == 0: return #Turn the number of encoder steps computed gopigo.enable_encoders() gopigo.enc_tgt(1, 1, abs(encoder_steps_needed)) turnAngleSign = 1 if degrees_to_turn > 0: gopigo.right_rot() else: gopigo.left_rot() turnAngleSign = -1 wait_till_encoder_target_reached() RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(currentRobotPose = RobotPosePrediction.currentRobotPose, movementType = RobotPosePrediction.MOVEMENT_TYPE_ROTATE_IN_PLACE, movementAmount = turnAngleSign * encoder_steps_needed * FULL_REVOLUTION_DEGREES / ENCODER_STEPS_PER_REVOLUTION) RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(currentRobotLocationUncertainty = RobotPosePrediction.currentRobotLocationUncertainty, movementType = RobotPosePrediction.MOVEMENT_TYPE_ROTATE_IN_PLACE, movementAmount = turnAngleSign * encoder_steps_needed * FULL_REVOLUTION_DEGREES / ENCODER_STEPS_PER_REVOLUTION, currentRobotPose = RobotPosePrediction.currentRobotPose)
def go_backwards(backwards_distance): gopigo.enable_encoders() encoder_steps_required = int(backwards_distance / DISTANCE_PER_ENCODER_STEP) gopigo.set_speed(DEFAULT_ROBOT_SPEED) gopigo.enc_tgt(1, 1, encoder_steps_required) gopigo.bwd() wait_till_encoder_target_reached() RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(currentRobotPose = RobotPosePrediction.currentRobotPose, movementType = RobotPosePrediction.MOVEMENT_TYPE_FORWARD, movementAmount = -1 * encoder_steps_required * DISTANCE_PER_ENCODER_STEP) RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(currentRobotLocationUncertainty = RobotPosePrediction.currentRobotLocationUncertainty, movementType = RobotPosePrediction.MOVEMENT_TYPE_FORWARD, movementAmount = -1 * encoder_steps_required * DISTANCE_PER_ENCODER_STEP, currentRobotPose = RobotPosePrediction.currentRobotPose)
def turn_in_place(degrees_to_turn):
# Turning amount should not be more than 360 degrees
degrees_to_turn = degrees_to_turn % FULL_REVOLUTION_DEGREES
if degrees_to_turn == 0:
return
# Make turning efficient so that robot does not turn more than half turn
if abs(degrees_to_turn) > HALF_REVOLUTION_DEGREES:
if degrees_to_turn > 0:
degrees_to_turn = -1 * (FULL_REVOLUTION_DEGREES - degrees_to_turn)
else:
degrees_to_turn = FULL_REVOLUTION_DEGREES + degrees_to_turn
#Compute the number of encoder steps needed
encoder_steps_needed = int(ENCODER_STEPS_FOR_ABOUT_TURN *
abs(degrees_to_turn) / FULL_REVOLUTION_DEGREES)
#If encoder steps needed are zero, due to truncation, do nothing
if encoder_steps_needed == 0:
return
#Turn the number of encoder steps computed
gopigo.enable_encoders()
gopigo.enc_tgt(1, 1, abs(encoder_steps_needed))
turnAngleSign = 1
if degrees_to_turn > 0:
gopigo.right_rot()
else:
gopigo.left_rot()
turnAngleSign = -1
wait_till_encoder_target_reached()
RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(
currentRobotPose=RobotPosePrediction.currentRobotPose,
movementType=RobotPosePrediction.MOVEMENT_TYPE_ROTATE_IN_PLACE,
movementAmount=turnAngleSign * encoder_steps_needed *
FULL_REVOLUTION_DEGREES / ENCODER_STEPS_PER_REVOLUTION)
RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(
currentRobotLocationUncertainty=RobotPosePrediction.
currentRobotLocationUncertainty,
movementType=RobotPosePrediction.MOVEMENT_TYPE_ROTATE_IN_PLACE,
movementAmount=turnAngleSign * encoder_steps_needed *
FULL_REVOLUTION_DEGREES / ENCODER_STEPS_PER_REVOLUTION,
currentRobotPose=RobotPosePrediction.currentRobotPose)
def go_forward(forward_distance):
gopigo.enable_encoders()
encoder_steps_required = int(forward_distance / DISTANCE_PER_ENCODER_STEP)
gopigo.set_speed(DEFAULT_ROBOT_SPEED)
gopigo.enc_tgt(1, 1, encoder_steps_required)
gopigo.fwd()
wait_till_encoder_target_reached()
RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(
currentRobotPose=RobotPosePrediction.currentRobotPose,
movementType=RobotPosePrediction.MOVEMENT_TYPE_FORWARD,
movementAmount=encoder_steps_required * DISTANCE_PER_ENCODER_STEP)
RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(
currentRobotLocationUncertainty=RobotPosePrediction.
currentRobotLocationUncertainty,
movementType=RobotPosePrediction.MOVEMENT_TYPE_FORWARD,
movementAmount=encoder_steps_required * DISTANCE_PER_ENCODER_STEP,
currentRobotPose=RobotPosePrediction.currentRobotPose)
