コード例 #1
0
ファイル: OrangeBallScript.py プロジェクト: abroun/UweSub
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"
                            
コード例 #2
0
ファイル: MonkeyDials.py プロジェクト: abroun/UweSub
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