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.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 _follow(self):
self.follow_loop = True
threading.Thread(target = self._random_walk).start()
while self.follow_loop:
if self.walk_state == 1 : self.ser.write(bytes('o', 'utf-8')) # Follow Pitch
elif self.walk_state == 2 : self.ser.write(bytes('p', 'utf-8')) # Follow Yaw
else: self.ser.write(bytes('5', 'utf-8')) # Follow Center All
self.ser.flushInput()
LocoRobo.wait(0.01)
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 _throttle_control(self):
self.throttle_ctrl_loop = True
while self.throttle_ctrl_loop:
acc = self.robot.get_sensor_value(Data.ACCELEROMETER)
x = acc['x']
y = acc['y']
z = acc['z']
z_angle = math.atan(z / math.sqrt(y**2 + x**2))
z_angle = max(-self.MAX_ANGLE, min(z_angle * 180 / math.pi, self.MAX_ANGLE))
if z_angle > 30 : self.ser.write(bytes('7', 'utf-8')) # Throttle Inc
elif z_angle < -30 : self.ser.write(bytes('6', 'utf-8')) # Throttle Dec
self.ser.flushInput()
LocoRobo.wait(0.01)
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 _fly(self):
self.fly_loop = True
NUM_POINTS = 4 # Exponential moving average setup
multp = (2/(NUM_POINTS+1)) #
pitchList = [0] * NUM_POINTS #
pitchList = deque(pitchList) #
pitchEMA = 0 #
rollList = [0] * NUM_POINTS #
rollList = deque(rollList) #
yawEMA = 0 #
while self.fly_loop:
pitch = unpack_from ('!f', self.phoneData, 40) # 40 = byte offset of Orientation Pitch
roll = unpack_from ('!f', self.phoneData, 44) # 44 = byte offset of Orientation Roll(used as Yaw)
pitchList.pop()
rollList.pop()
pitchList.appendleft( float(pitch[0]) )
rollList.appendleft( float(roll[0]) )
pitchEMA = ( ( pitchList[0] - pitchEMA ) * multp + pitchEMA ) # Pitch Moving Average
yawEMA = ( ( rollList[0] - yawEMA ) * multp + yawEMA ) # Yaw Moving Average
pitch_angle = int(pitchEMA)
yaw_angle = int(yawEMA)
if pitch_angle > (self.MAX_ANGLE-10) : self.ser.write(bytes('X', 'utf-8')) # Pitch Forward HI
elif pitch_angle > 55 : self.ser.write(bytes('W', 'utf-8')) # Pitch Forward MED
elif pitch_angle > 20 : self.ser.write(bytes('w', 'utf-8')) # Pitch Forward LOW
elif pitch_angle < -(self.MAX_ANGLE-10) :self.ser.write(bytes('x', 'utf-8')) # Pitch Backward HI
elif pitch_angle < -55 : self.ser.write(bytes('S', 'utf-8')) # Pitch Backward MED
elif pitch_angle < -20 : self.ser.write(bytes('s', 'utf-8')) # Pitch Backward LOW
else : self.ser.write(bytes('3', 'utf-8')) # Pitch center
if yaw_angle > (self.MAX_ANGLE-10) : self.ser.write(bytes('Z', 'utf-8')) # Yaw Right HI
elif yaw_angle > 45: self.ser.write(bytes('D', 'utf-8')) # Yaw Right MED
elif yaw_angle > 20: self.ser.write(bytes('d', 'utf-8')) # Yaw Right LOW
elif yaw_angle < -(self.MAX_ANGLE-10) : self.ser.write(bytes('z', 'utf-8')) # Yaw Left HI
elif yaw_angle < -45 : self.ser.write(bytes('A', 'utf-8')) # Yaw Left MED
elif yaw_angle < -20 : self.ser.write(bytes('a', 'utf-8')) # Yaw Left LOW
else : self.ser.write(bytes('1', 'utf-8')) # Yaw center
LocoRobo.wait(0.01)
# 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()
# If we are on the main thread, run the program
if __name__ == "__main__":
try:
main()
except:
LocoRobo.stop()
raise
LocoRobo.stop()
# For compatibility with webapp's python, we can't use finally.
# If you are using local python, you can do the following
#
# try:
# main()
# finally:
# LocoRobo.stop()
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()
dist_val = 0
# Remove first element of each list
dist_data.pop(0)
dist_data.append(dist_val)
axis_plot.clear()
axis_plot.plot(indicies, dist_data, 'r', label='X Axis')
axis_plot.set_title('Axis Plot')
axis_plot.set_xlabel('Time')
axis_plot.set_ylabel('Distance (cm)')
axis_plot.grid(True)
axis_plot.legend(loc='best')
animation = FuncAnimation(fig, update, interval=50)
# Output of FuncAnimation MUST BE USED or it does not work
if animation:
pass
plt.show()
print(__name__)
# If we are on the main thread, run the program
if __name__ == "__main__":
LocoRobo.graph(run_robot=run_robot, run_graph=graph)
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')
# If we are on the main thread, run the program
if __name__ == "__main__":
try:
main()
finally:
# If the program has been stopped by the user, KeyboardInterrupt
# Is thrown
LocoRobo.stop()
drone.open_serial()
user_control(drone)
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()
if __name__ == "__main__":
try:
main()
except:
LocoRobo.stop()
