DockingTankTestPitchControl.xml

Page auto-generated from XML file.

Failure

The generated TethysL source could not be parsed, likely due to source XML not being well-formed.

Below is the compile error at the TethysL level, as well as the source XML.

The TethysL file name shown below does not necessarily indicate the file exists, but hints about the actual corresponding .xml file.

Syntax error:
   --> lrauv-application/Missions/Demo/DockingTankTestPitchControl.xml:85:5
    | 
 81 |   timeout duration=P15M
 82 | 
 83 |   aggregate TankDocking {
 84 |     # Be neutral the entire time
 85 |     behavior Guidance:Buoyancy {
    |     ^^^^^^^^
 86 |       run in parallel
 87 |       set Guidance:Buoyancy.position = Control:VerticalControl.buoyancyNeutral
 88 |     }
 89 | 
    | 
Unexpected: `behavior`
One of the following is possible:
  """
  arguments
  output
  run
  test_code
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
mission DockingTankTestPitchControl {
  """
  1.) Wait timeout for loading in launcher.
  2.) Turn on prop and arm for docking.
  3.) Wait timeout for either cable present or timeout.
  4.) Cable present == latch for timeout then release, back up and end mission.
  5.) No cable present == speed 0 and end mission.
  6.) end mission == float up at 0 speed
  """

  arguments {
    Speed = 1 meter_per_second
      """
      Thruster speed for docking.
      """

    WaitForLoadTimeout = 90 second
      """
      Time to wait prior to turning on the prop.
      """

    LaunchTimeout = 60 second
      """
      Time to wait with the prop on driving to the dock.
      """

    LatchTimeout = 15 second
      """
      Time to wait before closing the latch after seeing the cable.
      """

    DetachTimeout = 7 second
      """
      Time to wait for dock detach.
      """

    FloatTimeout = 120 second
      """
      How long to wait for the vehicle to float to the surface.
      """

    MassDefault = Control:VerticalControl.massDefault
      """
      Static setting for mass during the mission. Set to NaN mm for active
      mass position
      """

    ElevtorDeadband = 0.1 arcdeg
      """
      Degree of rounding in elevator command output value
      """

    kpPitchElevatorGain = 0.25 none
      """
      proportional gain
      """

    kiPitchElevatorGain = 0.008 reciprocal_second
      """
      integral gain
      """

    kdPitchElevatorGain = 0.05 second
      """
      derivative gain
      """

    RudderDefault = 3.0 degree
      """
      Static setting for rudder.
      """
  }

  output {
    DockingDepth = 1.22 meter
      """
      How deep in the tank do you want to swim?
      """
  }

  timeout duration=P15M

  aggregate TankDocking {
    # Be neutral the entire time
    behavior Guidance:Buoyancy {
      run in parallel
      set Guidance:Buoyancy.position = Control:VerticalControl.buoyancyNeutral
    }

    behavior Guidance:Pitch {
      run in parallel
      set Guidance:Pitch.massPosition = MassDefault
      set Guidance:Pitch.pitch = 0 degree
      set Guidance:Pitch.depth = DockingDepth
    }

    # Set up the elevator gains and deadband
    assign in parallel Control:VerticalControl.elevDeadband = ElevtorDeadband

    assign in parallel Control:VerticalControl.kpPitchElevator = kpPitchElevatorGain

    assign in parallel Control:VerticalControl.kiPitchElevator = kiPitchElevatorGain

    assign in parallel Control:VerticalControl.kdPitchElevator = kdPitchElevatorGain

    # set up the horizontal control
    behavior Guidance:Point {
      run in parallel
      set Guidance:Point.rudderAngle = RudderDefault
    }

    # Let's fire up the DockingServo component here just so it's ready to go
    readData strategy="MinError" {
      in parallel
      Dock:Dock.docking_state
    }

    # Start with the arm closed if it isn't already
    assign in sequence Dock:Dock.docking_state_cmd = 1 enum

    aggregate Wait_for_loading {
      syslog info "Waiting to load for "
           + WaitForLoadTimeout ~ second

      behavior Guidance:Wait {
        run in sequence
        set Guidance:Wait.duration = WaitForLoadTimeout
      }
    }

    aggregate LaunchAndDock {
      timeout duration=LaunchTimeout {
        syslog important "Timed out while driving to the dock."
      }


      syslog info "Prop on and arming!"

      behavior Guidance:SetSpeed {
        run in parallel
        set Guidance:SetSpeed.speed = Speed
      }

      # Arm the....arm. Hmm....
      assign in sequence Dock:Dock.docking_state_cmd = 2 enum

      aggregate Latch {
        run when (
          Dock:Dock.dock_cable_present == true
        )

        behavior Guidance:Wait {
          run in sequence
          set Guidance:Wait.duration = LatchTimeout
        }

        # close the arm cut the prop
        assign in sequence Dock:Dock.docking_state_cmd = 1 enum

        behavior Guidance:SetSpeed {
          run in parallel
          set Guidance:SetSpeed.speed = 0 meter_per_second
        }
      }

      behavior Guidance:Wait {
        run in sequence
        set Guidance:Wait.duration = LaunchTimeout
      }
    }

    aggregate Detach {
      # open to detach
      assign in sequence Dock:Dock.docking_state_cmd = 2 enum

      # And reverse
      behavior Guidance:SetSpeed {
        run in parallel
        set Guidance:SetSpeed.speed = -1 meter_per_second
      }

      behavior Guidance:Wait {
        run in sequence
        set Guidance:Wait.duration = DetachTimeout
      }
    }
  }

  aggregate Float_Up {
    # close
    assign in sequence Dock:Dock.docking_state_cmd = 1 enum

    behavior Guidance:Buoyancy {
      run in parallel
      set Guidance:Buoyancy.position = Control:VerticalControl.buoyancyDefault
    }

    behavior Guidance:SetSpeed {
      run in parallel
      set Guidance:SetSpeed.speed = 0 meter_per_second
    }

    behavior Guidance:Wait {
      run in sequence
      set Guidance:Wait.duration = FloatTimeout
    }
  }
}
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Sensor="Tethys/Sensor" 
       xmlns:Units="Tethys/Units"
       xmlns:Estimation="Tethys/Estimation"
       xmlns:Dock="Tethys/Dock"       
       xmlns:Guidance="Tethys/Guidance"
       xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
       xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
                           Tethys/Control http://okeanids.mbari.org/tethys/Xml/Control.xsd
                           Tethys/Dock http://okeanids.mbari.org/tethys/Xml/Dock.xsd
                           Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                           Tethys/Sensor http://okeanids.mbari.org/tethys/Xml/Sensor.xsd
                           Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Id="DockingTankTestPitchControl">

   <Description>
        1.) Wait timeout for loading in launcher.
        2.) Turn on prop and arm for docking.
        3.) Wait timeout for either cable present or timeout. 
        4.) Cable present == latch for timeout then release, back up and end mission.
        5.) No cable present == speed 0 and end mission.
        6.) end mission == float up at 0 speed
    </Description>

    <DefineArg Name="Speed"><Description>
        Thruster speed for docking.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineOutput Name="DockingDepth"><Description>
        How deep in the tank do you want to swim?
    </Description><Units:meter/><Value>1.22</Value></DefineOutput>    

    <DefineArg Name="WaitForLoadTimeout"><Description>
        Time to wait prior to turning on the prop.
    </Description><Units:second/><Value>90</Value></DefineArg>    

    <DefineArg Name="LaunchTimeout"><Description>
        Time to wait with the prop on driving to the dock.
    </Description><Units:second/><Value>60</Value></DefineArg>  

    <DefineArg Name="LatchTimeout"><Description>
        Time to wait before closing the latch after seeing the cable.
    </Description><Units:second/><Value>15</Value></DefineArg> 

    <DefineArg Name="DetachTimeout"><Description>
        Time to wait for dock detach.
    </Description><Units:second/><Value>7</Value></DefineArg>      

    <DefineArg Name="FloatTimeout"><Description>
        How long to wait for the vehicle to float to the surface.
    </Description><Units:second/><Value>120</Value></DefineArg>

    <DefineArg Name="MassDefault"><Description>
        Static setting for mass during the mission. Set to NaN mm for active
        mass position
    </Description><Control:VerticalControl.massDefault/></DefineArg>

    <DefineArg Name="ElevtorDeadband"><Description>
        Degree of rounding in elevator command output value
    </Description><Units:arcdeg/><Value>0.1</Value></DefineArg>

    <DefineArg Name="kpPitchElevatorGain"><Description>
        proportional gain
    </Description><Units:none/><Value>0.25</Value></DefineArg>

    <DefineArg Name="kiPitchElevatorGain"><Description>
        integral gain
    </Description><Units:reciprocal_second/><Value>0.008</Value></DefineArg>

    <DefineArg Name="kdPitchElevatorGain"><Description>
        derivative gain
    </Description><Units:second/><Value>0.05</Value></DefineArg>    

    <DefineArg Name="RudderDefault"><Description>
        Static setting for rudder.
    </Description><Units:degree/><Value>3.0</Value></DefineArg>

    <Timeout Duration="P15M"/>

    <Aggregate Id="TankDocking">

        <!-- Be neutral the entire time -->   
        <Guidance:Buoyancy>
            <Parallel/>
            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
        </Guidance:Buoyancy>

        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.massPosition/><Arg Name="MassDefault"/></Setting>
            <Setting>:<Guidance:Pitch.pitch/><Units:degree/><Value>0</Value></Setting>
            <Setting><Guidance:Pitch.depth/><Arg Name="DockingDepth"/></Setting>
        </Guidance:Pitch>

        <!-- Set up the elevator gains and deadband -->
        <Assign><Parallel/><Control:VerticalControl.elevDeadband/><Arg Name="ElevtorDeadband"/></Assign>
        <Assign><Parallel/><Control:VerticalControl.kpPitchElevator/><Arg Name="kpPitchElevatorGain"/></Assign>
        <Assign><Parallel/><Control:VerticalControl.kiPitchElevator/><Arg Name="kiPitchElevatorGain"/></Assign>
        <Assign><Parallel/><Control:VerticalControl.kdPitchElevator/><Arg Name="kdPitchElevatorGain"/></Assign>

        <!-- set up the horizontal control -->           
        <Guidance:Point>
            <Parallel/>
            <Setting><Guidance:Point.rudderAngle/><Arg Name="RudderDefault"/></Setting>
        </Guidance:Point>

        <!-- Let's fire up the DockingServo component here just so it's ready to go -->   
        <ReadData Strategy="MinError">
              <Parallel/>
            <Dock:Dock.docking_state/>
        </ReadData>

        <!-- Start with the arm closed if it isn't already -->  
        <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>             


        <Aggregate Id="Wait for loading">
              <Syslog Severity="Info">Waiting to load for <Arg Name="WaitForLoadTimeout"/><Units:second/></Syslog>
            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="WaitForLoadTimeout"/></Setting>
            </Guidance:Wait>             
        </Aggregate>


        <Aggregate Id="LaunchAndDock">
            <Timeout Duration="LaunchTimeout">
                <Syslog Severity="Important">Timed out while driving to the dock.</Syslog>
            </Timeout>

            <Syslog Severity="Info">Prop on and arming!</Syslog>
            <Guidance:SetSpeed>
                <Parallel/>
                <Setting><Guidance:SetSpeed.speed/><Arg Name="Speed"/></Setting>
            </Guidance:SetSpeed>

            <!-- Arm the....arm. Hmm....  --> 
            <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>2</Value></Assign>


                <Aggregate Id="Latch">
                <When>
                    <Dock:Dock.dock_cable_present/>
                    <Eq><True/></Eq>
                </When>

                    <Guidance:Wait>
                        <Sequence/>
                        <Setting><Guidance:Wait.duration/><Arg Name="LatchTimeout"/></Setting>
                    </Guidance:Wait>                

                    <!-- close the arm cut the prop --> 
                    <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>             
                    <Guidance:SetSpeed>
                        <Parallel/>
                        <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
                    </Guidance:SetSpeed>                
                </Aggregate>

            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="LaunchTimeout"/></Setting>
            </Guidance:Wait> 

        </Aggregate>


        <Aggregate Id="Detach">
            <!-- open to detach  --> 
            <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>2</Value></Assign>

            <!-- And reverse -->   
            <Guidance:SetSpeed>
                <Parallel/>
                <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>-1</Value></Setting>
            </Guidance:SetSpeed>  

            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="DetachTimeout"/></Setting>
            </Guidance:Wait> 

        </Aggregate>

    </Aggregate>


    <Aggregate Id="Float Up">    
        <!-- close  --> 
        <Assign><Sequence/><Dock:Dock.docking_state_cmd/><Units:enum/><Value>1</Value></Assign>

        <Guidance:Buoyancy>
            <Parallel/>
            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyDefault/></Setting>
        </Guidance:Buoyancy>

        <Guidance:SetSpeed>
            <Parallel/>
            <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
        </Guidance:SetSpeed>          

        <Guidance:Wait>
            <Sequence/>
            <Setting><Guidance:Wait.duration/><Arg Name="FloatTimeout"/></Setting>
        </Guidance:Wait>     

    </Aggregate>        

</Mission>