def decision(self, samplesRB, landerRB, obstaclesRB, rocksRB):
self.searching = True
target_angle = 0
if obstaclesRB is not None and not self.at_target:
for obstacle in obstaclesRB:
if obstacle[0] < 0.35:
# opencollection.open()
# tiltupcollection.up()
angle_scale = 1
if obstaclesRB is not None:
obstacle = obstaclesRB[0]
if obstacle[0] < 0.2:
angle_scale = 2.5
obs_x, obs_y = self.determinePos(
obstacle[0], obstacle[1])
# print("seem")
U = potentialField.getForce([0, 0], [0.5, 0],
[[obs_x, obs_y]])
# print(obs)
self.memory = [obs_x, obs_y]
self.start_timer = time.time()
elif self.memory is not None and time.time(
) - self.start_timer < 1:
obs_x, obs_y = self.determinePos(
self.memory[0], self.memory[1])
# print("Going off memory")
U = potentialField.getForce([0, 0], [0.5, 0],
[[obs_x, obs_y]])
else:
U = [0.1, 0]
obs_x, obs_y = self.determinePos(U[0], U[1])
self.memory = None
target_angle += motorControl.WrapToPi(
math.atan2(U[1], U[0]) * angle_scale)
# target_angle, target_mag = getForce(self)
accuracy = 5
# print("target angle: {:.2f} current angle: {:.2f} x: {:.2f} y: {:.2f}".format(target_angle, self.bearing, self.x, self.y))
# if abs(math.atan2(obs_y, obs_x)) > 90:
# self.move("back", "normal")
# elif math.isclose(self.bearing, target_angle, abs_tol=math.radians(accuracy)):
# self.move("forward", "normal")
# elif abs(self.bearing - target_angle) < math.pi:
# if self.bearing - target_angle < 0:
# self.move("left", "normal")
# elif self.bearing - target_angle > 0:
# self.move("right", "normal")
# elif abs(self.bearing - target_angle) > math.pi:
# if self.bearing - target_angle < 0:
# self.move("right", "normal")
# elif self.bearing - target_angle > 0:
# self.move("left", "normal")
if self.on_lander:
self.seaching = False
opencollection.open()
tiltupcollection.up()
holdSample.hold()
target_angle += 0
# self.move("forward", "normal")
if time.time() - self.start_timer > 2.5:
self.on_lander = False
self.start_time = time.time()
elif samplesRB is not None and not self.on_lander and not self.has_ball:
self.searching = False
led.led_state("collecting")
sample = samplesRB[0]
if sample[0] > 0.25:
speed = "normal"
accuracy = 10
else:
speed = "slow"
accuracy = 3
if self.at_target:
self.move("stop", "stop")
tiltdowncollection.down()
time.sleep(1)
closecollection.close()
time.sleep(1)
holdSample.hold()
self.has_ball = True
else:
opencollection.open()
holdSample.hold()
# print(sample[0])
if sample[0] < 0.135 or self.at_target:
self.move("stop", "stop")
self.at_target = True
return
target_angle += sample[1]
# avoid rocks too when moving to sample
# if rocksRB is not None:
if accuracy == 3:
if math.radians(-accuracy) < sample[1] < math.radians(
accuracy):
self.move("forward", speed)
elif sample[1] < math.radians(-accuracy):
self.move("right", speed)
elif sample[1] > math.radians(accuracy):
self.move("left", speed)
elif self.has_ball and landerRB is not None:
self.searching = False
led.led_state("SampleReturn")
lander = landerRB[0]
speed = "normal"
accuracy = 10
# print(lander[0])
if lander[0] < 0.5:
self.move("forward", "normal")
time.sleep(2.8)
self.move("stop", "stop")
tiltdowncollection.down()
time.sleep(1)
opencollection.open()
self.at_target = False
self.on_lander = True
self.has_ball = False
self.ingore_rocks = False
time.sleep(2)
holdSample.hold()
self.move("back", "normal")
time.sleep(2)
else:
closecollection.close()
holdSample.hold()
target_angle += lander[0]
# if math.radians(-accuracy) < lander[1] < math.radians(accuracy):
# self.move("forward", speed)
# elif lander[1] < math.radians(-accuracy):
# self.move("right", speed)
# elif lander[1] > math.radians(accuracy):
# self.move("left", speed)
elif self.has_ball and landerRB is None:
self.searching = True
led.led_state("SampleReturn")
closecollection.close()
holdSample.hold()
self.move("right", "normal")
elif rocksRB is not None and samplesRB is None and not self.has_ball and not self.ingore_rocks or self.at_rock:
self.searching = False
# Look for a rock to flip
led.led_state("searching")
rock = rocksRB[0]
tiltdowncollection.down()
closecollection.close()
if rock[0] > 0.35 and not self.at_target:
speed = "normal"
accuracy = 10
else:
self.at_rock
speed = "slow"
accuracy = 3
if self.at_target:
tiltdowncollection.down()
closecollection.close()
self.move("forward", "slow")
time.sleep(1)
liftrock.lift()
time.sleep(1)
self.at_target = False
self.has_ball = False
self.on_lander = False
self.ingore_rocks = True
self.at_rock = False
opencollection.open()
self.move("back", "slow")
time.sleep(2)
if rock[0] < 0.145 or self.at_target:
self.move("forward", "slow")
time.sleep(0.5)
self.move("stop", "stop")
self.at_target = True
return
target_angle += rock[1]
if accuracy == 3:
if math.radians(-accuracy) < rock[1] < math.radians(accuracy):
self.move("forward", speed)
elif rock[1] < math.radians(-accuracy):
self.move("right", speed)
elif rock[1] > math.radians(accuracy):
self.move("left", speed)
else:
led.led_state("searching")
self.move("right", "normal")
return
if not self.searching:
if accuracy == 10:
if math.radians(10) < target_angle < math.radians(10):
self.move("forward", "normal")
elif target_angle < math.radians(-10):
self.move("right", "normal")
elif target_angle > math.radians(10):
self.move("right", "normal")
def decision(self, samplesRB, landerRB, obstaclesRB, rocksRB):
if Sample_demo:
if samplesRB is not None:
led.led_state("collecting")
sample = samplesRB[0]
if sample[0] > 0.25:
speed = "normal"
accuracy = 10
else:
speed = "slow"
accuracy = 3
# print(speed)
if self.at_target:
tiltdowncollection.down()
time.sleep(1)
closecollection.close()
time.sleep(1)
holdSample.hold()
self.done = True
led.led_state("SampleReturn")
else:
opencollection.open()
holdSample.hold()
if sample[0] < 0.114 or self.at_target:
self.move("forward", "stop")
self.at_target = True
return
if math.radians(-accuracy) < sample[1] < math.radians(
accuracy):
self.move("forward", speed)
elif sample[1] < math.radians(-accuracy):
self.move("right", speed)
elif sample[1] > math.radians(accuracy):
self.move("left", speed)
else:
led.led_state("searching")
self.move("right", "normal")
return
elif Rock_demo:
if rocksRB is not None:
rock = rocksRB[0]
if rock[0] > 0.25:
speed = "normal"
accuracy = 10
else:
speed = "slow"
accuracy = 3
if self.at_target:
tiltdowncollection.down()
time.sleep(1)
liftrock.lift()
self.done = True
else:
closecollection.close()
tiltdowncollection.down()
print(rock[0])
if rock[0] < 0.145 or self.at_target:
time.sleep(0.5)
self.move("forward", "stop")
self.at_target = True
return
if math.radians(-accuracy) < rock[1] < math.radians(accuracy):
self.move("forward", speed)
elif rock[1] < math.radians(-accuracy):
self.move("right", speed)
elif rock[1] > math.radians(accuracy):
self.move("left", speed)
else:
opencollection.open()
self.move("right", "normal")
return
elif obstacle_avoidance:
opencollection.open()
tiltupcollection.up()
angle_scale = 1
if obstaclesRB is not None:
obstacle = obstaclesRB[0]
if obstacle[0] < 0.2:
angle_scale = 2.5
obs_x, obs_y = self.determinePos(obstacle[0], obstacle[1])
# print("seem")
U = potentialField.getForce([0, 0], [0.5, 0], [[obs_x, obs_y]])
# print(obs)
self.memory = [obs_x, obs_y]
self.start_timer = time.time()
elif self.memory is not None and time.time() - self.start_timer < 1:
obs_x, obs_y = self.determinePos(self.memory[0],
self.memory[1])
# print("Going off memory")
U = potentialField.getForce([0, 0], [0.5, 0], [[obs_x, obs_y]])
else:
U = [0.1, 0]
obs_x, obs_y = self.determinePos(U[0], U[1])
target_angle = motorControl.WrapToPi(
math.atan2(U[1], U[0]) * angle_scale)
# target_angle, target_mag = getForce(self)
accuracy = 5
# print("target angle: {:.2f} current angle: {:.2f} x: {:.2f} y: {:.2f}".format(target_angle, self.bearing, self.x, self.y))
if abs(math.atan2(obs_y, obs_x)) > 90:
self.move("back", "normal")
elif math.isclose(self.bearing,
target_angle,
abs_tol=math.radians(accuracy)):
self.move("forward", "normal")
elif abs(self.bearing - target_angle) < math.pi:
if self.bearing - target_angle < 0:
self.move("left", "normal")
elif self.bearing - target_angle > 0:
self.move("right", "normal")
elif abs(self.bearing - target_angle) > math.pi:
if self.bearing - target_angle < 0:
self.move("right", "normal")
elif self.bearing - target_angle > 0:
self.move("left", "normal")
def decision(self, samplesRB, landerRB, obstaclesRB, rocksRB):
if self.on_lander:
holdSample.hold()
self.move("forward", 200)
if time.time() - self.start_time > 4:
self.on_lander = False
self.start_time = time.time()
elif samplesRB is not None and not self.on_lander and not self.has_ball:
sample = samplesRB[0]
if sample[0] > 0.25:
speed = 200
accuracy = 10
else:
speed = 200
accuracy = 3
if self.at_target:
tiltdowncollection.down()
time.sleep(1)
closecollection.close()
time.sleep(1)
holdSample.hold()
self.has_ball = True
else:
opencollection.open()
holdSample.hold()
if sample[0] < 0.114 or self.at_target:
self.move("forward", 0)
self.at_target = True
return
if math.radians(-accuracy) < sample[1] < math.radians(accuracy):
self.move("forward", speed)
elif sample[1] < math.radians(-accuracy):
self.move("right", speed)
elif sample[1] > math.radians(accuracy):
self.move("left", speed)
elif self.has_ball and landerRB is not None and not self.sample_collected:
lander = landerRB[0]
speed = 200
accuracy = 10
print(lander[0])
if lander[0] < 0.5:
time.sleep(2)
opencollection.open()
self.sample_collected = True
else:
closecollection.close()
holdSample.hold()
if math.radians(-accuracy) < lander[1] < math.radians(accuracy):
self.move("forward", speed)
elif lander[1] < math.radians(-accuracy):
self.move("right", speed)
elif lander[1] > math.radians(accuracy):
self.move("left", speed)
elif self.sample_collected and rocksRB is not None:
# Look for a rock to flip
rock = rocksRB[0]
if rock[0] > 0.25:
speed = 200
accuracy = 10
else:
speed = 120
accuracy = 3
if self.at_target:
tiltdowncollection.down()
time.sleep(1)
liftrock.lift()
self.done = True
else:
closecollection.close()
tiltdowncollection.down()
if rock[0] < 0.135 or self.at_target:
self.move("forward", 0)
self.at_target = True
return
if math.radians(-accuracy) < rock[1] < math.radians(accuracy):
self.move("forward", speed)
elif rock[1] < math.radians(-accuracy):
self.move("right", speed)
elif rock[1] > math.radians(accuracy):
self.move("left", speed)
else:
self.move("right", 300)
return
def decision(self, samplesRB, landerRB, obstaclesRB, rocksRB):
if Sample_demo:
if samplesRB is not None:
sample = samplesRB[0]
if sample[0] > 0.25:
speed = 200
accuracy = 10
else:
speed = 120
accuracy = 3
if self.at_target:
tiltdowncollection.down()
time.sleep(1)
closecollection.close()
time.sleep(1)
holdSample.hold()
self.done = True
else:
opencollection.open()
holdSample.hold()
if sample[0] < 0.114 or self.at_target:
self.move("forward", 0)
self.at_target = True
return
if math.radians(-accuracy) < sample[1] < math.radians(
accuracy):
self.move("forward", speed)
elif sample[1] < math.radians(-accuracy):
self.move("right", speed)
elif sample[1] > math.radians(accuracy):
self.move("left", speed)
else:
self.move("right", 200)
return
elif Rock_demo:
if rocksRB is not None:
rock = rocksRB[0]
if rock[0] > 0.25:
speed = 200
accuracy = 10
else:
speed = 120
accuracy = 3
if self.at_target:
tiltdowncollection.down()
time.sleep(1)
liftrock.lift()
self.done = True
else:
closecollection.close()
tiltdowncollection.down()
if rock[0] < 0.135 or self.at_target:
self.move("forward", 0)
self.at_target = True
return
if math.radians(-accuracy) < rock[1] < math.radians(accuracy):
self.move("forward", speed)
elif rock[1] < math.radians(-accuracy):
self.move("right", speed)
elif rock[1] > math.radians(accuracy):
self.move("left", speed)
else:
self.move("right", 200)
return
elif obstacle_avoidance:
if rocksRB is not None:
rock = rocksRB[0]
rock_x, rock_y = self.determinePos(rock[0], rock[1])
U = potentialField.getForce([self.x, self.y], [0.75, 0],
[[rock_x, rock_y]])
self.memory = [rock_x, rock_y]
elif self.memory is not None:
U = potentialField.getForce([self.x, self.y], [0.75, 0],
[self.memory])
target_angle = math.atan2(U[1], U[0])
# target_angle, target_mag = getForce(self)
accuracy = 5
# print("target angle: {:.2f} current angle: {:.2f} x: {:.2f} y: {:.2f}".format(target_angle, self.bearing, self.x, self.y))
if math.isclose(self.bearing,
target_angle,
abs_tol=math.radians(accuracy)):
self.move("forward", 150)
elif abs(self.bearing - target_angle) < math.pi:
if self.bearing - target_angle < 0:
self.move("left", 250)
elif self.bearing - target_angle > 0:
self.move("right", 200)
elif abs(self.bearing - target_angle) > math.pi:
if self.bearing - target_angle < 0:
self.move("right", 200)
elif self.bearing - target_angle > 0:
self.move("left", 200)