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 MainWindow:
#---------------------------------------------------------------------------
def __init__( self, config = SubControllerConfig() ):
self.config = config
self.controlActive = False
self.normalisedLinearSpeed = 0.0
self.depthSpeed = 0.0
self.linearSpeed = 0.0
self.angularSpeed = 0.0
self.degCompassPitchAngle = 0.0
self.arrayPitchAngles = [ 0 ]
self.startPitchGraph = False
self.degCompassYawAngle = 0.0
self.arrayYawAngles = [ 0 ]
self.startYawGraph = False
self.degCompassRollAngle = 0.0
self.depthSensorDepth = 0.0
self.arrayDepthValues = [ 0 ]
self.arrayDepthSpeeds = [ 0 ]
self.startDepthGraph = False
self.pitchpTest = []
self.pitchiTest = []
self.pitchdTest = []
self.yawpTest = []
self.yawiTest = []
self.yawdTest = []
self.depthpTest = []
self.depthiTest = []
self.depthdTest = []
self.connectToPlayer()
self.pitchController = PitchControl( self.playerPos3d,
self.playerCompass, self.playerSimPos3d )
self.yawController = YawControl( self.playerPos3d,
self.playerCompass, self.playerSimPos3d )
self.depthController = DepthControl( self.playerPos3d,
self.playerDepthSensor, self.playerSimPos3d )
self.arbitrator = Arbitrator( self.playerPos3d,
self.playerCompass, self.playerDepthSensor, self.playerSimPos3d )
self.killing = KillMotors( self.playerPos3d )
# Setup the GUI
builder = gtk.Builder()
builder.add_from_file( os.path.dirname( __file__ ) + "/MonkeyDials.glade" )
self.window = builder.get_object( "winMain" )
self.spinMaxLinearSpeed = builder.get_object( "spinMaxLinearSpeed" )
self.linearSpeed = builder.get_object( "scaleLinearSpeed" )
self.spinPitchKp = builder.get_object( "spinPitchKp" )
self.spinPitchKi = builder.get_object( "spinPitchKi" )
self.spinPitchiMin = builder.get_object( "spinPitchiMin" )
self.spinPitchiMax = builder.get_object( "spinPitchiMax" )
self.spinPitchKd = builder.get_object( "spinPitchKd" )
self.spinYawKp = builder.get_object( "spinYawKp" )
self.spinYawKi = builder.get_object( "spinYawKi" )
self.spinYawiMin = builder.get_object( "spinYawiMin" )
self.spinYawiMax = builder.get_object( "spinYawiMax" )
self.spinYawKd = builder.get_object( "spinYawKd" )
self.spinDepthKp = builder.get_object( "spinDepthKp" )
self.spinDepthKi = builder.get_object( "spinDepthKi" )
self.spinDepthiMin = builder.get_object( "spinDepthiMin" )
self.spinDepthiMax = builder.get_object( "spinDepthiMax" )
self.spinDepthKd = builder.get_object( "spinDepthKd" )
self.spinDesiredPitchAngle = builder.get_object( "spinDesiredPitchAngle" )
self.spinDesiredYawAngle = builder.get_object( "spinDesiredYawAngle" )
self.spinDesiredDepth = builder.get_object( "spinDesiredDepth" )
self.lblCompassPitchAngle = builder.get_object( "lblCompassPitchAngle" )
self.lblCompassYawAngle = builder.get_object( "lblCompassYawAngle" )
self.lblCompassRollAngle = builder.get_object( "lblCompassRollAngle" )
self.lblDepthSensorDepth = builder.get_object( "lblDepthSensorDepth" )
self.checkKill = builder.get_object( "checkKill" )
self.chkControlLeftMotor = builder.get_object( "chkControlLeftMotor" )
self.chkControlRightMotor = builder.get_object( "chkControlRightMotor" )
self.chkControlFrontMotor = builder.get_object( "chkControlFrontMotor" )
self.chkControlBackMotor = builder.get_object( "chkControlBackMotor" )
self.spinMaxLinearSpeed.set_value( 1.0 )
self.spinDesiredPitchAngle.set_value( -5.0 )
self.spinDesiredYawAngle.set_value( 180.0 )
self.spinDesiredDepth.set_value( 7456)
self.spinPitchKp.set_value( 3.0 )
self.spinPitchKi.set_value( 0.0 )
self.spinPitchiMin.set_value( -1.57 )
self.spinPitchiMax.set_value( 1.57 )
self.spinPitchKd.set_value( 0.0 )
self.spinYawKp.set_value( -1.4 )
self.spinYawKi.set_value( 0.0 )
self.spinYawiMin.set_value( -1.57 )
self.spinYawiMax.set_value( 1.57 )
self.spinYawKd.set_value( 0.25 )
self.spinDepthKp.set_value( 0.3 )
self.spinDepthKi.set_value( 0.0 )
self.spinDepthiMin.set_value( -1.57 )
self.spinDepthiMax.set_value( 1.57 )
self.spinDepthKd.set_value( 0.0 ) # AB: Should be 0.5. Change back after qualifying run
self.RANGE = 100
self.DEAD_ZONE = self.RANGE*0.01
builder.connect_signals( self )
self.window.show()
#self.yawController.foo = 10
#print dir( self.yawController )
updateLoop = self.update()
gobject.idle_add( updateLoop.next )
#---------------------------------------------------------------------------
def connectToPlayer( self ):
try:
# Create a client object to connect to Player
self.playerClient = playerc_client( None,
self.config.playerServerAddress, self.config.playerServerPort )
# Connect it
if self.playerClient.connect() != 0:
raise Exception( playerc_error_str() )
# Create a proxy for position3d.0
self.playerPos3d = playerc_position3d( self.playerClient, 0 )
if self.playerPos3d.subscribe( PLAYERC_OPEN_MODE ) != 0:
raise Exception( playerc_error_str() )
# Try to also create one for position3d.1 if it exists
self.playerSimPos3d = playerc_position3d( self.playerClient, 1 )
if self.playerSimPos3d.subscribe( PLAYERC_OPEN_MODE ) != 0:
self.playerSimPos3d = None
# Try to connect to imu:0
self.playerCompass = playerc_imu( self.playerClient, 0 )
if self.playerCompass.subscribe( PLAYERC_OPEN_MODE ) != 0:
self.playerCompass = None
self.playerDepthSensor = playerc_position1d( self.playerClient, 0 )
if self.playerDepthSensor.subscribe( PLAYERC_OPEN_MODE ) != 0:
self.playerDepthSensor = None
if self.playerClient.datamode( PLAYERC_DATAMODE_PULL ) != 0:
raise Exception( playerc_error_str() )
if self.playerClient.set_replace_rule( -1, -1, PLAYER_MSGTYPE_DATA, -1, 1 ) != 0:
raise Exception( playerc_error_str() )
except Exception as e:
self.showErrorMessage( "Exception when connecting to Player - " + str( e ) )
sys.exit( -1 )
print "Connected to Player!"
#---------------------------------------------------------------------------
# The program exits when the window is closed
def onWinMainDestroy( self, widget, data = None ):
gtk.main_quit()
#---------------------------------------------------------------------------
def main( self ):
# All PyGTK applications must have a gtk.main(). Control ends here
# and waits for an event to occur (like a key press or mouse event).
gtk.main()
#---------------------------------------------------------------------------
def showErrorMessage( self, msg ):
dialog = gtk.MessageDialog( parent=None,
flags=gtk.DIALOG_MODAL | gtk.DIALOG_DESTROY_WITH_PARENT,
type=gtk.MESSAGE_ERROR,
buttons=gtk.BUTTONS_OK,
message_format=msg )
dialog.set_title( "Error" )
dialog.run()
dialog.destroy()
#---------------------------------------------------------------------------
def updateNormalisedY( self, y ):
# y -> linear speed
maxLinearSpeed = self.spinMaxLinearSpeed.get_value()
if self.controlActive:
# Apply the dead zone
if y >= -self.DEAD_ZONE and y <= self.DEAD_ZONE:
y = 0.0
self.normalisedLinearSpeed = -y / self.RANGE
#---------------------------------------------------------------------------
def onScaleLinearSpeedValueChanged( self, widget, data = None ):
y = gtk.Range.get_value(widget);
self.updateNormalisedY( y )
self.controlActive = True
#---------------------------------------------------------------------------
def onYawPosButtonClicked( self, button ):
time = len( self.arrayYawAngles )
plt.clf()
#plt.figure(1)
plt.plot( range( time ), self.arrayYawAngles )
plt.ylabel( 'Yaw angle [deg/s]' )
plt.xlabel( 'Time' )
self.startYawGraph = False
plt.show()
def onYawPosStartButtonClicked( self, button ):
self.startYawGraph = True
self.arrayYawValues = []
self.yawpTest = []
self.yawdTest = []
#---------------------------------------------------------------------------
def onPitchPosButtonClicked( self, button ):
time = len( self.arrayPitchAngles )
plt.clf()
#plt.figure(2)
plt.plot( range( time ), self.arrayPitchAngles )
plt.ylabel( 'Pitch angle [deg/s]' )
plt.xlabel( 'Time' )
self.startPitchGraph = False
plt.show()
def onPitchPosStartButtonClicked( self, button ):
self.startPitchGraph = True
self.arrayPitchAngles = []
self.pitchpTest = []
self.pitchdTest = []
#---------------------------------------------------------------------------
def onDepthPosButtonClicked( self, button ):
time1 = len( self.arrayDepthValues )
plt.clf()
plt.figure(1)
plt.plot( range( time1 ), self.arrayDepthValues )
plt.ylabel( 'Depth [mbars]' )
plt.xlabel( 'Time' )
time2 = len( self.arrayDepthSpeeds )
plt.clf()
plt.figure(2)
plt.plot( range( time2 ), self.arrayDepthSpeeds )
plt.ylabel( 'Depth speed [deg/s]' )
plt.xlabel( 'Time' )
self.depthSpeed
self.startDepthGraph = False
plt.show()
def onDepthPosStartButtonClicked( self, button ):
self.startDepthGraph = True
self.arrayDepthValues = []
arrayDepthSpeeds = []
self.depthpTest = []
self.depthdTest = []
#---------------------------------------------------------------------------
def onKillMotorsButtonClicked( self, button ):
self.killing.update( )
#---------------------------------------------------------------------------
def update( self ):
while 1:
# Update the compass value if needed
if self.playerCompass != None and self.playerClient.peek( 0 ):
self.playerClient.read()
# Get compass pitch and yaw angle
radCompassPitchAngle = self.playerCompass.pose.ppitch
radCompassYawAngle = self.playerCompass.pose.pyaw
radCompassRollAngle = self.playerCompass.pose.proll
# 0 < angle < 2*pi
while radCompassPitchAngle >= 2*math.pi:
radCompassPitchAngle -= 2*math.pi
while radCompassYawAngle >= 2*math.pi:
radCompassYawAngle -= 2*math.pi
while radCompassRollAngle >= 2*math.pi:
radCompassRollAngle -= 2*math.pi
self.degCompassPitchAngle = radCompassPitchAngle*180.0/math.pi # from rad to degrees
self.degCompassYawAngle = radCompassYawAngle*180.0/math.pi # from rad to degrees
self.degCompassRollAngle = radCompassRollAngle*180.0/math.pi # from rad to degrees
#print it on the GUI
self.lblCompassPitchAngle.set_text( "{0:.3}".format( self.degCompassPitchAngle ) )
self.lblCompassYawAngle.set_text( "{0:.3}".format( self.degCompassYawAngle ) )
self.lblCompassRollAngle.set_text( "{0:.3}".format( self.degCompassRollAngle ) )
# Update the depth sensor value if needed
if self.playerDepthSensor != None and self.playerClient.peek( 0 ):
self.playerClient.read()
# Get pressure sensor depth
self.depthSensorDepth = self.playerDepthSensor.pos
self.lblDepthSensorDepth.set_text( "{0:2.3f}".format( self.depthSensorDepth ) )
maxLinearSpeed = self.spinMaxLinearSpeed.get_value()
if self.controlActive:
newLinearSpeed = self.normalisedLinearSpeed*maxLinearSpeed
else:
newLinearSpeed = 0.0
pitchKp = self.spinPitchKp.get_value()
pitchKi = self.spinPitchKi.get_value()
pitchiMin = self.spinPitchiMin.get_value()
pitchiMax = self.spinPitchiMax.get_value()
pitchKd = self.spinPitchKd.get_value()
yawKp = self.spinYawKp.get_value()
yawKi = self.spinYawKi.get_value()
yawiMin = self.spinYawiMin.get_value()
yawiMax = self.spinPitchiMax.get_value()
yawKd = self.spinYawKd.get_value()
depthKp = self.spinDepthKp.get_value()
depthKi = self.spinDepthKi.get_value()
depthiMin = self.spinDepthiMin.get_value()
depthiMax = self.spinDepthiMax.get_value()
depthKd = self.spinDepthKd.get_value()
newDesiredPitchAngle = self.spinDesiredPitchAngle.get_value()*math.pi/180.0 # from degrees to rad
newDesiredYawAngle = self.spinDesiredYawAngle.get_value()*math.pi/180.0 # from degrees to rad
newDesiredDepth = self.spinDesiredDepth.get_value()
if self.startPitchGraph:
self.arrayPitchAngles.append( self.degCompassPitchAngle)
self.pitchpTest.append (self.pitchController.pitchpTerm)
self.pitchdTest.append(self.pitchController.pitchdTerm)
#if radCompassPitchAngle - newDesiredPitchAngle < 0.01 \
# and radCompassPitchAngle - newDesiredPitchAngle> -0.01:
# self.pitchController.iState = 0.0
# self.startPitchGraph = False
if self.startYawGraph:
mydegCompassYawAngle = self.degCompassYawAngle
while mydegCompassYawAngle >= math.pi:
mydegCompassYawAngle -= 2*math.pi
while mydegCompassYawAngle < -math.pi:
mydegCompassYawAngle += 2*math.pi
self.arrayYawAngles.append( mydegCompassYawAngle )
self.yawpTest.append (self.yawController.yawpTerm)
self.yawdTest.append(self.yawController.yawdTerm)
# if radCompassYawAngle - newDesiredYawAngle < 0.01 \
# and radCompassYawAngle - newDesiredYawAngle> -0.01:
# self.yawController.iState = 0.0
#figure(2)
#plot(range(len(self.yawpTest)),self.pTest,'r',\
#range(len(self.yawiTest)),self.iTest,'k',\
#range(len(self.yawdTest)),self.dTest,'m')
#xlabel('Time')
#ylabel('yaw pTerm, yaw iTerm & yaw dTerm')
#show()
# self.startYawGraph = False
if self.startDepthGraph:
self.arrayDepthValues.append( self.depthSensorDepth)
self.arrayDepthSpeeds.append( self.depthController.depthSpeed)
self.depthpTest.append (self.depthController.depthpTerm)
self.depthdTest.append(self.depthController.depthdTerm)
#if self.depthSensorDepth - newDesiredDepth < 0.01 \
# and self.depthSensorDepth - newDesiredDepth> -0.01:
# self.depthController.iState = 0.0
# self.startDepthGraph = False
if self.checkKill.get_active():
self.killing.update( )
else:
self.arbitrator.setControlGains( pitchKp, pitchKi, pitchiMin, pitchiMax, pitchKd,
yawKp, yawKi, yawiMin, yawiMax, yawKd,
depthKp, depthKi, depthiMin, depthiMax, depthKd )
self.arbitrator.setDesiredState( newDesiredPitchAngle, newDesiredYawAngle, newDesiredDepth ) # rad
self.arbitrator.setUncontrolledMotors(
self.chkControlLeftMotor.get_active(), self.chkControlRightMotor.get_active(),
self.chkControlFrontMotor.get_active(), self.chkControlBackMotor.get_active() )
self.arbitrator.update( newLinearSpeed )
if newLinearSpeed != self.linearSpeed:
# Store the speed
self.linearSpeed = newLinearSpeed
yield True
yield False
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 )