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 )
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"
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 )
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 )
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 )
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 )
