Example #1
0
    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")
Example #2
0
    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")
Example #3
0
    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
Example #4
0
    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)