예제 #1
0
class DepthTest( ControlScript ):
    
    STATE_DIVING = "Diving"
    STATE_PERFORMING_SCANS = "Performing Scans"
    STATE_SURFACING = "Surfacing"
    
    #---------------------------------------------------------------------------
    def __init__( self, config, logger, playerPos3d, playerCompass, 
        playerDepthSensor, playerSonar = None ):
        
        ControlScript.__init__( self, config, logger, playerPos3d, playerCompass, 
            playerDepthSensor, playerSonar )
        
        self.scanDepth = -1.0   # TODO: Set from config file
        
        self.arbitrator = Arbitrator( playerPos3d, playerCompass, playerDepthSensor )
        self.arbitrator.setDesiredDepth( self.scanDepth )
        self.linearSpeed = 0.0
        self.setState( self.STATE_DIVING )
        self.driveTimer = time.time()
    
    #---------------------------------------------------------------------------
    def update( self ):
        
        curTime = time.time()
        
        if self.state == self.STATE_DIVING:

            if self.arbitrator.atDesiredDepth():
                self.arbitrator.setDesiredYaw( Maths.degToRad( 0.0 ) )
                self.setState( self.STATE_TURNING_TO_GATE_1 )
                
        elif self.state == self.STATE_SURFACING:
            
            if self.arbitrator.atDesiredDepth():
                self.arbitrator.setDesiredDepth( self.scanDepth )
                self.setState( self.STATE_FINISHED )
            
        elif self.state == self.STATE_FINISHED:
            # Nothig to do
            pass
        else:
            self.logger.logError( "Unrecognised state - surfacing" )
            self.setState( self.STATE_SURFACING )
        
        self.arbitrator.update( self.linearSpeed )
예제 #2
0
class OrangeBallScript ( ControlScript ):    
    
     #---------------------------------------------------------------------------
    def __init__( self, config, logger, playerPos3D, playerCompass, 
        playerDepthSensor, playerSonar = None ):
        
        self.arbitrator = Arbitrator( playerPos3D, playerCompass, playerDepthSensor )
        self.ballFinder = bouy_finder.BouyFinder()

        self.pitchAngle = -5.0                       # temporary value - zero pitch
        self.initYawAngle = Maths.degToRad( 85.0 )   # temporary value - heading towards the wall
        self.initDepth = 7460.0                      # temporary value - 0.5 metres beneath the surface
        self.cuttingDepth = 7480.0
        self.noLinearSpeed = 0.0
        self.movingLinearSpeed = 1.0
        self.arbitrator.setDesiredState( self.pitchAngle, self.initYawAngle, self.initDepth )
        
        # AB: Disabled for now as this script will be running inside another script
        #self.arbitrator.update( self.noLinearSpeed )
        
        self.oneMetresArea = 2700                    # rough estimate        
        
        self.forwardTimer = time.time()
        self.closeEnoughFlag = 0.0
        self.detectOrangeFlag = 0.0
        self.ballArea = 0.0
        self.lastBallArea = 0.0
        self.image = None
        
    #---------------------------------------------------------------------------
    def setImage( self, image ):
        self.image = image

    #---------------------------------------------------------------------------
    def update( self ):
        
        curTime = time.time()
        
        if self.closeEnoughFlag == 0.0:
            if self.arbitrator.atDesiredYaw() and self.arbitrator.atDesiredDepth():
                self.arbitrator.update( self.movingLinearSpeed )
                self.forwardTimer = time.time()
            
            if curTime - self.forwardTimer >= 30.0:         # temporary value - determined by test
                self.arbitrator.update( self.noLinearSpeed )
                self.closeEnoughFlag == 1.0

        else:
            angle = self.initYawAngle
            lefScanAngleRange = self.initYawAngle - math.pi/2
            rightScanAngleRange = self.initYawAngle + math.pi/2
            
            while angle >= lefScanAngleRange and self.detectOrangeFlag == 0.0:
                self.arbitrator.setDesiredYawAngle( angle )
                self.arbitrator.update( self.noLinearSpeed )
                self.ballFinder.run( )
                self.detectOrangeFlag = self.ballFinder.target_aquired
                if self.detectOrangeFlag == 0.0:
                    angle -= Maths.degToRad( 10 )
                            
            while angle <= rightScanAngleRange and self.detectOrangeFlag == 0:
                self.arbitrator.setDesiredYawAngle( angle )
                self.arbitrator.update( noLinearSpeed )
                self.ballFinder.run( )
                self.detectOrangeFlag = self.ballFinder.target_aquired
                if self.detectOrangeFlag == 0.0:
                    angle += Maths.degToRad( 10 )
            
            if self.detectOrangeFlag == 1.0:
                compassYawAngle = self.playerCompass.pose.pyaw
                self.ballFinder.run( )
                ballX = self.ballFinder.corr_x
                self.ballArea = self.ballFinder.pixel_count
                #if self.ballArea > twoMetresArea and self.ballArea < onefiveMetresArea:
                    #distance = 2.0
                #elif self.ballArea > onefiveMetresArea and self.ballArea < oneMetresArea:
                    #distance = 1.5
                #elif self.ballArea > onefiveMetresArea and self.ballArea < oneMetresArea:
                    #distance = 1.0
                while ballX > 10 or ballX < -10:
                    if ballX > 0:
                        newYaw = compassYawAngle + Maths.degToRad( 5 )
                    elif ballX < 0:
                        newYaw = compassYawAngle - Maths.degToRad( 5 )
                    self.arbitrator.setDesiredYaw( newYaw )
                    self.arbitrator.update ( movingLinearSpeed )    # don't wait for the yaw control, because it's a rough estimate anyway
                    if self.ballArea > self.oneMetresArea:               # needs to be set
                        self.arbitrator.setDesiredDepth( self.cuttingDepth )
                        self.arbitrator.update( noLinearSpeed )
                        if atDesiredDepth():
                            self.arbitrator.update( movingLinearSpeed )
                            #"activate cutter"
                            
