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 __init__(self, recordFreq=0.5, camera=None, stream=None):
"""this function intializes everything, including setting the speed"""
go.set_right_speed(50)
go.set_left_speed(50)
go.enable_encoders()
self.camera = camera
self.stream = stream
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)
def enable_encoders():
go.enable_encoders()
def process_command(self, command):
parts = command.split("/")
if parts[1] == "poll":
print "poll"
self.us_dist = gopigo.us_dist(usdist_pin)
self.enc_status = gopigo.read_status()[0]
self.volt = gopigo.volt()
self.fw_ver = gopigo.fw_ver()
self.trim = gopigo.trim_read() - 100
if self.enc_status == 0:
self.waitingOn = None
elif parts[1] == "stop":
gopigo.stop()
elif parts[1] == "trim_write":
gopigo.trim_write(int(parts[2]))
self.trim = gopigo.trim_read()
elif parts[1] == "trim_read":
self.trim = gopigo.trim_read() - 100
elif parts[1] == "set_speed":
if parts[2] == "left":
self.left_speed = int(parts[3])
elif parts[2] == "right":
self.right_speed = int(parts[3])
else:
self.right_speed = int(parts[3])
self.left_speed = int(parts[3])
gopigo.set_left_speed(self.left_speed)
gopigo.set_right_speed(self.right_speed)
elif parts[1] == "leds":
val = 0
if parts[3] == "on":
val = 1
elif parts[3] == "off":
val = 0
elif parts[3] == "toggle":
val = -1
if parts[2] == "right" or parts[2] == "both":
if val >= 0:
self.ledr = val
else:
self.ledr = 1 - self.ledr
if parts[2] == "left" or parts[2] == "both":
if val >= 0:
self.ledl = val
else:
self.ledl = 1 - self.ledl
gopigo.digitalWrite(ledr_pin, self.ledr)
gopigo.digitalWrite(ledl_pin, self.ledl)
elif parts[1] == "servo":
gopigo.servo(int(parts[2]))
elif parts[1] == "turn":
self.waitingOn = parts[2]
direction = parts[3]
amount = int(parts[4])
encleft = 0 if direction == "left" else 1
encright = 1 if direction == "left" else 0
gopigo.enable_encoders()
gopigo.enc_tgt(encleft, encright, int(amount / DPR))
if direction == "left":
gopigo.left()
else:
gopigo.right()
elif parts[1] == "move":
self.waitingOn = int(parts[2])
direction = parts[3]
amount = int(parts[4])
gopigo.enable_encoders()
gopigo.enc_tgt(1, 1, amount)
if direction == "backward":
gopigo.bwd()
else:
gopigo.fwd()
elif parts[1] == "beep":
gopigo.analogWrite(buzzer_pin, self.beep_volume)
time.sleep(self.beep_time)
gopigo.analogWrite(buzzer_pin, 0)
elif parts[1] == "reset_all":
self.ledl = 0
self.ledr = 0
gopigo.digitalWrite(ledl_pin, self.ledl)
gopigo.digitalWrite(ledr_pin, self.ledr)
gopigo.analogWrite(buzzer_pin, 0)
# gopigo.servo(90)
gopigo.stop()
LEFT = 0
RIGHT = 1
SPEED = int(sys.argv[1])
INC = (SPEED / 100) * 10 #7#15
SAFE_DISTANCE = 45
USS = 15
TRIM = int(sys.argv[2])
CORRECTION = sys.argv[3] == "true"
print(CORRECTION)
# Might as well try this out, probably doesn't do anything though
gopigo.disable_encoders()
gopigo.enable_encoders()
gopigo.trim_write(TRIM)
ADDRESS = 0x08
ENC_READ_CMD = [53]
import smbus
bus = smbus.SMBus(1)
def write_i2c_block(address, block):
try:
op = bus.write_i2c_block_data(address, 1, block)
time.sleep(0.005)
return op
def enable_encoders(kargs):
r = {'return_value': gopigo.enable_encoders()}
return r
