示例#1
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 )
示例#2
0
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
示例#3
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 )
示例#4
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 )