예제 #3
0
class TimeBasedGatesScript( ControlScript ):
    
    STATE_DIVING = "Diving"
    STATE_TURNING_TO_GATE_1 = "TurningToGate_1"
    STATE_DRIVING_THROUGH_GATE_1 = "DrivingThroughGate_1"
    STATE_TURNING_TO_GATE_2 = "TurningToGate_2"
    STATE_DRIVING_THROUGH_GATE_2 = "DrivingThroughGate_2"
    STATE_TURNING_TO_START = "TurningToStart"
    STATE_DRIVING_BACK_TO_START = "DrivingBackToStart"
    STATE_SURFACING = "Surfacing"
    STATE_FINISHED = "Finished"
    
    #---------------------------------------------------------------------------
    def __init__( self, config, logger, playerPos3d, playerCompass, 
        playerDepthSensor, playerSonar = None ):
        
        ControlScript.__init__( self, config, logger, playerPos3d, playerCompass, 
            playerDepthSensor, playerSonar )
        
        self.arbitrator = Arbitrator( playerPos3d, playerCompass, playerDepthSensor )
        self.arbitrator.setDesiredDepth( -1.0 )
        self.linearSpeed = 0.0
        self.setState( self.STATE_DIVING )
        self.driveTimer = time.time()
    
    #---------------------------------------------------------------------------
    def update( self ):
        
        curTime = time.time()
        
        if self.state == self.STATE_DIVING:

            if self.arbitrator.atDesiredDepth():
                self.arbitrator.setDesiredYaw( Maths.degToRad( 0.0 ) )
                self.setState( self.STATE_TURNING_TO_GATE_1 )
            
        elif self.state == self.STATE_TURNING_TO_GATE_1:
            
            if self.arbitrator.atDesiredYaw():
                self.linearSpeed = 1.0
                self.driveTimer = time.time()
                self.setState( self.STATE_DRIVING_THROUGH_GATE_1 )
                        
        elif self.state == self.STATE_DRIVING_THROUGH_GATE_1:
            
            if curTime - self.driveTimer >= 20.0:
                self.linearSpeed = 0.0
                self.arbitrator.setDesiredYaw( Maths.degToRad( 180.0 ) )
                self.setState( self.STATE_TURNING_TO_GATE_2 )
            
        elif self.state == self.STATE_TURNING_TO_GATE_2:
            
            if self.arbitrator.atDesiredYaw():
                self.linearSpeed = 1.0
                self.driveTimer = time.time()
                self.setState( self.STATE_DRIVING_THROUGH_GATE_2 )
            
        elif self.state == self.STATE_DRIVING_THROUGH_GATE_2:
            
            if curTime - self.driveTimer >= 40.0:
                self.linearSpeed = 0.0
                self.arbitrator.setDesiredYaw( Maths.degToRad( 0.0 ) )
                self.setState( self.STATE_TURNING_TO_START )
            
        elif self.state == self.STATE_TURNING_TO_START:
            
            if self.arbitrator.atDesiredYaw():
                self.linearSpeed = 1.0
                self.driveTimer = time.time()
                self.setState( self.STATE_DRIVING_BACK_TO_START )
            
        elif self.state == self.STATE_DRIVING_BACK_TO_START:
            
            if curTime - self.driveTimer >= 20.0:
                self.linearSpeed = 0.0
                self.arbitrator.setDesiredDepth( 0.0 )
                self.setState( self.STATE_SURFACING )
            
        elif self.state == self.STATE_SURFACING:
            
            if self.arbitrator.atDesiredDepth():
                self.setState( self.STATE_FINISHED )
            
        elif self.state == self.STATE_FINISHED:
            # Nothing to do
            pass
        else:
            self.logger.logError( "Unrecognised state - surfacing" )
            self.setState( self.STATE_SURFACING )
        
        self.arbitrator.update( self.linearSpeed )
