autonomous.py 文件源码

def drive_to_wall(self, initial_call):
        if initial_call:
            self.profilefollower.stop()
            if self.target == Targets.Centre:
                peg_range = self.centre_airship_distance
            else:
                peg_range = 1.5
            peg_range -= self.centre_to_front_bumper
            peg_range += 0.3
            r = self.range_filter.range
            print("AUTO DRIVE WALL RANGE: %s" % (self.range_finder.getDistance()))
            print("AUTO DRIVE WALL FILTER RANGE: %s" % (self.range_filter.range))
            # 40 is range finder max distance, better failure mode than inf or really small
            if not math.isfinite(r):
                r = 40
            elif r < 0.5:
                r = 40
            to_peg = None
            if r > (2 if self.target != Targets.Centre else 4):
                print("DEAD RECKON AUTO")
                to_peg = generate_trapezoidal_trajectory(
                    0, 0, peg_range + 0.1, 0, self.displace_velocity,
                    self.displace_accel, -self.displace_decel,
                    Chassis.motion_profile_freq)
            else:
                print("RANGE AUTO")
                to_peg = generate_trapezoidal_trajectory(0, 0,
                    self.range_finder.getDistance() - self.lidar_to_front_bumper + 0.1,
                    0, self.displace_velocity, self.displace_accel, -self.displace_decel,
                    Chassis.motion_profile_freq)
            self.profilefollower.modify_queue(self.bno055.getHeading(),
                linear=to_peg, overwrite=True)
            self.profilefollower.execute_queue()
            self.manipulategear.engage()

        if not self.profilefollower.executing:
            self.next_state("deploying_gear")