def __init__( self, config, logger, playerPos3d,
playerDepthSensor, playerSonar, playerFrontCamera ):
ControlScript.__init__( self, config, logger,
playerPos3d, playerCompass = None,
playerDepthSensor = playerDepthSensor, playerSonar = playerSonar,
playerFrontCamera = playerFrontCamera )
self.sonarScanner = SonarScanner(
logger, playerSonar,
playerDepthSensor, config )
self.setState( self.STATE_PERFORMING_SCANS )
self.lastCameraFrameTime = 0.0
self.lastSonarFrameTime = 0.0
self.lastCameraImageSaveTime = 0.0
self.sonarImagesDir = string.Template( config.sonarImagesDir ).safe_substitute( os.environ )
self.cameraImagesDir = string.Template( config.cameraImagesDir ).safe_substitute( os.environ )
self.sonarScanner.setSonarConfig(
self.config.IC_Script_sonarRange,
self.config.IC_Script_sonarNumBins,
self.config.IC_Script_sonarGain )
self.sonarScanner.setScanAngleRange(
Maths.degToRad( self.config.IC_Script_sonarScanStartAngleDegrees ),
Maths.degToRad( self.config.IC_Script_sonarScanEndAngleDegrees ) )
self.bgrImage = None
def __init__( self, config, logger, playerPos3d, playerDepthSensor, playerCompass ):
ControlScript.__init__( self, config, logger,
playerPos3d, playerCompass = playerCompass,
playerDepthSensor = playerDepthSensor )
self.setState( self.STATE_GATHERING_DATA )
self.lastLogTime = 0.0
self.numIdenticalDepthReadings = 0
self.lastDepth = 0
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 __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 __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 __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 )