예제 #4
0
class QualifyingRunScript( ControlScript ):
    
    STATE_WAITING_TO_START = "WaitingToStart"
    STATE_DIVING = "Diving"
    STATE_RISING = "Rising"
    
    #---------------------------------------------------------------------------
    def __init__( self, config, logger, playerPos3d, playerCompass, 
        playerDepthSensor, playerSonar, playerFrontCamera ):
        
        ControlScript.__init__( self, config, logger, 
            playerPos3d=playerPos3d, playerCompass=playerCompass, 
            playerDepthSensor=playerDepthSensor, playerSonar=playerSonar, 
            playerFrontCamera=playerFrontCamera )
        
        self.UP_DEPTH = self.config.B_upDepth
        self.DOWN_DEPTH = self.config.B_downDepth
        self.START_DELAY_TIME = self.config.B_startDelayTime
        
        self.USE_IMAGE_CAPTURE = self.config.useImageCapture
        
        self.imageCaptureScript = ImageCaptureScript( self.config, self.logger,
            self.playerPos3d, self.playerDepthSensor, self.playerSonar, self.playerFrontCamera )
        
        # Setup the arbitrator
        self.arbitrator = Arbitrator( playerPos3d, playerCompass, playerDepthSensor )
        self.arbitrator.setDesiredPitch( Maths.degToRad( -4.0 ) )
        self.arbitrator.setControlGains(
            pitchKp=self.config.pitchKp, 
            pitchKi=self.config.pitchKi, pitchiMin=self.config.pitchiMin, pitchiMax=self.config.pitchiMax, 
            pitchKd=self.config.pitchKd,
            yawKp=self.config.yawKp, 
            yawKi=self.config.yawKi, yawiMin=self.config.yawiMin, yawiMax=self.config.yawiMax, 
            yawKd=self.config.yawKd,
            depthKp=self.config.depthKp, 
            depthKi=self.config.depthKi, depthiMin=self.config.depthiMin, depthiMax=self.config.depthiMax,
            depthKd=self.config.depthKd )
        self.arbitrator.setEpsilons( 
            self.config.arbitratorDepthEpsilon, 
            Maths.degToRad( self.config.arbitratorYawEpsilonDegrees ) )
        
        # Clear timers
        self.waitTimer = 0.0
        self.delayTimer = 0.0
        
        # Move into the first state
        self.linearSpeed = 0.0
        self.delayTimer = time.time()
        self.setState( self.STATE_WAITING_TO_START )    
    
    #---------------------------------------------------------------------------
    def update( self ):
        
        curTime = time.time()
        
        if self.USE_IMAGE_CAPTURE \
            and self.state != self.STATE_WAITING_TO_START:
            self.imageCaptureScript.update()
        
        if self.state == self.STATE_WAITING_TO_START:
            
            timeDiff = curTime - self.delayTimer
            print "timeDiff is", timeDiff, "delay is", self.START_DELAY_TIME
            
            if curTime - self.delayTimer >= self.START_DELAY_TIME:
                self.logger.logMsg( "Going to " + str( self.DOWN_DEPTH ) )
                self.arbitrator.setDesiredDepth( self.DOWN_DEPTH )
                self.linearSpeed = 0.0
                self.setState( self.STATE_DIVING )
            
        elif self.state == self.STATE_DIVING:

            if self.arbitrator.atDesiredDepth():
                self.arbitrator.setDesiredDepth( self.UP_DEPTH )
                self.linearSpeed = 0.0
                self.setState( self.STATE_RISING )
            
        elif self.state == self.STATE_RISING:

            if self.arbitrator.atDesiredDepth():
                self.arbitrator.setDesiredDepth( self.DOWN_DEPTH )
                self.linearSpeed = 0.0
                self.setState( self.STATE_DIVING )
        
        self.arbitrator.update( self.linearSpeed )
