示例#1
0
 def handleEntering(self):
     if (self.entering_enabled):
         if (not self.started_entering):
             sensors.setLeftMotor(self.entering_fwd_speed)
             sensors.setRightMotor(self.entering_fwd_speed)
             self.started_entering = True
         else:
             if (not self.getLeftAvailable()
                     and not self.getRightAvailable()):
                 # drive until left and right see the walls
                 # motor off may be removed
                 time.sleep(0.2)
                 sensors.setMotorOff()
                 print("Entered Maze")
                 self.gui.log_message("Entered Maze")
                 map.logStartPoint(self.robot.odometry.getFieldToVehicle().transformBy( \
                                 RigidTransform2d(Translation2d(self.hallway_width/2, 0), \
                                 Rotation2d(1, 0, False))).getTranslation())
                 self.state = self.Drive_State_t.HALLWAY_FOLLOWING
                 self.started_entering = False
                 self.setHeading(0)
     else:
         print("Entering bypassed")
         self.gui.log_message("Entering Bypassed")
         self.state = self.Drive_State_t.HALLWAY_FOLLOWING
         map.logStartPoint(
             self.robot.odometry.getFieldToVehicle().getTranslation())
         self.setHeading(0)
示例#2
0
    def loop(self):
        loop_counter = 0
        self.current = RigidTransform2d(Translation2d(0, 0),
                                        Rotation2d.fromDegrees(0))

        while (self.enabled):
            loop_counter += 1

            #self.gui.log_message("robot main")

            # Able to decrease sensor update frequency
            if (loop_counter % 1 == 0):
                sensors.updateSensors()

            self.odometry.updateOdometry()
            self.drive.updateDrive()

            if (loop_counter % 1 == 0):
                self.current = self.odometry.getFieldToVehicle()

                self.gui.log_pos(self.current)
                self.gui.log_sonics(sensors.getLeftWallDistance(),
                                    sensors.getForwardWallDistance(),
                                    sensors.getRightWallDistance())
                self.gui.log_mag(sensors.getMagneticMagnitude())
                self.gui.log_ir(0.0, 0.0)
                self.gui.log_state(self.drive.state)

            if (loop_counter >= 1000):
                loop_counter = 0

            time.sleep(self.cycle_time)

        sensors.shutdown()
 def integrateForwardKinematics(self, current_pose, left, right, heading):
     rads = current_pose.getRotation().inverse().rotateBy(
         heading).getRadians()
     with_gyro = self.forwardKinematics(left, right, rads)
     return current_pose.transformBy(
         RigidTransform2d.fromVelocity(with_gyro))
 def forwardKinematics(self, left, right, rads):
     return RigidTransform2d.Delta((left + right) / 2, 0, rads)
 def reset(self):
     self.current_pos = RigidTransform2d(Translation2d(0, 0),
                                         Rotation2d.fromDegrees(90))
     self.last_left_encoder_reading = 0
     self.last_right_encoder_reading = 0
 def __init__(self, dt):
     self.current_pos = RigidTransform2d(Translation2d(0, 0),
                                         Rotation2d.fromDegrees(90))
     self.last_left_encoder_reading = 0
     self.last_right_encoder_reading = 0
     self.dt = dt
