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