예제 #5
0
class HailMaryScript( ControlScript ):
    
    STATE_WAITING_TO_START = "WaitingToStart"
    STATE_DIVING = "Diving"
    STATE_TURNING_TO_GATE_1 = "TurningToGate_1"
    STATE_DRIVING_THROUGH_GATE_1 = "DrivingThroughGate_1"
    STATE_DESCENDING_TO_PIPE = "DescendingToPipe"
    STATE_TURNING_TO_GATE_2 = "TurningToGate_2"
    STATE_SURVEYING_PIPE = "SurveyingPipe"
    STATE_RETURNING_TO_NORMAL_DEPTH = "ReturningToNormalDepth"
    STATE_TURNING_TO_WALL = "TurningToWall"
    STATE_APPROACHING_WALL = "ApproachingWall"
    STATE_SURVEYING_WALL = "SurveyingWall"
    STATE_TURNING_TO_HUNT_BUOY = "TurningToHuntBuoy"
    STATE_HUNTING_BUOY = "HuntingBuoy"
    STATE_RETURNING_TO_CENTRE = "ReturningToCentre"
    STATE_SURFACING = "Surfacing"
    
    #---------------------------------------------------------------------------
    def __init__( self, config, logger, playerPos3d, playerCompass, 
        playerDepthSensor, playerSonar, playerFrontCamera ):
        
        ControlScript.__init__( self, config, logger, 
            playerPos3d=playerPos3d, playerCompass=playerCompass, 
            playerDepthSensor=playerDepthSensor, playerSonar=playerSonar, 
            playerFrontCamera=playerFrontCamera )
        
        self.NORMAL_DEPTH = self.config.HM_normalDepth
        self.PIPE_SURVEY_DEPTH = self.config.HM_pipeSurveyDepth
        self.FORWARD_SPEED = self.config.HM_forwardSpeed
        self.START_DELAY_TIME = self.config.HM_startDelayTime
        self.END_DELAY_TIME = self.config.HM_endDelayTime
        self.DRIVING_THROUGH_GATE_1_TIME = self.config.HM_drivingThroughGate_1_Time
        self.SURVEYING_PIPE_TIME = self.config.HM_surveyingPipeTime
        self.APPROACHING_WALL_TIME = self.config.HM_approachingWallTime
        self.SURVEYING_WALL_TIME = self.config.HM_surveyingWallTime
        self.MAX_HUNTING_BUOY_TIME = self.config.HM_maxHuntingBuoyTime
        self.RETURNING_TO_CENTRE_TIME = self.config.HM_returningToCentreTime
        self.NORTH_HEADING_DEGREES = self.config.HM_northHeadingDegrees
        self.EAST_HEADING_DEGREES = self.config.HM_eastHeadingDegrees
        self.SOUTH_HEADING_DEGREES = self.config.HM_southHeadingDegrees
        self.WEST_HEADING_DEGREES = self.config.HM_westHeadingDegrees
        
        # Configure the image capture script as we please
        imageCaptureConfig = copy.deepcopy( self.config )
        imageCaptureConfig.IC_Script_sonarGain = 0.3
        imageCaptureConfig.IC_Script_sonarRange = 25
        imageCaptureConfig.IC_Script_sonarNumBins = 300
        imageCaptureConfig.IC_Script_sonarScanStartAngleDegrees = 0.0
        imageCaptureConfig.IC_Script_sonarScanEndAngleDegrees = 350.0
        imageCaptureConfig.IC_Script_numImagesSavedPerSecond = 1.0
        imageCaptureConfig.IC_Script_enableCamera = True
        imageCaptureConfig.IC_Script_enableSonar = False
        
        self.imageCaptureScript = ImageCaptureScript( imageCaptureConfig, self.logger,
            self.playerPos3d, self.playerDepthSensor, self.playerSonar, self.playerFrontCamera )
        
        self.motorKiller = KillMotors( self.playerPos3d )
        self.ballChopper = OrangeBallScript( config, logger, 
            playerPos3d, playerCompass, playerDepthSensor )
        
        # Setup the arbitrator
        self.arbitrator = Arbitrator( playerPos3d, playerCompass, playerDepthSensor, logger=logger )
        self.arbitrator.setDesiredPitch( Maths.degToRad( -4.0 ) )
        self.arbitrator.setControlGains(
            pitchKp=self.config.pitchKp, 
            pitchKi=self.config.pitchKi, pitchiMin=self.config.pitchiMin, pitchiMax=self.config.pitchiMax, 
            pitchKd=self.config.pitchKd,
            yawKp=self.config.yawKp, 
            yawKi=self.config.yawKi, yawiMin=self.config.yawiMin, yawiMax=self.config.yawiMax, 
            yawKd=self.config.yawKd,
            depthKp=self.config.depthKp, 
            depthKi=self.config.depthKi, depthiMin=self.config.depthiMin, depthiMax=self.config.depthiMax,
            depthKd=self.config.depthKd )
        self.arbitrator.setEpsilons( 
            self.config.arbitratorDepthEpsilon, 
            Maths.degToRad( self.config.arbitratorYawEpsilonDegrees ) )
        
        # Clear timer
        self.stateTimer = 0.0
        
        # Move into the first state
        self.setState( self.STATE_WAITING_TO_START )    
    
    #---------------------------------------------------------------------------
    def setState( self, newState ):
        
        self.exitState()
        
        if newState == self.STATE_WAITING_TO_START:
            
            self.linearSpeed = 0.0
            self.stateTimer = time.time()
            
        elif newState == self.STATE_DIVING:

            self.linearSpeed = 0.0
            self.logAction( "Descending to " + str( self.NORMAL_DEPTH ) )
            self.arbitrator.setDesiredDepth( self.NORMAL_DEPTH )

        elif newState == self.STATE_TURNING_TO_GATE_1:

            self.linearSpeed = 0.0
            self.arbitrator.setDesiredYaw( 
                Maths.degToRad( self.NORTH_HEADING_DEGREES ) )
            self.setSonarLocatorActive( True )
                        
        elif newState == self.STATE_DRIVING_THROUGH_GATE_1:
            
            self.linearSpeed = self.FORWARD_SPEED
            self.staterTimer = time.time()
    
        elif newState == self.STATE_DESCENDING_TO_PIPE:
            
            self.linearSpeed = 0.0
            self.logAction( "Descending to " + str( self.PIPE_SURVEY_DEPTH ) )
            self.arbitrator.setDesiredDepth( self.PIPE_SURVEY_DEPTH )
            
        elif newState == self.STATE_TURNING_TO_GATE_2:
            
            self.linearSpeed = 0.0
            self.arbitrator.setDesiredYaw( 
                Maths.degToRad( self.SOUTH_HEADING_DEGREES ) )
                
        elif newState == self.STATE_SURVEYING_PIPE:
            
            self.linearSpeed = self.FORWARD_SPEED
            self.staterTimer = time.time()
            
        elif newState == self.STATE_RETURNING_TO_NORMAL_DEPTH:
            
            self.linearSpeed = 0.0
            self.logAction( "Climbing to " + str( self.NORMAL_DEPTH ) )
            self.arbitrator.setDesiredDepth( self.NORMAL_DEPTH )
            
        elif newState == self.STATE_TURNING_TO_WALL:
            
            self.linearSpeed = 0.0
            self.arbitrator.setDesiredYaw( 
                Maths.degToRad( self.EAST_HEADING_DEGREES ) )
            
        elif newState == self.STATE_APPROACHING_WALL:
            
            self.linearSpeed = self.FORWARD_SPEED
            self.staterTimer = time.time()
            
        elif newState == self.STATE_SURVEYING_WALL:
            
            self.linearSpeed = 0.0
            self.stateTimer = time.time()
            self.setSonarLocatorActive( False )
            self.imageCaptureScript.config.IC_Script_enableSonar = True
            
        elif newState == self.STATE_TURNING_TO_HUNT_BUOY:
            
            self.linearSpeed = 0.0
            self.arbitrator.setDesiredYaw( 
                Maths.degToRad( self.WEST_HEADING_DEGREES ) )
            
        elif newState == self.STATE_HUNTING_BUOY:
    
            # Go, go, go!
            self.ballChopper.initYawAngle = self.playerCompass.pose.pyaw
            self.stateTimer = time.time()
            
        elif newState == self.STATE_RETURNING_TO_CENTRE:
    
            # Blast forward in hopeful direction
            self.linearSpeed = self.FORWARD_SPEED
            heading = ( self.WEST_HEADING_DEGREES + 45.0 )
            self.arbitrator.setDesiredYaw( 
                Maths.degToRad( heading ) )
            self.stateTimer = time.time()
    
        elif newState == self.STATE_SURFACING:
            
            self.linearSpeed = 0.0
            self.stateTimer = time.time()
            
        else:
            self.logger.logError( "Enter State - Unrecognised state - ({0})".format( self.state ) )
            return
        
        ControlScript.setState( self, newState )
    
    #---------------------------------------------------------------------------
    def exitState( self ):
        
        if self.state ==  self.STATE_INVALID:
            pass
        if self.state == self.STATE_WAITING_TO_START:
            pass
        elif self.state == self.STATE_DIVING:
            pass
        elif self.state == self.STATE_TURNING_TO_GATE_1:
            pass        
        elif self.state == self.STATE_DRIVING_THROUGH_GATE_1:
            pass
        elif self.state == self.STATE_DESCENDING_TO_PIPE:
            pass
        elif self.state == self.STATE_TURNING_TO_GATE_2:
            pass
        elif self.state == self.STATE_SURVEYING_PIPE:
            pass
        elif self.state == self.STATE_RETURNING_TO_NORMAL_DEPTH:
            pass
        elif self.state == self.STATE_TURNING_TO_WALL:
            pass
        elif self.state == self.STATE_APPROACHING_WALL:
            pass
        elif self.state == self.STATE_SURVEYING_WALL:
            
            self.imageCaptureScript.config.IC_Script_enableSonar = False
            self.setSonarLocatorActive( True )
        
        elif self.state == self.STATE_TURNING_TO_HUNT_BUOY:
            pass
        elif self.state == self.STATE_HUNTING_BUOY:
            pass
        elif self.state == self.STATE_RETURNING_TO_CENTRE:
            pass
        elif self.state == self.STATE_SURFACING:
            pass
        else:
            self.logger.logError( "Exit State - Unrecognised state - ({0})".format( self.state ) )
        
    #---------------------------------------------------------------------------
    #@Profiling.printTiming
    def update( self ):
        
        curTime = time.time()
        
        ControlScript.update( self )
        
        if self.state != self.STATE_WAITING_TO_START:
            self.imageCaptureScript.update()
        
        if self.state == self.STATE_WAITING_TO_START:
            
            timeDiff = curTime - self.stateTimer
            print "timeDiff is", timeDiff, "delay is", self.START_DELAY_TIME
            
            if curTime - self.stateTimer >= self.START_DELAY_TIME:
                self.setState( self.STATE_DIVING )
            
        elif self.state == self.STATE_DIVING:

            if self.arbitrator.atDesiredDepth():
                self.setState( self.STATE_TURNING_TO_GATE_1 )
            
        elif self.state == self.STATE_TURNING_TO_GATE_1:
            
            if self.arbitrator.atDesiredYaw():
                self.setState( self.STATE_DRIVING_THROUGH_GATE_1 )
                        
        elif self.state == self.STATE_DRIVING_THROUGH_GATE_1:
            
            if curTime - self.staterTimer >= self.DRIVING_THROUGH_GATE_1_TIME:
                self.setState( self.STATE_DESCENDING_TO_PIPE )
                
        elif self.state == self.STATE_DESCENDING_TO_PIPE:
            
            if self.arbitrator.atDesiredDepth():
                self.setState( self.STATE_TURNING_TO_GATE_2 )
            
        elif self.state == self.STATE_TURNING_TO_GATE_2:
            
            if self.arbitrator.atDesiredYaw():
                self.setState( self.STATE_SURVEYING_PIPE )
        
        elif self.state == self.STATE_SURVEYING_PIPE:
        
            if curTime - self.staterTimer >= self.SURVEYING_PIPE_TIME:
                self.setState( self.STATE_RETURNING_TO_NORMAL_DEPTH )
                
        elif self.state == self.STATE_RETURNING_TO_NORMAL_DEPTH:
            
            if self.arbitrator.atDesiredDepth():
                self.setState( self.STATE_TURNING_TO_WALL )
                
        elif self.state == self.STATE_TURNING_TO_WALL:
            
            if self.arbitrator.atDesiredYaw():
                self.setState( self.STATE_APPROACHING_WALL )
        
        elif self.state == self.STATE_APPROACHING_WALL:
            
            if curTime - self.staterTimer >= self.APPROACHING_WALL_TIME:
                self.setState( self.STATE_SURVEYING_WALL )
                
        elif self.state == self.STATE_SURVEYING_WALL:
            
            if curTime - self.stateTimer >= self.SURVEYING_WALL_TIME:
                self.setState( self.STATE_TURNING_TO_HUNT_BUOY )
        
        elif self.state == self.STATE_TURNING_TO_HUNT_BUOY:
            
            if self.arbitrator.atDesiredYaw():
                self.setState( self.STATE_RETURNING_TO_CENTRE )
        
        elif self.state == self.STATE_HUNTING_BUOY:
            
            if curTime - self.stateTimer >= self.MAX_HUNTING_BUOY_TIME:
                self.setState( self.STATE_RETURNING_TO_CENTRE )
                
        elif self.state == self.STATE_RETURNING_TO_CENTRE:
            
            if curTime - self.stateTimer >= self.RETURNING_TO_CENTRE_TIME:
                self.setState( self.STATE_SURFACING )
        
        elif self.state == self.STATE_SURFACING:
            # Wait for a bit and then exit the program
            timeDiff = curTime - self.stateTimer    
            if timeDiff >= self.END_DELAY_TIME:
                sys.exit( 0 )   # Quit
            
        else:
            self.logger.logError( "Update - Unrecognised state - ({0})".format( self.state ) )
        
        if self.state == self.STATE_SURFACING:
            # Kill motors to come up and end mission
            self.motorKiller.update()
        elif self.state == self.STATE_HUNTING_BUOY:
            self.ballChopper.setImage( self.imageCaptureScript.bgrImage )
            self.ballChopper.update()
        else:
            self.arbitrator.update( self.linearSpeed )
