def __init__(self):
"""Initialisation."""
self.robot = PiBot()
if self.robot.is_simulation():
self.is_simulation = True
self.speed = 15
else:
self.is_simulation = False
self.speed = 13
self.speed_r = self.speed
self.speed_l = self.speed
self.problem_solved = False
self.state = "working"
self.adjust_left = 0
self.adjust_right = 0
"""Robot."""
from PiBot import PiBot # robot class
robot = PiBot() # robot object
a = robot.get_left_line_sensors()
robot.set_wheels_speed(30)
while a[0] > 0:
robot.set_wheels_speed(50)
a = robot.get_left_line_sensors()
print(a)
robot.sleep(0.01)
for i in range(50):
robot.set_wheels_speed(50)
robot.sleep(0.01)
robot.set_wheels_speed(30)
robot.sleep(0.05)
robot.set_wheels_speed(0)
print("STARTING ROBOT.PY")
from PiBot import PiBot
print("IMPORTED PIBOT")
import rospy
print("IMPORTED ROSPY")
# Create a robot instance
robot = PiBot()
left_enc_0 = 0
right_enc_0 = 0
def correction_start():
left_enc_0 = robot.get_right_wheel_encoder()
right_enc_0 = robot.get_left_wheel_encoder()
def correction():
left_enc = robot.get_right_wheel_encoder() - left_enc_0
right_enc = robot.get_left_wheel_encoder() - right_enc_0
return (left_enc - right_enc) / 2 # / 10
# Get distance from object using the front middle IR sensor
mid_dist = robot.get_front_middle_laser()
goal = 0.18
class Robot:
"""Do robot."""
def __init__(self):
"""Initialisation."""
self.robot = PiBot()
if self.robot.is_simulation():
self.is_simulation = True
self.speed = 15
else:
self.is_simulation = False
self.speed = 13
self.speed_r = self.speed
self.speed_l = self.speed
self.problem_solved = False
self.state = "working"
self.adjust_left = 0
self.adjust_right = 0
def set_speed(self, percentage):
"""
Set wheel speeds, but treats backwards as forwards and vice versa. Mostly for shorter typing.
:param percentage: percentage
:return: None
"""
self.robot.set_wheels_speed(-percentage)
def set_speed_r(self, percentage):
"""
Set right wheel speed as negative of perc(entage) because treating back as front.
:param percentage: percentage
:return: None
"""
self.robot.set_right_wheel_speed(-percentage)
def set_speed_l(self, percentage):
"""
Set left wheel speed as negative or perc(entage) because treating back as front.
:param percentage: percentage
:return: None
"""
self.robot.set_left_wheel_speed(-percentage)
def close_and_lift_grabber(self):
"""Function sets robot's grabber in a highest position possible."""
self.robot.close_grabber(95)
self.robot.set_grabber_height(95)
def sense(self):
"""sensing."""
self.left_straight = self.robot.get_rear_left_straight_ir()
self.left_diagonal = self.robot.get_rear_left_diagonal_ir()
self.left_side = self.robot.get_rear_left_side_ir()
self.right_straight = self.robot.get_rear_right_straight_ir()
self.right_diagonal = self.robot.get_rear_right_diagonal_ir()
self.right_side = self.robot.get_rear_right_side_ir()
self.front_middle = self.robot.get_front_middle_ir()
self.front_right = self.robot.get_front_right_ir()
def rotate_x_degrees(self, degrees):
"""
Robot does a turn to the chosen direction.
If degree > 0 --> pivot to the right.
If degree < 0 --> pivot to the left.
"""
# calculate amount of degrees encoders have to turn
degrees_to_spin = (self.robot.AXIS_LENGTH * degrees) / self.robot.WHEEL_DIAMETER
sign = (abs(degrees) // degrees)
left_start = self.robot.get_left_wheel_encoder()
right_start = self.robot.get_right_wheel_encoder()
self.reset_adjust()
while abs(self.robot.get_left_wheel_encoder() - left_start) < abs(degrees_to_spin) and abs(
self.robot.get_right_wheel_encoder() - right_start) < abs(degrees_to_spin):
self.adjust_speed(right_start, left_start, sign, -1)
self.reset_adjust()
self.set_speed(0)
def plan(self):
"""follow the wall on the left."""
self.reset_adjust()
if self.left_straight < 0.043 or self.left_diagonal < 0.043 or self.left_side < 0.043:
self.state = "rotating"
else:
self.state = "moving forward"
if self.right_straight >= 0.045 and self.right_diagonal >= 0.045 and self.right_side >= 0.045 and self.robot.get_front_middle_ir() >= 0.70 and self.robot.get_front_right_ir() >= 0.70:
self.problem_solved = True
self.robot.set_grabber_height(50)
def act(self):
"""acting."""
print("Acting...")
if self.state == "rotating":
self.rotate_x_degrees(-15)
else:
self.set_speed_l(13)
self.set_speed_r(24)
def adjust_speed(self, right_start, left_start, sign, rotation=1):
"""Adjust wheel speeds based on encoder difference's difference."""
diff_encoders = abs(self.robot.get_right_wheel_encoder() - right_start) - abs(
self.robot.get_left_wheel_encoder() - left_start)
if diff_encoders > 5:
if self.adjust_left < 7:
self.adjust_left += 1
self.adjust_right = 0
elif diff_encoders < -5:
if self.adjust_right < 7:
self.adjust_right += 1
self.adjust_left = 0
if self.adjust_left > 0 and self.adjust_right > 0:
self.adjust_left -= 1
self.adjust_right -= 1
self.set_speed_l(rotation * sign * (self.speed + self.adjust_left))
self.set_speed_r(sign * (self.speed + self.adjust_right))
def reset_adjust(self):
"""Reset wheel adjustments."""
self.adjust_left = 0
self.adjust_right = 0
def main(self):
"""Main loop function."""
self.close_and_lift_grabber()
while not self.problem_solved:
self.sense()
self.plan()
self.act()
rospy.sleep(0.05)
self.set_speed(0)
"""Robothard."""
from PiBot import PiBot # robot class
robot = PiBot() # robot object
for i in range(2100):
a = robot.get_left_line_sensors()
b = robot.get_right_line_sensors()
if a[0] > 90 and b[0] > 90:
robot.set_wheels_speed(50)
robot.sleep(0.01)
else:
robot.set_left_wheel_speed(2)
robot.set_right_wheel_speed(0)
robot.sleep(0.01)
robot.set_wheels_speed(0)
robot.done()
# Enable parent directory import
import sys
import os
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
sys.path.append(
os.path.abspath(os.path.join(os.path.dirname(__file__), "../..")))
"""Line follower bronze."""
from PiBot import PiBot
import time
robot = PiBot()
robot.set_grabber_height(10)
prev_time = robot.get_time()
for i in range(100):
objects = robot.get_camera_objects()
robot.set_wheels_speed(0)
time = robot.get_time() - prev_time
print(f"{time}: {objects}")
prev_time = robot.get_time()
robot.set_wheels_speed(0)
from PiBot import PiBot
import time
if __name__ == "__main__":
while True:
try:
robot = PiBot()
robot.set_wheels_speed(0)
break
except Exception as e:
time.sleep(1)
continue
"""O2."""
from PiBot import PiBot
import rospy
from math import cos, sin, sqrt, asin, pi, degrees, radians
import math
GAIN = 50
robot = PiBot()
def fmir_buffer_init():
"""
Fill fmir buffer with average value over 10 measurements. Only done once at the beginning.
:return: last_fmir, 4-element fmir buffer and fmir, all of them average values.
"""
# take 10 measurements, get their average and fill buffer
total = 0
for i in range(10):
total += robot.get_front_middle_ir()
rospy.sleep(0.05)
fmir_average = total / (i + 1)
return fmir_average, [fmir_average, fmir_average,
fmir_average], fmir_average
def fmir_buffering(variables):
"""
See if fmir value is correct. If not, return last fmir.
:param variables: variables dict
:return: variables dict with new fmir and buffer
if side == 1:
robot.set_left_wheel_speed(currentspeed + speed)
robot.set_right_wheel_speed(currentspeed - speed)
while lencgoal > robot.get_left_wheel_encoder():
rospy.sleep(0.05)
else:
robot.set_left_wheel_speed(currentspeed - speed)
robot.set_right_wheel_speed(currentspeed + speed)
while lencgoal < robot.get_left_wheel_encoder():
rospy.sleep(0.05)
robot.set_wheels_speed(currentspeed)
# Create a robot instance
robot = PiBot()
# Get distance from object using the front middle IR sensor
distance_from_object = robot.get_front_middle_ir()
# Drive towards object
robot.set_wheels_speed(30)
while distance_from_object > 0.18:
distance_from_object = robot.get_front_middle_ir()
print(distance_from_object)
rospy.sleep(0.05)
robot.set_wheels_speed(0)
turn(90, 0, 17, 0)
lenc = robot.get_left_wheel_encoder()
def grab(r):
r.set_grabber_height(60)
r.close_grabber(5)
def open(r):
r.set_grabber_height(10)
r.close_grabber(5)
def grab_test(r):
grab(r)
r.sleep(2)
up(r)
r.set_wheels_speed(11)
r.sleep(8)
open(r)
r.set_wheels_speed(0)
if __name__ == "__main__":
robot = PiBot()
robot.set_wheels_speed(0)
#grab_test(robot)
while True:
print("l {} m {} r {}".format(robot.get_front_left_laser(),
robot.get_front_middle_laser(),
robot.get_front_right_laser()))
robot.sleep(0.2)
"""PR14. Robot."""
from PiBot import PiBot
robot = PiBot()
right_colour = robot.get_third_line_sensor_from_right()
left_colour = robot.get_third_line_sensor_from_left()
robot.set_wheels_speed(50)
# robot will travel until sensors see the line.
while left_colour > 200 and right_colour > 200:
right_colour = robot.get_third_line_sensor_from_right()
left_colour = robot.get_third_line_sensor_from_left()
robot.sleep(0.05)
# when he saw the line, for loop to make robot move 5 'seconds' more.
for second in range(5):
robot.set_wheels_speed(100)
robot.sleep(0.05)
robot.set_wheels_speed(0)
robot.done()