示例#7
0
    def handleHazardScanning(self):
        if (self.hzd_scan_state == 0):
            # begins looking for hazards in all three directions, goal is to narrow down which directions need to be scanned
            sensors.updateSensors()
            self.hzd_lt_avail = self.getLeftAvailable()
            self.hzd_rt_avail = self.getRightAvailable()
            self.hzd_ft_avail = self.getFrontAvailable()
            # check to ensure we are still in a maze
            open_count = 0
            open_count += 1 if self.hzd_lt_avail else 0
            open_count += 1 if self.hzd_rt_avail else 0
            open_count += 1 if self.hzd_ft_avail else 0
            self.gui.log_message("3: Beginning of Hazard Scanning")
            print("Left Wall: " + str(self.hzd_lt_avail) + " Fwd: " +
                  str(self.hzd_ft_avail) + " Right: " + str(self.hzd_rt_avail))
            print("Sum: " + str(open_count))
            #if(self.getLeftAvailable() or self.getRightAvailable()):
            if (self.prev_all_open and open_count > 2):  # >= 2
                # we have exited the maze lol yeet
                # exit location 1 backward and 1 to the right
                self.gui.log_message("Maze Exited")
                curr_rigid = self.robot.odometry.getFieldToVehicle()
                # current intersection
                map.removePoint(
                    curr_rigid.transformBy(
                        RigidTransform2d(Translation2d(0, -5),
                                         Rotation2d(1, 0,
                                                    False))).getTranslation())
                # previous intersection
                map.removePoint(
                    curr_rigid.transformBy(
                        RigidTransform2d(
                            Translation2d(0, -5 - self.hallway_width),
                            Rotation2d(1, 0, False))).getTranslation())
                map.logEndPoint(curr_rigid.transformBy( \
                                RigidTransform2d(Translation2d(self.hallway_width, -self.hallway_width - 5), \
                                Rotation2d(1, 0, False))).getTranslation())
                self.state = self.Drive_State_t.DELIVERING
            self.prev_all_open = open_count == 3

            hazard_detection.startDirectionalScan(not self.getLeftAvailable(),
                                                  not self.getFrontAvailable(),
                                                  not self.getRightAvailable())
            print("Left Haz: " + str(hazard_detection.leftHazardPresent()) +
                  " Front Haz: " + str(hazard_detection.frontHazardPresent()) +
                  " Right Haz: " + str(hazard_detection.rightHazardPresent()))
            self.hzd_start_rigid = self.robot.odometry.getFieldToVehicle()
            self.hzd_start_heading = self.robot.odometry.getFieldToVehicle(
            ).getRotation()
            self.hzd_scan_state += 1
        else:
            # Updates all scan directions, chooses which to update based on if heading is in correct direction
            hazard_detection.updateAllScans(
                self.robot.odometry.getFieldToVehicle().getRotation(),
                self.hzd_start_heading)

        if (self.hzd_scan_state == 1):
            # see whats left to scan
            sensors.updateSensors()
            print("Left Haz: " + str(hazard_detection.leftHazardPresent()) +
                  " Front Haz: " + str(hazard_detection.frontHazardPresent()) +
                  " Right Haz: " + str(hazard_detection.rightHazardPresent()))

            #if(hazard_detection.needToScanFwd()):

            if (hazard_detection.needToScanLeft()):
                # turn left 90
                self.hzd_turn_direction = map.Turn_Direction_Robot_t.LFT
                #self.gui.log_message("Scanning Left")
            elif (hazard_detection.needToScanRight()):
                # turn right 90
                self.hzd_turn_direction = map.Turn_Direction_Robot_t.RHT
                #self.gui.log_message("Scanning Right")
            else:
                #completely done scanning!!!
                #self.gui.log_message("4: Done scanning, turning to exit direction")
                if (self.hzd_rt_avail
                        and not hazard_detection.rightHazardPresent()):
                    self.hzd_turn_direction = map.Turn_Direction_Robot_t.RHT
                    self.gui.log_message("Turning Right")
                elif (self.hzd_ft_avail
                      and not hazard_detection.frontHazardPresent()):
                    self.hzd_turn_direction = map.Turn_Direction_Robot_t.FWD
                    self.gui.log_message("Turning Forward")
                elif (self.hzd_lt_avail
                      and not hazard_detection.leftHazardPresent()):
                    self.hzd_turn_direction = map.Turn_Direction_Robot_t.LFT
                    self.gui.log_message("Turning Left")
                else:
                    # no available dirs, turn around
                    self.hzd_turn_direction = map.Turn_Direction_Robot_t.BCK
                    self.gui.log_message("Turning Around")

            #heading_offset = self.hzd_start_heading.inverse().rotateBy(self.robot.odometry.getFieldToVehicle().getRotation())
            heading_change = self.robot.odometry.getFieldToVehicle(
            ).getRotation().inverse().rotateBy(
                self.hzd_start_heading.rotateBy(
                    Rotation2d.fromDegrees(
                        self.hzd_turn_direction))).getDegrees()
            #heading_change = Rotation2d.fromDegrees(self.hzd_turn_direction).rotateBy(heading_offset).getDegrees()
            #heading_change = round((self.hzd_turn_direction - heading_offset)/90)*90
            heading_change = round(heading_change / 90) * 90
            print("Heading Change: " + str(heading_change))
            if (math.fabs(heading_change) > 45):
                # still need to change direction
                self.setHeading(heading_change)
                self.hzd_scan_state += 1
                mult = 1
                if (heading_change < 0):
                    mult = -1
                sensors.setLeftMotor(-self.int_turn_speed * mult)
                sensors.setRightMotor(self.int_turn_speed * mult)
            else:
                # we are already at the deired location!
                # move to last step in hazard scanning
                self.hzd_scan_state += 2

        elif (self.hzd_scan_state == 2):
            # Turning
            if (math.fabs(self.getHeadingError()) <= self.int_turn_tolerance):
                # turning complete
                #self.gui.log_message("Scan Turn Complete")
                sensors.setMotorOff()
                self.hzd_scan_state = 1

        elif (self.hzd_scan_state == 3):
            # log the hazards and scram to the exit intersection mode
            #self.gui.log_message("5: Done with hazard detection")
            hazard_detection.logAllScans(self.hzd_start_rigid)
            self.state = self.Drive_State_t.INTERSECTION_EXITING
            self.hzd_scan_state = 0