예제 #6
0
class QualifyingRunScript( ControlScript ):
    
    STATE_WAITING_TO_START = "WaitingToStart"
    STATE_DIVING = "Diving"
    STATE_TURNING_TO_GATE_1 = "TurningToGate_1"
    STATE_DRIVING_THROUGH_GATE_1 = "DrivingThroughGate_1"
    STATE_SURFACING = "Surfacing"
    
    #---------------------------------------------------------------------------
    def __init__( self, config, logger, playerPos3d, playerCompass, 
        playerDepthSensor, playerSonar, playerFrontCamera ):
        
        ControlScript.__init__( self, config, logger, 
            playerPos3d=playerPos3d, playerCompass=playerCompass, 
            playerDepthSensor=playerDepthSensor, playerSonar=playerSonar, 
            playerFrontCamera=playerFrontCamera )
        
        self.RUN_DEPTH = self.config.QR_runDepth
        self.FORWARD_SPEED = self.config.QR_forwardSpeed
        self.START_DELAY_TIME = self.config.QR_startDelayTime
        self.END_DELAY_TIME = 40.0
        self.MOVE_FORWARD_TIME = self.config.QR_moveForwardTime  # Will probably be caught by the net
        self.HEADING_TO_GATE_DEGREES = self.config.QR_headingToGateDegrees
    
        self.USE_IMAGE_CAPTURE = self.config.useImageCapture
        
        self.imageCaptureScript = ImageCaptureScript( self.config, self.logger,
            self.playerPos3d, self.playerDepthSensor, self.playerSonar, self.playerFrontCamera )
        
        self.motorKiller = KillMotors( self.playerPos3d )
        
        # Setup the arbitrator
        self.arbitrator = Arbitrator( playerPos3d, playerCompass, playerDepthSensor )
        self.arbitrator.setDesiredPitch( Maths.degToRad( -4.0 ) )
        self.arbitrator.setControlGains(
            pitchKp=self.config.pitchKp, 
            pitchKi=self.config.pitchKi, pitchiMin=self.config.pitchiMin, pitchiMax=self.config.pitchiMax, 
            pitchKd=self.config.pitchKd,
            yawKp=self.config.yawKp, 
            yawKi=self.config.yawKi, yawiMin=self.config.yawiMin, yawiMax=self.config.yawiMax, 
            yawKd=self.config.yawKd,
            depthKp=self.config.depthKp, 
            depthKi=self.config.depthKi, depthiMin=self.config.depthiMin, depthiMax=self.config.depthiMax,
            depthKd=self.config.depthKd )
        self.arbitrator.setEpsilons( 
            self.config.arbitratorDepthEpsilon, 
            Maths.degToRad( self.config.arbitratorYawEpsilonDegrees ) )
        
        # Clear timers
        self.waitTimer = 0.0
        self.delayTimer = 0.0
        
        # Move into the first state
        self.linearSpeed = 0.0
        self.delayTimer = time.time()
        self.setState( self.STATE_WAITING_TO_START )    
    
    #---------------------------------------------------------------------------
    def update( self ):
        
        curTime = time.time()
        
        if self.USE_IMAGE_CAPTURE \
            and self.state != self.STATE_WAITING_TO_START:
            self.imageCaptureScript.update()
        
        if self.state == self.STATE_WAITING_TO_START:
            
            timeDiff = curTime - self.delayTimer
            print "timeDiff is", timeDiff, "delay is", self.START_DELAY_TIME
            
            if curTime - self.delayTimer >= self.START_DELAY_TIME:
                self.logger.logMsg( "Going to " + str( self.RUN_DEPTH ) )
                self.arbitrator.setDesiredDepth( self.RUN_DEPTH )
                self.linearSpeed = 0.0
                self.setState( self.STATE_DIVING )
            
        elif self.state == self.STATE_DIVING:

            if self.arbitrator.atDesiredDepth():
                self.arbitrator.setDesiredYaw( 
                    Maths.degToRad( self.HEADING_TO_GATE_DEGREES ) )
                self.setState( self.STATE_TURNING_TO_GATE_1 )
            
        elif self.state == self.STATE_TURNING_TO_GATE_1:
            
            if self.arbitrator.atDesiredYaw():
                self.linearSpeed = self.FORWARD_SPEED
                self.driveTimer = time.time()
                self.setState( self.STATE_DRIVING_THROUGH_GATE_1 )
                        
        elif self.state == self.STATE_DRIVING_THROUGH_GATE_1:
            
            if curTime - self.driveTimer >= self.MOVE_FORWARD_TIME:
                self.linearSpeed = 0.0
                self.delayTimer = time.time()
                self.setState( self.STATE_SURFACING )
            
        elif self.state == self.STATE_SURFACING:

            # Wait for a bit and then exit the program
            timeDiff = curTime - self.delayTimer    
            if timeDiff >= self.END_DELAY_TIME:
                sys.exit( 0 )   # Quit
        else:
            self.logger.logError( "Unrecognised state - ({0}) - surfacing".format( self.state ) )
            self.linearSpeed = 0.0
            self.delayTimer = time.time()
            self.setState( self.STATE_SURFACING )
        
        if self.state == self.STATE_SURFACING:
            # Kill motors to come up and end mission
            self.motorKiller.update()
        else:
            self.arbitrator.update( self.linearSpeed )