if flip == 0:
spin *= (-1
) # Make it negative 50% of the time
# Timer for the backward movement, then the spin
if (time.time() -
moveHelper) < backTime: # Backward movement stuff
set_forward_value = -100
set_spin_value = 0
elif (time.time() - moveHelper) < (
backTime + spinTime): # Spin movement stuff
set_forward_value = 0
set_spin_value = spin # Determined above when bumped
else: # Normal movement stuff
set_spin_value = DHTurn(
angle, desired_heading, epsilon
) # Determine the spin speed to turn toward the desired heading
set_forward_value = DDSpeed(angle, desired_heading,
desired_distance)
# If the light sensors detect something, then slow down
if light_bumper > 0:
# Cut the speed in half
set_forward_value = set_forward_value // 2
set_spin_value = set_spin_value // 2
# Increment forward movement speed toward the set speed
if forward_value < set_forward_value: # If it is less than the set speed...
forward_value += speed_step # Increment the speed by a step
if forward_value > set_forward_value: # If the speed went to far...
forward_value = set_forward_value # Set it to the set speed
if line_dist < ROOMBA_RADIUS: # If distance is smaller than the radius of the Roomba... can_form_neighbors = False # point_1 and point_2 are not neighbors break # Don't need to check any other wall_points # else, check for the next wall point if can_form_neighbors: # If no wall_points are close enough to the line segment from goal to point... neighbors = Create_Neighbors(point_1, point_2, neighbors) # point_1 and point_2 are neighbors else: # If a wall point is too close... can_form_neighbors = True # Reset boolean for next iteration in "for wall in wall_points" take_a_break = True # Used to break out of "for p in path:" break # while desired_distance < 2: # Fourth Phase: Move To Next Point (default) else: # Get the spin and move speeds for heading to p spin_value = DHTurn(angle, desired_heading, 0.5) move_speed = DDSpeed(angle, desired_heading, desired_distance) if lightVal > 0: # If the light bumpers detect something... # Cut speeds in half #maybe just do the move_speed, and not spin_speed? move_speed = move_speed // 2 spin_value = spin_value // 2 # Determine Roomba movement speeds based on set values # Increment move speed by move_step to avoid sudden changes in movement. if actual_move < move_speed: actual_move += move_step if actual_move > move_speed: actual_move = move_speed if actual_move > move_speed:
GPIO.output(yled, GPIO.LOW) # Indicate setup sequence complete
# Main Code #
angle = imu.CalculateHeading() # Get initial heading information
forward = 0
desired_heading = 0
data_base = time.time()
reset_base = time.time()
while True:
try:
# Update heading of Roomba
angle = imu.CalculateHeading()
spin = DHTurn(angle, desired_heading,
epsilon) # Value needed to turn to desired heading point
Roomba.Move(forward, spin) # Move Roomba to desired heading point
if spin == 0:
GPIO.output(yled, GPIO.LOW) # Indicate Roomba is not turning
else:
GPIO.output(yled, GPIO.HIGH) # Indicate Roomba is turning
if (time.time() - reset_base) > reset_timer:
desired_heading += 90
if desired_heading >= 360:
desired_heading -= 360
reset_base += reset_timer
# Print heading data to monitor every second
if (time.time() - data_base) > data_timer: # After one second
math.radians(angle - (0.5 * angle_change)))
x_pos += delta_x_pos
y_pos += delta_y_pos
final_distance = distance
#appending to the lists
l_counts_list.append(l_counts)
r_counts_list.append(r_counts)
angle_list.append(angle)
x_pos_list.append(x_pos)
y_pos_list.append(y_pos)
data_time_list.append(data_time)
spin_value = DHTurn(
angle, 0.0,
0.5) # Determine the spin speed to turn toward the desired heading
Roomba.Move(forward_value, spin_value)
l_counts_current = l_counts
r_counts_current = r_counts
#end if roomba.available > 0
#end while loop
Roomba.Move(0, 0)
time.sleep(0.5)
print("Roomba GOING STRAIGHT TESTING", file=open("outputStraight.txt", "a"))
print("\nL Count", file=open("outputStraight.txt", "a"))
for i in range(len(l_counts_list)):
print("{:.3f}".format(l_counts_list[i]),
file=open("outputStraight.txt", "a"),
end="")
bumper_byte, l_counts_current, r_counts_current, l_speed, r_speed, light_bumper = Roomba.Query(
7, 43, 44, 42, 41, 45) # Read new wheel counts
# Request for the desired angle to turn to
# Print current angle of Roomba
print("Current Location: ({0:.3f}, {1:.3f})".format(x_pos, y_pos))
print("Current heading is {0:.3f}\n".format(angle))
desired_heading = float(input("Desired heading? "))
desired_distance = 0
data_time = 0.0 # 0 seconds initial
# Restart base timers
base = time.time()
query_base = time.time()
# Setting the original spin value.
spin_value = DHTurn(angle, desired_heading, epsilon)
print("\nThe Roomba angle will now be changed.\n")
# This while loop is for setting direction
while spin_value != 0:
try:
# Have the roomba move to the desired direction to check
Roomba.Move(forward_value, spin_value) # Spin the Roomba
# This conditional checks for the new angle and distance changes
if (time.time() - query_base
) > query_timer: # Every (query_timer) seconds...
l_counts, r_counts = Roomba.Query(
43, 44) # Read new wheel counts
# Record the current time since the beginning of loop