示例#8
0
    def handleHallwayFollowing(self):

        if (self.intersection_enabled):
            if (hazard_detection.irHazardExists()
                    or hazard_detection.magHazardExists()):
                # jeepers, get outta here!
                if (hazard_detection.irHazardExists()):
                    map.logHeatSource(
                        self.robot.odometry.getFieldToVehicle().transformBy(
                            RigidTransform2d(
                                Translation2d(
                                    hazard_detection.getIRDistanceLeft(), 0),
                                Rotation2d(1, 0, False))).getTranslation(),
                        sensors.getIRLevelLeft())
                    self.gui.log_message("IR Detected, turning around")
                if (hazard_detection.magHazardExists()):
                    map.logMagneticSource(
                        self.robot.odometry.getFieldToVehicle().transformBy(
                            RigidTransform2d(
                                Translation2d(
                                    hazard_detection.getIRDistanceLeft(), 0),
                                Rotation2d(1, 0, False))).getTranslation(),
                        sensors.getMagneticMagnitude())
                    self.gui.log_message("Mag Detected, turning around")
                self.state = self.Drive_State_t.TURNING_AROUND
                sensors.setMotorOff()
                time.sleep(1)
                return
            if (self.getLeftAvailable() or self.getRightAvailable()):
                sensors.setMotorOff()
                fwd_dist = sensors.getForwardWallDistance()
                #if(fwd_dist < 35):
                #    self.intersection_forward_dist = fwd_dist - 12
                #else:
                self.intersection_forward_dist = self.int_side_fwd_dist
                self.state = self.Drive_State_t.INTERSECTION_ENTERING
                self.gui.log_message("Left or right path open")
                time.sleep(1)
                return
            elif (not self.getFrontAvailable()):
                self.intersection_forward_dist = self.int_front_fwd_dist
                self.state = self.Drive_State_t.INTERSECTION_ENTERING
                self.gui.log_message("Forward closed")
                sensors.setMotorOff()
                time.sleep(1)
                return

        # >0 if too far to the right, <0 if too far left
        # if error >0 => correct by increasing right speed
        error = sensors.getLeftWallDistance() - sensors.getRightWallDistance()
        # use error to control difference in motor speed
        pError = error * self.kP * self.hallway_speed
        if (math.fabs(pError) > self.kLimit):
            pError = self.kLimit * (pError / math.fabs(pError))
        leftSpeed = self.hallway_speed
        rightSpeed = leftSpeed
        if (pError > 0):
            rightSpeed += pError
        else:
            leftSpeed -= pError
        # leftSpeed = self.hallway_speed - pError
        # rightSpeed = self.hallway_speed + pError

        heading_error = self.getHeadingError()
        lSpeed = self.hallway_speed - heading_error * self.kPHeading
        rSpeed = self.hallway_speed + heading_error * self.kPHeading

        #sensors.setLeftMotor(leftSpeed)
        #sensors.setRightMotor(rightSpeed)
        sensors.setLeftMotor(lSpeed)
        sensors.setRightMotor(rSpeed)