def main(): # Tell LocoRobo what serial port to use LocoRobo.setup("/dev/tty.usbmodem1") # Scan for robots robots = LocoRobo.scan(2000) # Use get_robots to find robot with name lr0007 in the scan result robot = get_robot(robots, "will123") robot.connect() robot.activate_motors() robot.enable_sensor(Data.ULTRASONIC, True) robot.enable_sensor(Data.ACCELEROMETER, True) robot.enable_sensor(Data.GYROSCOPE_RAW, True) robot.enable_sensor(Data.RUNNING_ENCODERS, True) while True: print(robot.get_sensor_value(Data.ULTRASONIC)) print(robot.get_sensor_value(Data.ACCELEROMETER)) print(robot.get_sensor_value(Data.GYROSCOPE_RAW)) print(robot.get_sensor_value(Data.RUNNING_ENCODERS)) time.sleep(0.1) robot.deactivate_motors() robot.disconnect() print('done')
def main(): # Tell LocoRobo what serial port to use LocoRobo.setup("/dev/tty.usbmodem1") # Scan for robots robots = LocoRobo.scan(2000) # Use get_robots to find robot with name lr d2:fa in the scan result robot = get_robot(robots, "lr d2:fa") robot.connect() robot.activate_motors() robot.enable_sensor(Data.ULTRASONIC, True) #setup the distance to be travelled in centimeters distance_cm = 100 # tried to use ultrasonic sensors to detect how far away a wall was, didn't go too well # distance_cm = robot.get_sensor_value(Data.ULTRASONIC) - 10 #so that the robot doesn't HIT the wall # print(distance_cm) robot.setup_wait(WaitType.DISTANCE, distance_cm * 1000) robot.move(MotorDirection.FORWARD, MotorDirection.FORWARD, 1, 1, True) robot.deactivate_motors() robot.disconnect()
def main (): # Tell LocoRobo what serial port to use LocoRobo.setup("/dev/tty.usbmodem11") # Scan for robots robots = LocoRobo.scan(2000) # Use get_robots to find robot with name lr 00:07 in the scan result robot = get_robot(robots, "lr d2:90") robot.connect() robot.activate_motors() robot.enable_sensor(Data.ULTRASONIC, True) robot.enable_sensor(Data.ACCELEROMETER, True) def check_tilt (tilt): print('tilt dict:', tilt) message = '' if tilt['y'] <= -.5: message += 'fell on left side' elif tilt['y'] >= .5: message += 'fell on right side' elif tilt['x'] <= -.5: # tilt[x]: 0.224917888641 message += 'fell forward' elif tilt['x'] >= .5: message += 'fell backward' return message # if message remains empty string, the boolean value of it # will be false def do_lights (): robot.set_light(0, 255, 0, 255) robot.sync_lights() robot.set_light(1, 200, 0, 255) robot.sync_lights() robot.set_light(2, 180, 0, 255) robot.sync_lights() robot.set_light(3, 20, 0, 255) robot.sync_lights() def up_ramp(): while True: robot.setup_wait(WaitType.DISTANCE, 4 * 1000) robot.move(MotorDirection.FORWARD, MotorDirection.FORWARD, .7, .7, True) tilt = robot.get_sensor_value(Data.ACCELEROMETER) fell = check_tilt(tilt) if fell: robot.deactivate_motors() print(fell) do_lights() break up_ramp() robot.disconnect()
def run_robot(value_queue): # Tell LocoRobo what serial port to use LocoRobo.setup("/dev/tty.usbmodem1") # Scan for robots robots = LocoRobo.scan(2000) # Use get_robots to find robot with name robot = get_robot(robots, "lr 69:5b") robot.connect() robot.activate_motors() robot.enable_sensor(Data.ULTRASONIC, True) while True: dist_val = robot.get_sensor_value(Data.ULTRASONIC) value_queue.put(dist_val) LocoRobo.wait(0.05)
def CONNECT_ROBOT(self): print("--Locating ", name, "--\n") LocoRobo.setup(BT_com_port) robots = LocoRobo.scan(2000) self.robot = None for r in robots.values(): if r.name == name: self.robot = r break if self.robot != None: self.robot.connect() self.robot.activate_motors() self.robot.enable_sensor(Data.ACCELEROMETER, True) self.robot.enable_sensor(Data.ULTRASONIC, True) print("<<--", name, " CONNECTED-->>\n") self.startBot.configure(state='disable', bg = "green") # Update GUI self.connections += 1 if self.connections == self.total_connections: self.enableButtons() else: print("****Could not find robot try again****\n")
def main(): # Tell LocoRobo what serial port to use LocoRobo.setup("/dev/tty.usbmodem11") # Scan for robots robots = LocoRobo.scan(2000) # Use get_robots to find robot with name lr 00:07 in the scan result robot = get_robot(robots, "lr d2:90") robot.connect() robot.activate_motors() robot.enable_sensor(Data.ULTRASONIC, True) def undo(side2, side1, direction): robot.setup_wait(WaitType.DISTANCE, side2 * 1000) robot.move(MotorDirection.BACKWARD, MotorDirection.BACKWARD, .9, .9, True) direction = 'l' if direction is 'r' else 'r' turn(direction) robot.setup_wait(WaitType.DISTANCE, side1 * 1000) robot.move(MotorDirection.BACKWARD, MotorDirection.BACKWARD, .9, .9, True) def turn(direction): robot.setup_wait(WaitType.ROTATION, degrees) if 'r' in direction: robot.move(MotorDirection.FORWARD, MotorDirection.BACKWARD, .7, .7, True) else: robot.move(MotorDirection.BACKWARD, MotorDirection.FORWARD, .7, .7, True) def forward(): print('moving forward\n') robot.setup_wait(WaitType.DISTANCE, d) robot.move(MotorDirection.FORWARD, MotorDirection.FORWARD, .8, .8, True) def get_side_length(): length = 0 while True: forward() # move forward length += 2 # robot moves forward 2 cm every time, so I add that to the length each iteration print('length after adding 2:', length) distance = robot.get_sensor_value(Data.ULTRASONIC) # get distance print('distance:', distance) if distance <= 6: # distance from wall, far to be safe return length + 6 def review_info(): print('RECTANGLE INFO:') print('\n\nFinal sides:', sides) if sides[0] > sides[1]: length, width = sides[0], sides[1] else: length, width = sides[1], sides[0] print('Length: {}, Width: {}'.format(length, width)) print('Area:', length * width) perim = length * 2 + width * 2 print('Perimeter:', perim) d = 2 * 1000 degrees = 90 * 1000 robot_length = 8.5725 # measured width_correction = 2 # the width is always off by a little bit because of its turning, # so I found that simply adding a correction does the trick sides = [] direction = input( 'Robot will be going forward and then -- l(eft) or r(ight): ') query = 'How many inches are you giving the robot on the {} side? '.format( direction) # allow gap # so that it can turn easily when it gets to end of first side starting_gap = int(input(query)) # turn input into int pure_sides = [] for side in range(2): # get length and width length = get_side_length() print('final length of side {}: {}'.format(side + 1, length)) pure_sides.append( length - 6 ) # 6 is the distance from wall, we don't want that included in the pure length sides.append(length + robot_length) # add length of side and robot length print('sides:', sides) if side == 0: turn( direction ) # if it is the first side, then turn in the direction inputted earlier else: sides[1] += starting_gap + width_correction # below: unnecessary but why not undo(pure_sides[1], pure_sides[0], direction) # go back to the starting point review_info() # output of rectangle info robot.play_song(Song.EyeOfTheTiger, False) # fun robot.deactivate_motors() robot.disconnect()
def main(): # Tell LocoRobo what serial port to use LocoRobo.setup("/dev/tty.usbmodem1") # Scan for robots for 2000 ms robots = LocoRobo.scan(2000) # Use get_robots to find robot with name "lr 67:c6" in the scan result robot = get_robot(robots, "joey") robot.connect() robot.enable_sensor(Data.Accelerometer, True) LocoRobo.wait(2) # Create the drone serial object drone = LocoDrone('/dev/tty.usbmodem1A1211', 115200) # Open the serial port drone.open_serial() drone.serial_control() drone.bind() # Main run loop while True: # Get accelerometer values from the control robot acc = robot.get_sensor_value(Data.Accelerometer) # Map the corresponding axis values to a variable x = acc['x'] y = acc['y'] z = acc['z'] # Get the angle of the x-axis and y-axis in radians pitch_rad = math.atan(x / math.sqrt(y**2 + z**2)) roll_rad = math.atan(y / math.sqrt(x**2 + z**2)) # Convert the angles of the x-axis and y-axis to degrees pitch_deg = -1 * (pitch_rad * 180) / math.pi roll_deg = (roll_rad * 180) / math.pi pitch = map_range(pitch_deg, -MAX_TILT_ANGLE, MAX_TILT_ANGLE, 0, 255) roll = map_range(roll_deg, -MAX_TILT_ANGLE, MAX_TILT_ANGLE, 0, 255) throttle = 30 yaw = 127 drone.set_throttle(throttle) drone.set_yaw(yaw) drone.set_roll(roll) drone.set_pitch(pitch) """ print("Throttle: ", drone.get_throttle(), " Yaw: ", drone.get_yaw(), " Roll: ", drone.get_roll(), " Pitch: ", drone.get_pitch()) """ drone.update_payload() debug_payload = [] control = drone.read_byte() length = drone.read_byte() if control == 255 and length == 8: for i in range(8): debug_payload.append(drone.read_byte()) print(debug_payload) LocoRobo.wait(0.05)
def main(): last_right_encoder = 0 last_left_encoder = 0 last_heading = 0 # Create the drone serial object drone = LocoDrone('/dev/tty.usbmodem1411', 115200) # Open the serial port drone.open_serial() user_control(drone) # Tell LocoRobo what serial port to use LocoRobo.setup("/dev/tty.usbmodem1") # Scan for robots for 2000 ms robots = LocoRobo.scan(2000) # Use get_robots to find robot with name "lr 67:c6" in the scan result robot = get_robot(robots, "joey") robot.connect() robot.enable_sensor(Data.Accelerometer, True) robot.enable_sensor(Data.RunningEncoders, True) # robot.enable_sensor(Data.MagnetometerRaw, True) #robot.enable_sensor(Data.Heading, True) time.sleep(5) while True: try: # Get accelerometer values from the control robot acc = robot.get_sensor_value(Data.Accelerometer) # Get magnetometer data from sensor #heading = robot.get_sensor_value(Data.Heading) # Map the corresponding axis values to a variable x = acc['x'] y = acc['y'] z = acc['z'] # Get the angle of the x-axis and y-axis in radians pitch_rad = math.atan(x / math.sqrt(y**2 + z**2)) roll_rad = math.atan(y / math.sqrt(x**2 + z**2)) # Convert the angles of the x-axis and y-axis to degrees pitch_deg = round(((pitch_rad * 180) / math.pi), 0) roll_deg = round(((roll_rad * 180) / math.pi), 0) pitch_deg *= -1 # Get the encoder ticks for both the left and right encoders tickcounts = robot.get_sensor_value(Data.RunningEncoders) left_encoder = -(tickcounts['left'] / 20) right_encoder = -(tickcounts['right'] / 20) # Use the encoders to yaw_diff = left_encoder - last_left_encoder last_left_encoder = left_encoder throttle_diff = right_encoder - last_right_encoder last_right_encoder = right_encoder yaw = drone.get_yaw() - yaw_diff throttle = drone.get_throttle() - throttle_diff pitch = map_range(pitch_deg, -MAX_TILT_ANGLE, MAX_TILT_ANGLE, 0, 255) roll = map_range(roll_deg, -MAX_TILT_ANGLE, MAX_TILT_ANGLE, 0, 255) drone.set_throttle(int(throttle)) drone.set_yaw(int(yaw)) drone.set_pitch(int(pitch)) drone.set_roll(int(roll)) drone.update_payload() except ZeroDivisionError as e: print('Got Zero Division', e) LocoRobo.wait(0.01) # Unbind from drone drone.unbind() # Deactivate motors and disconnect from robot robot.deactivate_motors() robot.disconnect()
def main(): # Create the drone serial object drone = Drone('/dev/tty.usbmodem1A1241', 19200) # Open the serial port drone.open_serial() # Bind to the drone drone.bind() # Tell LocoRobo what serial port to use LocoRobo.setup("/dev/tty.usbmodem1") # Scan for robots for 2000 ms robots = LocoRobo.scan(2000) # Use get_robots to find robot with name "lr 67:c6" in the scan result robot = get_robot(robots, "robot4") robot.connect() robot.activate_motors() robot.enable_sensor(Data.Accelerometer, True) robot.enable_sensor(Data.RunningEncoders, True) LocoRobo.wait(2.0) #thread to get sensor values and map them to drone commands #threading.Thread(target = self._throttle_control).start() #thread to send commands to the drone # Main run loop while True: # Get accelerometer values from the control robot acc = robot.get_sensor_value(Data.Accelerometer) # Map the corresponding axis values to a variable x = acc['x'] y = acc['y'] z = acc['z'] # Get the angle of the x-axis and y-axis in radians pitch_rad = math.atan(x / math.sqrt(y**2 + z**2)) roll_rad = math.atan(y / math.sqrt(x**2 + z**2)) # Convert the angles of the x-axis and y-axis to degrees pitch_deg = int((pitch_rad * 180) / math.pi * -1) roll_deg = int((roll_rad * 180) / math.pi) tickcounts = robot.get_sensor_value(Data.RunningEncoders) left_encoder = tickcounts['left'] right_encoder = tickcounts['right'] # print("Throttle: ", right_encoder) # print("Yaw: ", left_encoder) # print("Roll: ", roll_deg) # print("Pitch: ", pitch_deg) drone.set_throttle(127) drone.set_yaw(127) drone.set_roll( map_range(roll_deg, -MAX_TILT_ANGLE, MAX_TILT_ANGLE, 0, 255)) drone.set_pitch( map_range(pitch_deg, -MAX_TILT_ANGLE, MAX_TILT_ANGLE, 0, 255)) drone.update_payload() #LocoRobo.wait(0.1) # Unbind from drone drone.unbind() # Deactivate motors and disconnect from robot robot.deactivate_motors() robot.disconnect()