NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
- ComputedModelVersion "1.260"
+ ComputedModelVersion "1.341"
NumModelReferences 0
NumTestPointedSignals 0
}
SavedCharacterEncoding "iso-8859-2"
PreLoadFcn "init"
- PostLoadFcn "figure(2); hold off\nfigure(1);\nsim('reg_test', 0)"
-"; % show figures"
+ PostLoadFcn "figure(2); hold off\nfigure(1);\nsim('reg_mcl', 0);"
+" % show figures"
SaveDefaultBlockParams on
SampleTimeColors off
LibraryLinkDisplay "none"
ModifiedByFormat "%<Auto>"
LastModifiedBy "wsh"
ModifiedDateFormat "%<Auto>"
- LastModifiedDate "Fri Apr 25 08:39:02 2008"
- ModelVersionFormat "1.%<AutoIncrement:260>"
+ LastModifiedDate "Sun May 4 09:07:40 2008"
+ ModelVersionFormat "1.%<AutoIncrement:341>"
ConfigurationManager "None"
SimulationMode "normal"
LinearizationMsg "none"
SampleTime "inf"
FramePeriod "inf"
}
+ Block {
+ BlockType Demux
+ Outputs "4"
+ DisplayOption "none"
+ BusSelectionMode off
+ }
Block {
BlockType Display
Format "short"
Floating off
SampleTime "-1"
}
+ Block {
+ BlockType EnablePort
+ StatesWhenEnabling "held"
+ ShowOutputPort off
+ ZeroCross on
+ }
Block {
BlockType Gain
Gain "1"
SaturateOnIntegerOverflow on
SampleTime "-1"
}
+ Block {
+ BlockType Ground
+ }
+ Block {
+ BlockType HitCross
+ HitCrossingOffset "0"
+ HitCrossingDirection "either"
+ ShowOutputPort on
+ ZeroCross on
+ SampleTime "-1"
+ }
Block {
BlockType Inport
Port "1"
FunctionName "mlfile"
DisplayMFileStacktrace on
}
+ Block {
+ BlockType SignalViewerScope
+ Floating off
+ ModelBased off
+ TickLabels "OneTimeTick"
+ ZoomMode "on"
+ Grid "on"
+ TimeRange "auto"
+ YMin "-5"
+ YMax "5"
+ SaveToWorkspace off
+ SaveName "ScopeData"
+ LimitDataPoints on
+ MaxDataPoints "5000"
+ Decimation "1"
+ SampleInput off
+ SampleTime "0"
+ Disabled off
+ ScrollMode on
+ }
Block {
BlockType Mux
Inputs "4"
InitialCondition "0"
LinearizeAsGain on
}
+ Block {
+ BlockType RelationalOperator
+ Operator ">="
+ InputSameDT on
+ LogicOutDataTypeMode "Logical (see Configuration Parameters: Optimiza"
+"tion)"
+ LogicDataType "uint(8)"
+ ZeroCross on
+ SampleTime "-1"
+ }
Block {
BlockType Saturate
UpperLimit "0.5"
SFunctionModules "''"
PortCounts "[]"
}
+ Block {
+ BlockType SignalSpecification
+ Dimensions "-1"
+ SampleTime "-1"
+ DataType "auto"
+ OutDataType "sfix(16)"
+ OutScaling "2^0"
+ SignalType "auto"
+ SamplingMode "auto"
+ }
Block {
BlockType SubSystem
ShowPortLabels "FromPortIcon"
SampleTime "-1"
}
Block {
- BlockType ToWorkspace
- VariableName "simulink_output"
- MaxDataPoints "1000"
- Decimation "1"
- SampleTime "0"
- FixptAsFi off
+ BlockType TransportDelay
+ DelayTime "1"
+ InitialOutput "0"
+ BufferSize "1024"
+ FixedBuffer off
+ TransDelayFeedthrough off
+ PadeOrder "0"
+ }
+ Block {
+ BlockType TriggerPort
+ TriggerType "rising"
+ StatesWhenEnabling "inherit"
+ ShowOutputPort off
+ OutputDataType "auto"
+ SampleTimeType "triggered"
+ SampleTime "1"
+ ZeroCross on
}
Block {
BlockType UnitDelay
}
System {
Name "reg_mcl"
- Location [1684, 55, 2539, 624]
+ Location [4, 523, 859, 1000]
Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
BlockType SubSystem
Name " "
Ports []
- Position [25, 14, 95, 48]
+ Position [30, 14, 100, 48]
OpenFcn "path=diag([area.w area.h])*rand(2,8)\nsim('reg_"
-"test',0) "
+"mcl',0) "
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
FunctionWithSeparateData off
ZoomFactor "100"
}
}
+ Block {
+ BlockType SubSystem
+ Name " 2"
+ Ports []
+ Position [15, 54, 121, 89]
+ Priority "0"
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ MaskDisplay "disp(sprintf('Sample time: %g', move_sample_tim"
+"e))"
+ MaskIconFrame on
+ MaskIconOpaque on
+ MaskIconRotate "none"
+ MaskIconUnits "autoscale"
+ System {
+ Name " 2"
+ Location [433, 403, 931, 703]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ }
+ }
Block {
BlockType Clock
Name "Clock"
- Position [465, 70, 485, 90]
+ Position [245, 55, 265, 75]
Decimation "10"
}
Block {
BlockType "S-Function"
Name "Controller"
Ports [2, 1]
- Position [175, 88, 235, 177]
+ Position [175, 118, 235, 207]
BackgroundColor "green"
FunctionName "sf_posreg"
- Parameters "[2 2 10],1/20"
- MaskDisplay "port_label('input', 1, 'ref')\nport_label('inpu"
-"t', 2, 'pos')\nport_label('output', 1, '(w1,w2)')"
+ Parameters "[2 2 10],move_sample_time"
+ MaskDisplay "port_label('input', 1, 'ref pos')\nport_label('"
+"input', 2, 'measured pos')\nport_label('output', 1, '(v,w)')"
MaskIconFrame on
MaskIconOpaque on
MaskIconRotate "none"
BlockType Display
Name "Display"
Ports [1]
- Position [515, 66, 600, 94]
+ Position [295, 51, 380, 79]
Lockdown off
}
Block {
- BlockType Gain
- Name "Gain"
- Position [260, 120, 290, 150]
- Gain "[1 1]/desc.robot.wheelr"
- ParameterDataTypeMode "Inherit via internal rule"
- OutDataTypeMode "Inherit via internal rule"
- SaturateOnIntegerOverflow off
+ BlockType "M-S-Function"
+ Name "Estimated\nposition"
+ Ports [2]
+ Position [725, 308, 790, 352]
+ ForegroundColor "blue"
+ FunctionName "sfdrawrobot"
+ Parameters "desc,'--b'"
+ MaskDisplay "disp('Drawer')\nport_label('input', 1, 'x')\npo"
+"rt_label('input', 2, 'y')\n"
+ MaskIconFrame on
+ MaskIconOpaque on
+ MaskIconRotate "none"
+ MaskIconUnits "autoscale"
}
Block {
BlockType SubSystem
Name "Graphs"
Ports [2]
- Position [720, 210, 820, 270]
+ Position [725, 180, 825, 240]
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
FunctionWithSeparateData off
Ports [1]
Position [410, 109, 440, 141]
Floating off
- Location [437, 662, 1088, 1030]
- Open on
+ Location [437, 630, 1088, 998]
+ Open off
NumInputPorts "1"
List {
ListType AxesTitles
Ports [1]
Position [410, 49, 440, 81]
Floating off
- Location [441, 56, 1084, 453]
- Open on
+ Location [254, 56, 1109, 218]
+ Open off
NumInputPorts "1"
ZoomMode "xonly"
List {
ListType AxesTitles
axes1 "%<SignalLabel>"
}
- TimeRange "100"
- YMin "-0.5"
- YMax "4"
+ TimeRange "25"
+ YMin "0"
+ YMax "2.75"
DataFormat "StructureWithTime"
SampleTime "0"
}
}
}
}
- Block {
- BlockType "M-S-Function"
- Name "Level-2 M-file\nS-Function"
- Ports [2]
- Position [725, 124, 785, 171]
- BackgroundColor "cyan"
- FunctionName "sfdrawrobot"
- Parameters "desc,0"
- MaskDisplay "disp('Drawer')\nport_label('input', 1, 'x')\npo"
-"rt_label('input', 2, 'y')\n"
- MaskIconFrame on
- MaskIconOpaque on
- MaskIconRotate "none"
- MaskIconUnits "autoscale"
- }
- Block {
- BlockType "M-S-Function"
- Name "Level-2 M-file\nS-Function1"
- Ports [2]
- Position [725, 293, 790, 337]
- ForegroundColor "blue"
- FunctionName "sfdrawrobot"
- Parameters "desc,'--b'"
- MaskDisplay "disp('Drawer')\nport_label('input', 1, 'x')\npo"
-"rt_label('input', 2, 'y')\n"
- MaskIconFrame on
- MaskIconOpaque on
- MaskIconRotate "none"
- MaskIconUnits "autoscale"
- }
- Block {
- BlockType "M-S-Function"
- Name "Level-2 M-file\nS-Function2"
- Ports [2]
- Position [715, 28, 780, 72]
- ForegroundColor "magenta"
- FunctionName "sfdrawrobot"
- Parameters "desc,'--m'"
- MaskDisplay "disp('Drawer')\nport_label('input', 1, 'x')\npo"
-"rt_label('input', 2, 'y')\n"
- MaskIconFrame on
- MaskIconOpaque on
- MaskIconRotate "none"
- MaskIconUnits "autoscale"
- }
Block {
BlockType "S-Function"
Name "MCL"
Ports [2, 2]
- Position [430, 288, 610, 357]
+ Position [215, 313, 395, 382]
+ Orientation "left"
BackgroundColor "green"
ShowName off
FunctionName "sf_mcl"
MaskVariables "count=@1;noise_xy=@2;noise_angle=@3;aeval_sigma"
"=@4;"
MaskDisplay "port_label('input', 1, '(dx,dy,da)')\nport_labe"
-"l('input', 2, '(th1,th2,th3)')\nport_label('output', 1, 'est_pos=(x,y,a)')\np"
-"ort_label('output', 2, 'particle bitmap')\ndisp('MCL')"
+"l('input', 2, 'measurement')\nport_label('output', 1, 'est_pos=(x,y,a)')\npor"
+"t_label('output', 2, 'particle bitmap')\ndisp('MCL')"
MaskIconFrame on
MaskIconOpaque on
MaskIconRotate "none"
MaskIconUnits "autoscale"
- MaskValueString "3000|0.01|2/180*pi|5/180*pi"
+ MaskValueString "1000|0.01|3/180*pi|3/180*pi"
MaskTabNameString ",,,"
+ Port {
+ PortNumber 1
+ Name "Estimated position"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
}
Block {
BlockType Reference
Name "Manual Switch"
Ports [2, 1]
- Position [125, 137, 155, 173]
+ Position [125, 167, 155, 203]
SourceBlock "simulink/Signal\nRouting/Manual Switch"
SourceType "Manual Switch"
ShowPortLabels "FromPortIcon"
varsize off
}
Block {
- BlockType SubSystem
- Name "Measurement"
- Ports [1, 1]
- Position [205, 397, 280, 433]
- MinAlgLoopOccurrences off
- RTWSystemCode "Auto"
- FunctionWithSeparateData off
- MaskHideContents off
- System {
- Name "Measurement"
- Location [458, 185, 768, 279]
- Open off
- ModelBrowserVisibility off
- ModelBrowserWidth 200
- ScreenColor "white"
- PaperOrientation "landscape"
- PaperPositionMode "auto"
- PaperType "A4"
- PaperUnits "centimeters"
- TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
- TiledPageScale 1
- ShowPageBoundaries off
- ZoomFactor "100"
- Block {
- BlockType Inport
- Name "Pos"
- Position [25, 43, 55, 57]
- IconDisplay "Port number"
- }
- Block {
- BlockType MATLABFcn
- Name "Beacon Angles"
- Ports [1, 1]
- Position [125, 35, 185, 65]
- MATLABFcn "mod(beacon_angles(u), 2*pi)"
- }
- Block {
- BlockType Outport
- Name "Angles"
- Position [255, 43, 285, 57]
- IconDisplay "Port number"
- BusOutputAsStruct off
- }
- Line {
- SrcBlock "Pos"
- SrcPort 1
- DstBlock "Beacon Angles"
- DstPort 1
- }
- Line {
- SrcBlock "Beacon Angles"
- SrcPort 1
- DstBlock "Angles"
- DstPort 1
- }
- }
+ BlockType Ground
+ Name "No laser"
+ Position [530, 220, 550, 240]
}
Block {
- BlockType Reference
- Name "Measurement noise\nstd. deviation [deg]"
- Ports [1, 1]
- Position [289, 460, 341, 480]
+ BlockType Ground
+ Name "No odometry"
+ Position [470, 220, 490, 240]
+ Orientation "left"
+ }
+ Block {
+ BlockType RateTransition
+ Name "Rate Transition1"
+ Position [55, 309, 95, 351]
+ Orientation "left"
+ X0 "desc.initpos"
+ }
+ Block {
+ BlockType RateTransition
+ Name "Rate Transition2"
+ Position [49, 230, 91, 270]
Orientation "up"
- NamePlacement "alternate"
- SourceBlock "simulink/Math\nOperations/Slider\nGain"
- SourceType "Slider Gain"
- ShowPortLabels "FromPortIcon"
- SystemSampleTime "-1"
- FunctionWithSeparateData off
- RTWMemSecFuncInitTerm "Inherit from model"
- RTWMemSecFuncExecute "Inherit from model"
- RTWMemSecDataConstants "Inherit from model"
- RTWMemSecDataInternal "Inherit from model"
- RTWMemSecDataParameters "Inherit from model"
- low "0"
- gain "1"
- high "5"
+ X0 "desc.initpos"
}
Block {
- BlockType Constant
- Name "No laser"
- Position [220, 369, 280, 391]
- NamePlacement "alternate"
- Value "[0 0 0]"
- SampleTime "-1"
+ BlockType "M-S-Function"
+ Name "Real\nposition"
+ Ports [2]
+ Position [725, 94, 785, 141]
+ BackgroundColor "cyan"
+ FunctionName "sfdrawrobot"
+ Parameters "desc,0"
+ MaskDisplay "disp('Drawer')\nport_label('input', 1, 'x')\npo"
+"rt_label('input', 2, 'y')\n"
+ MaskIconFrame on
+ MaskIconOpaque on
+ MaskIconRotate "none"
+ MaskIconUnits "autoscale"
}
Block {
- BlockType Constant
- Name "No odometry"
- Position [245, 269, 305, 291]
- NamePlacement "alternate"
- Value "[0 0 0]"
- SampleTime "-1"
+ BlockType "M-S-Function"
+ Name "Reference\nposition"
+ Ports [2]
+ Position [725, 13, 790, 57]
+ ForegroundColor "magenta"
+ FunctionName "sfdrawrobot"
+ Parameters "desc,'--m'"
+ MaskDisplay "disp('Drawer')\nport_label('input', 1, 'x')\npo"
+"rt_label('input', 2, 'y')\n"
+ MaskIconFrame on
+ MaskIconOpaque on
+ MaskIconRotate "none"
+ MaskIconUnits "autoscale"
}
Block {
BlockType SubSystem
- Name "Pos to deltas"
- Ports [1, 1]
- Position [210, 296, 305, 334]
+ Name "Robot kinematics"
+ Ports [1, 2]
+ Position [260, 137, 385, 188]
+ BackgroundColor "[0.912000, 0.763112, 0.484696]"
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
FunctionWithSeparateData off
MaskHideContents off
+ MaskDisplay "disp('Robot\\n\\n')\nport_label('input', 1, 'u="
+"(w1,w2)')\nport_label('output', 1, 'x=(X,Y,th)')\nport_label('output', 2, 'od"
+"o=(dx,dy,da)')"
+ MaskIconFrame on
+ MaskIconOpaque on
+ MaskIconRotate "none"
+ MaskIconUnits "autoscale"
+ Port {
+ PortNumber 1
+ Name "Real robot position"
+ PropagatedSignals "x,y,phi [m,m,rad]"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
System {
- Name "Pos to deltas"
- Location [50, 111, 624, 360]
+ Name "Robot kinematics"
+ Location [4, 55, 1176, 412]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ZoomFactor "100"
Block {
BlockType Inport
- Name "Pos"
- Position [30, 103, 60, 117]
+ Name "In1"
+ Position [20, 38, 50, 52]
IconDisplay "Port number"
+ Port {
+ PortNumber 1
+ Name "(v,w)"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
}
Block {
BlockType Selector
Name "Angle"
Ports [1, 1]
- Position [190, 21, 230, 59]
+ Position [675, 151, 715, 189]
IndexOptions "Index vector (dialog)"
- Indices "3"
+ Indices "[3]"
OutputSizes "1"
InputPortWidth "3"
}
BlockType Reference
Name "Difference"
Ports [1, 1]
- Position [100, 92, 145, 128]
+ Position [670, 222, 715, 258]
NamePlacement "alternate"
SourceBlock "simulink/Discrete/Difference"
SourceType "Difference"
RTWMemSecDataConstants "Inherit from model"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
- ICPrevInput "0.0"
+ ICPrevInput "desc.initpos"
OutputDataTypeScalingMode "Inherit via internal rule"
OutDataType "sfix(16)"
OutScaling "2^-10"
Block {
BlockType Gain
Name "Gain"
- Position [250, 24, 285, 56]
+ Position [815, 154, 850, 186]
Gain "-1"
ParameterDataTypeMode "Inherit via internal rule"
OutDataTypeMode "Inherit via internal rule"
SaturateOnIntegerOverflow off
}
Block {
- BlockType Mux
- Name "Mux1"
+ BlockType Integrator
+ Name "Integrator"
+ Ports [1, 1]
+ Position [510, 40, 540, 70]
+ InitialCondition "desc.initpos"
+ LimitOutput on
+ UpperSaturationLimit "[area.w area.h Inf]"
+ LowerSaturationLimit "[0 0 -Inf]"
+ IgnoreLimit off
+ Port {
+ PortNumber 1
+ Name "x,y,phi [m,m,rad]"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ }
+ Block {
+ BlockType SubSystem
+ Name "Motors"
+ Ports [1, 1]
+ Position [210, 13, 310, 77]
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ Port {
+ PortNumber 1
+ Name "(vl,vr)"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ System {
+ Name "Motors"
+ Location [197, 366, 934, 571]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "ref omega"
+ Position [25, 43, 55, 57]
+ IconDisplay "Port number"
+ Port {
+ PortNumber 1
+ Name "m/s"
+ PropagatedSignals "(vl,vr)"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ }
+ Block {
+ BlockType Reference
+ Name "Manual Switch"
+ Ports [2, 1]
+ Position [430, 32, 460, 68]
+ SourceBlock "simulink/Signal\nRouting/Manual Switch"
+ SourceType "Manual Switch"
+ ShowPortLabels "FromPortIcon"
+ SystemSampleTime "-1"
+ FunctionWithSeparateData off
+ RTWMemSecFuncInitTerm "Inherit from model"
+ RTWMemSecFuncExecute "Inherit from model"
+ RTWMemSecDataConstants "Inherit from model"
+ RTWMemSecDataInternal "Inherit from model"
+ RTWMemSecDataParameters "Inherit from model"
+ sw "1"
+ action "0"
+ varsize off
+ }
+ Block {
+ BlockType RateTransition
+ Name "Rate Transition1"
+ Position [325, 89, 365, 131]
+ X0 "[0 0]"
+ }
+ Block {
+ BlockType RateTransition
+ Name "Rate Transition2"
+ Position [325, 19, 365, 61]
+ X0 "[0 0]"
+ }
+ Block {
+ BlockType RateLimiter
+ Name "acc limit"
+ Position [260, 25, 290, 55]
+ AttributesFormatString "<%<FallingSlewLimit>,%<RisingSlewLimit>"
+"> m/s^2"
+ RisingSlewLimit "3"
+ FallingSlewLimit "-3"
+ SampleTimeMode "inherited"
+ }
+ Block {
+ BlockType Saturate
+ Name "speed limit"
+ Position [175, 35, 205, 65]
+ AttributesFormatString "<%<LowerLimit>,%<UpperLimit>> m/s"
+ UpperLimit "2"
+ LowerLimit "-2"
+ }
+ Block {
+ BlockType Outport
+ Name "(vl,vr)"
+ Position [485, 43, 515, 57]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "Rate Transition1"
+ SrcPort 1
+ Points [45, 0]
+ DstBlock "Manual Switch"
+ DstPort 2
+ }
+ Line {
+ SrcBlock "Rate Transition2"
+ SrcPort 1
+ DstBlock "Manual Switch"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "acc limit"
+ SrcPort 1
+ DstBlock "Rate Transition2"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "speed limit"
+ SrcPort 1
+ Points [20, 0]
+ Branch {
+ Points [0, 60]
+ DstBlock "Rate Transition1"
+ DstPort 1
+ }
+ Branch {
+ Points [0, -10]
+ DstBlock "acc limit"
+ DstPort 1
+ }
+ }
+ Line {
+ SrcBlock "Manual Switch"
+ SrcPort 1
+ DstBlock "(vl,vr)"
+ DstPort 1
+ }
+ Line {
+ Name "m/s"
+ Labels [1, 0]
+ SrcBlock "ref omega"
+ SrcPort 1
+ DstBlock "speed limit"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType Mux
+ Name "Mux1"
Ports [2, 1]
- Position [315, 81, 320, 119]
+ Position [360, 36, 365, 74]
ShowName off
Inputs "2"
DisplayOption "bar"
BlockType Mux
Name "Mux2"
Ports [2, 1]
- Position [460, 91, 465, 129]
+ Position [885, 211, 890, 249]
ShowName off
Inputs "2"
DisplayOption "bar"
}
+ Block {
+ BlockType Mux
+ Name "Mux3"
+ Ports [2, 1]
+ Position [1030, 221, 1035, 259]
+ ShowName off
+ Inputs "2"
+ DisplayOption "bar"
+ }
+ Block {
+ BlockType RateTransition
+ Name "Rate Transition2"
+ Position [589, 85, 631, 125]
+ Orientation "down"
+ X0 "desc.initpos"
+ OutPortSampleTime "move_sample_time"
+ }
Block {
BlockType MATLABFcn
Name "Rotate XY"
Ports [1, 1]
- Position [350, 83, 435, 117]
+ Position [920, 213, 1005, 247]
MATLABFcn "[cos(u(1)) -sin(u(1)); sin(u(1)) cos(u(1))]"
"*[u(2); u(3)]"
}
+ Block {
+ BlockType Selector
+ Name "Selector"
+ Ports [1, 1]
+ Position [546, 85, 584, 125]
+ Orientation "down"
+ NamePlacement "alternate"
+ IndexOptions "Index vector (dialog)"
+ Indices "[3]"
+ OutputSizes "1"
+ InputPortWidth "3"
+ Port {
+ PortNumber 1
+ Name "angle"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ }
Block {
BlockType Selector
Name "Speed XY"
Ports [1, 1]
- Position [190, 91, 230, 129]
+ Position [760, 221, 800, 259]
IndexOptions "Index vector (dialog)"
Indices "[1 2]"
OutputSizes "1"
BlockType Selector
Name "Speed angle"
Ports [1, 1]
- Position [190, 151, 230, 189]
+ Position [760, 281, 800, 319]
IndexOptions "Index vector (dialog)"
Indices "3"
OutputSizes "1"
}
}
Block {
- BlockType UnitDelay
- Name "Unit Delay"
- Position [110, 23, 145, 57]
- SampleTime "-1"
+ BlockType MATLABFcn
+ Name "kinematics"
+ Ports [1, 1]
+ Position [410, 40, 470, 70]
+ MATLABFcn "kinematics"
+ }
+ Block {
+ BlockType MATLABFcn
+ Name "speed2wheel"
+ Ports [1, 1]
+ Position [105, 30, 165, 60]
+ MATLABFcn "[u(1)-robot.w/2*u(2); u(1)+robot.w/2*u(2)]"
+ Port {
+ PortNumber 1
+ Name "(vl,vr)"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ }
+ Block {
+ BlockType Outport
+ Name "Pos"
+ Position [780, 48, 810, 62]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
}
Block {
BlockType Outport
Name "Odometry"
- Position [500, 103, 530, 117]
+ Position [1090, 233, 1120, 247]
+ Port "2"
IconDisplay "Port number"
BusOutputAsStruct off
}
Line {
- SrcBlock "Gain"
+ SrcBlock "kinematics"
+ SrcPort 1
+ DstBlock "Integrator"
+ DstPort 1
+ }
+ Line {
+ Name "x,y,phi [m,m,rad]"
+ SrcBlock "Integrator"
+ SrcPort 1
+ Points [20, 0]
+ Branch {
+ DstBlock "Selector"
+ DstPort 1
+ }
+ Branch {
+ Points [45, 0]
+ Branch {
+ Labels [1, 1; 1, 0]
+ DstBlock "Pos"
+ DstPort 1
+ }
+ Branch {
+ DstBlock "Rate Transition2"
+ DstPort 1
+ }
+ }
+ }
+ Line {
+ SrcBlock "Mux1"
+ SrcPort 1
+ DstBlock "kinematics"
+ DstPort 1
+ }
+ Line {
+ Name "angle"
+ Labels [0, 0]
+ SrcBlock "Selector"
+ SrcPort 1
+ Points [0, 15; -230, 0; 0, -80]
+ DstBlock "Mux1"
+ DstPort 2
+ }
+ Line {
+ Name "(vl,vr)"
+ Labels [0, 0]
+ SrcBlock "Motors"
SrcPort 1
- Points [0, 50]
DstBlock "Mux1"
DstPort 1
}
Line {
- SrcBlock "Angle"
+ Name "(vl,vr)"
+ Labels [0, 0]
+ SrcBlock "speed2wheel"
SrcPort 1
- DstBlock "Gain"
+ DstBlock "Motors"
DstPort 1
}
Line {
- SrcBlock "Unit Delay"
+ Name "(v,w)"
+ Labels [0, 0]
+ SrcBlock "In1"
SrcPort 1
- DstBlock "Angle"
+ DstBlock "speed2wheel"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Gain"
+ SrcPort 1
+ Points [0, 50]
+ DstBlock "Mux2"
DstPort 1
}
Line {
SrcPort 1
Points [5, 0]
Branch {
- Points [0, 60]
- DstBlock "Speed angle"
+ DstBlock "Speed XY"
DstPort 1
}
Branch {
- DstBlock "Speed XY"
+ Points [0, 60]
+ DstBlock "Speed angle"
DstPort 1
}
}
Labels [0, 0]
SrcBlock "Speed XY"
SrcPort 1
- DstBlock "Mux1"
+ DstBlock "Mux2"
DstPort 2
}
Line {
SrcBlock "Speed angle"
SrcPort 1
Points [210, 0]
- DstBlock "Mux2"
+ DstBlock "Mux3"
DstPort 2
}
Line {
- SrcBlock "Mux2"
+ SrcBlock "Mux3"
SrcPort 1
DstBlock "Odometry"
DstPort 1
Line {
SrcBlock "Rotate XY"
SrcPort 1
- DstBlock "Mux2"
+ DstBlock "Mux3"
DstPort 1
}
Line {
- SrcBlock "Mux1"
+ SrcBlock "Mux2"
SrcPort 1
DstBlock "Rotate XY"
DstPort 1
}
Line {
- SrcBlock "Pos"
+ SrcBlock "Angle"
+ SrcPort 1
+ DstBlock "Gain"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Rate Transition2"
SrcPort 1
- Points [10, 0]
+ Points [0, 40]
Branch {
- Points [0, -70]
- DstBlock "Unit Delay"
+ Points [0, 70]
+ DstBlock "Difference"
DstPort 1
}
Branch {
- DstBlock "Difference"
+ DstBlock "Angle"
DstPort 1
}
}
}
}
- Block {
- BlockType RandomNumber
- Name "Random\nNumber"
- Position [350, 455, 380, 485]
- Orientation "down"
- Variance "(1/180*pi)^2"
- Seed "abs(rand(3,1)*10000)"
- }
- Block {
- BlockType RateTransition
- Name "Rate Transition"
- Position [135, 394, 175, 436]
- OutPortSampleTime "1/3"
- }
- Block {
- BlockType RateTransition
- Name "Rate Transition1"
- Position [135, 494, 175, 536]
- Orientation "left"
- X0 "desc.initpos"
- }
- Block {
- BlockType "S-Function"
- Name "S-Function2"
- Ports [0, 1]
- Position [55, 94, 115, 126]
- BackgroundColor "green"
- FunctionName "sf_trgen"
- Parameters "path, desc.initpos, desc.wayconstraints"
- MaskDisplay "disp('Trajectory\\ngenerator')"
- MaskIconFrame on
- MaskIconOpaque on
- MaskIconRotate "none"
- MaskIconUnits "autoscale"
- }
Block {
BlockType SubSystem
- Name "Subsystem"
- Ports [1, 1]
- Position [310, 111, 435, 159]
- BackgroundColor "yellow"
+ Name "Rotating laser"
+ Ports [1, 3]
+ Position [425, 119, 570, 181]
+ BackgroundColor "[0.912000, 0.763112, 0.484696]"
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
FunctionWithSeparateData off
MaskHideContents off
- MaskDisplay "disp('Robot\\n\\n')\nport_label('input', 1, 'u="
-"(w1,w2)')\nport_label('output', 1, 'x=(X,Y,th)')"
+ MaskPromptString "Rotations per second|Noise standard deviation ["
+"deg]|Zero-cross detection"
+ MaskStyleString "edit,edit,checkbox"
+ MaskTunableValueString "on,on,on"
+ MaskCallbackString "||"
+ MaskEnableString "on,on,on"
+ MaskVisibilityString "on,on,on"
+ MaskToolTipString "on,on,on"
+ MaskVarAliasString ",,"
+ MaskVariables "rps=@1;noise_stdev=@2;zerocross=@3;"
+ MaskDisplay "text(0.3,0.5, sprintf('RPS=%g\\nNoise=%g', rps,"
+" noise_stdev))\nport_label('input', 1, 'Pos')\nport_label('output', 1, 'Ideal"
+"')\nport_label('output', 2, 'Immediate')\nport_label('output', 3, 'Buffered')"
+"\n"
MaskIconFrame on
MaskIconOpaque on
MaskIconRotate "none"
- MaskIconUnits "autoscale"
- Port {
- PortNumber 1
- Name "Real robot position"
- PropagatedSignals "x,y,phi [m,m,rad]"
- RTWStorageClass "Auto"
- DataLoggingNameMode "SignalName"
- }
+ MaskIconUnits "normalized"
+ MaskValueString "3|1|on"
+ MaskTabNameString ",,"
System {
- Name "Subsystem"
- Location [121, 481, 955, 665]
+ Name "Rotating laser"
+ Location [651, 66, 1675, 656]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ZoomFactor "100"
Block {
BlockType Inport
- Name "In1"
- Position [15, 38, 45, 52]
+ Name "Pos"
+ Position [25, 33, 55, 47]
IconDisplay "Port number"
- Port {
- PortNumber 1
- Name "rad/s"
- RTWStorageClass "Auto"
- DataLoggingNameMode "SignalName"
- }
}
Block {
- BlockType Integrator
- Name "Integrator"
+ BlockType MATLABFcn
+ Name "Beacon Angles"
Ports [1, 1]
- Position [530, 40, 560, 70]
- InitialCondition "desc.initpos"
- UpperSaturationLimit "[area.w area.h Inf]"
- LowerSaturationLimit "[0 0 -Inf]"
- IgnoreLimit off
- Port {
- PortNumber 1
- Name "x,y,phi [m,m,rad]"
- RTWStorageClass "Auto"
- DataLoggingNameMode "SignalName"
+ Position [150, 25, 210, 55]
+ MATLABFcn "mod(beacon_angles(u), 2*pi)"
+ }
+ Block {
+ BlockType SubSystem
+ Name "Beacon memory"
+ Ports [1, 1, 0, 1]
+ Position [510, 299, 565, 331]
+ TreatAsAtomicUnit on
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Beacon memory"
+ Location [401, 346, 899, 646]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "In1"
+ Position [110, 103, 140, 117]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType TriggerPort
+ Name "Trigger"
+ Ports []
+ Position [225, 20, 245, 40]
+ StatesWhenEnabling "held"
+ }
+ Block {
+ BlockType Outport
+ Name "Out1"
+ Position [360, 103, 390, 117]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Out1"
+ DstPort 1
+ }
}
}
+ Block {
+ BlockType SubSystem
+ Name "Beacon memory1"
+ Ports [1, 1, 0, 1]
+ Position [510, 369, 565, 401]
+ TreatAsAtomicUnit on
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Beacon memory1"
+ Location [401, 346, 899, 646]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "In1"
+ Position [110, 103, 140, 117]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType TriggerPort
+ Name "Trigger"
+ Ports []
+ Position [225, 20, 245, 40]
+ StatesWhenEnabling "held"
+ }
+ Block {
+ BlockType Outport
+ Name "Out1"
+ Position [360, 103, 390, 117]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Out1"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType SubSystem
+ Name "Beacon memory2"
+ Ports [1, 1, 0, 1]
+ Position [510, 439, 565, 471]
+ TreatAsAtomicUnit on
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Beacon memory2"
+ Location [401, 346, 899, 646]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "In1"
+ Position [110, 103, 140, 117]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType TriggerPort
+ Name "Trigger"
+ Ports []
+ Position [225, 20, 245, 40]
+ StatesWhenEnabling "held"
+ }
+ Block {
+ BlockType Outport
+ Name "Out1"
+ Position [360, 103, 390, 117]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Out1"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType SubSystem
+ Name "Beacon memory3"
+ Ports [1, 1, 0, 1]
+ Position [510, 509, 565, 541]
+ TreatAsAtomicUnit on
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Beacon memory3"
+ Location [401, 346, 899, 646]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "In1"
+ Position [110, 103, 140, 117]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType TriggerPort
+ Name "Trigger"
+ Ports []
+ Position [225, 20, 245, 40]
+ StatesWhenEnabling "held"
+ }
+ Block {
+ BlockType Outport
+ Name "Out1"
+ Position [360, 103, 390, 117]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Out1"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType Clock
+ Name "Clock"
+ Position [740, 320, 760, 340]
+ Decimation "10"
+ }
Block {
BlockType Reference
- Name "Manual Switch"
- Ports [2, 1]
- Position [250, 27, 280, 63]
- SourceBlock "simulink/Signal\nRouting/Manual Switch"
- SourceType "Manual Switch"
+ Name "Compare\nTo Constant"
+ Ports [1, 1]
+ Position [35, 164, 100, 196]
+ Orientation "left"
+ SourceBlock "simulink/Logic and Bit\nOperations/Compare"
+"\nTo Constant"
+ SourceType "Compare To Constant"
ShowPortLabels "FromPortIcon"
SystemSampleTime "-1"
FunctionWithSeparateData off
RTWMemSecDataConstants "Inherit from model"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
- sw "1"
- action "0"
- varsize off
+ relop ">="
+ const "2*pi"
+ LogicOutDataTypeMode "boolean"
+ ZeroCross off
+ }
+ Block {
+ BlockType Reference
+ Name "Compare\nTo Zero"
+ Ports [1, 1]
+ Position [800, 315, 830, 345]
+ SourceBlock "simulink/Logic and Bit\nOperations/Compare"
+"\nTo Zero"
+ SourceType "Compare To Zero"
+ ShowPortLabels "FromPortIcon"
+ SystemSampleTime "-1"
+ FunctionWithSeparateData off
+ RTWMemSecFuncInitTerm "Inherit from model"
+ RTWMemSecFuncExecute "Inherit from model"
+ RTWMemSecDataConstants "Inherit from model"
+ RTWMemSecDataInternal "Inherit from model"
+ RTWMemSecDataParameters "Inherit from model"
+ relop ">"
+ LogicOutDataTypeMode "uint8"
+ ZeroCross off
+ }
+ Block {
+ BlockType Constant
+ Name "Constant1"
+ Position [35, 239, 100, 271]
+ NamePlacement "alternate"
+ Value "rps*2*pi"
+ }
+ Block {
+ BlockType Demux
+ Name "Demux"
+ Ports [1, 4]
+ Position [346, 190, 399, 200]
+ Orientation "down"
+ BackgroundColor "black"
+ NamePlacement "alternate"
+ ShowName off
+ DisplayOption "bar"
+ }
+ Block {
+ BlockType Demux
+ Name "Demux1"
+ Ports [1, 4]
+ Position [421, 190, 474, 200]
+ Orientation "down"
+ BackgroundColor "black"
+ NamePlacement "alternate"
+ ShowName off
+ DisplayOption "bar"
+ }
+ Block {
+ BlockType SubSystem
+ Name "Do not produce\ndata at start. We\nhave to "
+"wait one turn."
+ Ports [1, 1, 1]
+ Position [825, 374, 925, 416]
+ TreatAsAtomicUnit on
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Do not produce\ndata at start. We\nhave t"
+"o wait one turn."
+ Location [421, 301, 919, 601]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "In1"
+ Position [110, 103, 140, 117]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType EnablePort
+ Name "Enable"
+ Ports []
+ Position [235, 20, 255, 40]
+ }
+ Block {
+ BlockType Outport
+ Name "Out1"
+ Position [360, 103, 390, 117]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Out1"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType HitCross
+ Name "Hit \nCrossing"
+ Ports [1, 1]
+ Position [320, 95, 350, 125]
+ AttributesFormatString "ZC:%<ZeroCross>"
+ HitCrossingDirection "rising"
+ }
+ Block {
+ BlockType SubSystem
+ Name "Individual output"
+ Ports [1, 1, 0, 1]
+ Position [745, 226, 800, 274]
+ TreatAsAtomicUnit on
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Individual output"
+ Location [401, 346, 899, 646]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "In1"
+ Position [110, 103, 140, 117]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType TriggerPort
+ Name "Trigger"
+ Ports []
+ Position [225, 20, 245, 40]
+ StatesWhenEnabling "held"
+ }
+ Block {
+ BlockType Outport
+ Name "Out1"
+ Position [360, 103, 390, 117]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Out1"
+ DstPort 1
+ }
+ }
+ }
+ Block {
+ BlockType Integrator
+ Name "Integrator"
+ Ports [2, 1, 0, 0, 1]
+ Position [130, 238, 180, 307]
+ ExternalReset "rising"
+ ShowStatePort on
+ IgnoreLimit off
+ }
+ Block {
+ BlockType RandomNumber
+ Name "Measurement noise"
+ Position [475, 145, 505, 175]
+ Orientation "left"
+ NamePlacement "alternate"
+ Variance "(noise_stdev/180*pi)^2"
+ Seed "abs(floor(rand(4,1)*10000))"
}
Block {
BlockType Mux
- Name "Mux1"
- Ports [2, 1]
- Position [380, 36, 385, 74]
+ Name "Mux"
+ Ports [4, 1]
+ Position [700, 354, 705, 436]
ShowName off
- Inputs "2"
DisplayOption "bar"
}
+ Block {
+ BlockType RateTransition
+ Name "Rate Transition"
+ Position [750, 374, 790, 416]
+ OutPortSampleTime "1/rps"
+ }
+ Block {
+ BlockType RateTransition
+ Name "Rate Transition1"
+ Position [735, 19, 775, 61]
+ OutPortSampleTime "1/rps"
+ }
Block {
BlockType Selector
Name "Selector"
Ports [1, 1]
- Position [420, 121, 460, 159]
- Orientation "left"
+ Position [585, 91, 625, 129]
IndexOptions "Index vector (dialog)"
- Indices "[3]"
+ Indices "[1 2 3 4]"
OutputSizes "1"
- InputPortWidth "3"
- Port {
- PortNumber 1
- Name "angle"
- RTWStorageClass "Auto"
- DataLoggingNameMode "SignalName"
- }
+ InputPortWidth "4"
}
Block {
- BlockType Gain
- Name "Wheel\nradius"
- Position [90, 27, 135, 63]
- Gain "robot.wheelr"
- ParameterDataTypeMode "Inherit via internal rule"
+ BlockType Sum
+ Name "Subtract"
+ Ports [2, 1]
+ Position [270, 95, 300, 125]
+ IconShape "round"
+ Inputs "-+|"
+ CollapseMode "All dimensions"
+ InputSameDT off
OutDataTypeMode "Inherit via internal rule"
+ OutScaling "2^-10"
SaturateOnIntegerOverflow off
- Port {
- PortNumber 1
- Name "m/s"
- RTWStorageClass "Auto"
- DataLoggingNameMode "SignalName"
- }
}
Block {
- BlockType MATLABFcn
- Name "kinematics"
- Ports [1, 1]
- Position [430, 40, 490, 70]
- MATLABFcn "kinematics"
+ BlockType Sum
+ Name "Sum"
+ Ports [2, 1]
+ Position [480, 240, 500, 260]
+ ShowName off
+ IconShape "round"
+ Inputs "++|"
+ CollapseMode "All dimensions"
+ InputSameDT off
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
+ Block {
+ BlockType Sum
+ Name "Sum1"
+ Ports [2, 1]
+ Position [460, 305, 480, 325]
+ ShowName off
+ IconShape "round"
+ Inputs "++|"
+ CollapseMode "All dimensions"
+ InputSameDT off
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
+ Block {
+ BlockType Sum
+ Name "Sum2"
+ Ports [2, 1]
+ Position [445, 375, 465, 395]
+ ShowName off
+ IconShape "round"
+ Inputs "++|"
+ CollapseMode "All dimensions"
+ InputSameDT off
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
+ Block {
+ BlockType Sum
+ Name "Sum3"
+ Ports [2, 1]
+ Position [430, 445, 450, 465]
+ ShowName off
+ IconShape "round"
+ Inputs "++|"
+ CollapseMode "All dimensions"
+ InputSameDT off
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
+ Block {
+ BlockType Sum
+ Name "Sum4"
+ Ports [2, 1]
+ Position [415, 515, 435, 535]
+ ShowName off
+ IconShape "round"
+ Inputs "++|"
+ CollapseMode "All dimensions"
+ InputSameDT off
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
+ Block {
+ BlockType Sum
+ Name "Sum5"
+ Ports [2, 1]
+ Position [400, 30, 420, 50]
+ ShowName off
+ IconShape "round"
+ Inputs "|++"
+ CollapseMode "All dimensions"
+ InputSameDT off
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
+ Block {
+ BlockType Outport
+ Name "Ideal"
+ Position [855, 33, 885, 47]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Block {
+ BlockType Outport
+ Name "Individual"
+ Position [875, 243, 905, 257]
+ Port "2"
+ IconDisplay "Port number"
+ BusOutputAsStruct off
}
Block {
- BlockType RateLimiter
- Name "max acc"
- Position [190, 20, 220, 50]
- RisingSlewLimit "3"
- FallingSlewLimit "-3"
- SampleTimeMode "inherited"
+ BlockType Outport
+ Name "Simultaneous"
+ Position [965, 388, 995, 402]
+ Port "3"
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "Demux1"
+ SrcPort 1
+ DstBlock "Sum4"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Demux1"
+ SrcPort 2
+ DstBlock "Sum3"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Demux1"
+ SrcPort 3
+ DstBlock "Sum2"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Demux"
+ SrcPort 1
+ Points [0, 289]
+ DstBlock "Beacon memory3"
+ DstPort trigger
+ }
+ Line {
+ SrcBlock "Demux"
+ SrcPort 2
+ Points [0, 219]
+ DstBlock "Beacon memory2"
+ DstPort trigger
+ }
+ Line {
+ SrcBlock "Demux"
+ SrcPort 3
+ Points [0, 149]
+ DstBlock "Beacon memory1"
+ DstPort trigger
+ }
+ Line {
+ SrcBlock "Demux"
+ SrcPort 4
+ Points [0, 79]
+ DstBlock "Beacon memory"
+ DstPort trigger
+ }
+ Line {
+ SrcBlock "Beacon memory3"
+ SrcPort 1
+ Points [55, 0; 0, -100]
+ DstBlock "Mux"
+ DstPort 4
+ }
+ Line {
+ SrcBlock "Demux1"
+ SrcPort 4
+ Points [0, 10]
+ Branch {
+ DstBlock "Sum1"
+ DstPort 1
+ }
+ Branch {
+ Points [20, 0]
+ DstBlock "Sum"
+ DstPort 1
+ }
+ }
+ Line {
+ SrcBlock "Sum4"
+ SrcPort 1
+ DstBlock "Beacon memory3"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Selector"
+ SrcPort 1
+ Points [140, 0]
+ DstBlock "Individual output"
+ DstPort trigger
}
- Block {
- BlockType Saturate
- Name "max m/s"
- Position [320, 30, 350, 60]
- UpperLimit "2"
- LowerLimit "-2"
+ Line {
+ SrcBlock "Beacon memory1"
+ SrcPort 1
+ DstBlock "Mux"
+ DstPort 2
}
- Block {
- BlockType Outport
- Name "Out1"
- Position [725, 48, 755, 62]
- IconDisplay "Port number"
- BusOutputAsStruct off
+ Line {
+ SrcBlock "Integrator"
+ SrcPort state
+ Points [0, -53]
+ DstBlock "Compare\nTo Constant"
+ DstPort 1
}
Line {
- SrcBlock "kinematics"
+ SrcBlock "Constant1"
SrcPort 1
DstBlock "Integrator"
DstPort 1
}
Line {
- Name "x,y,phi [m,m,rad]"
+ SrcBlock "Compare\nTo Constant"
+ SrcPort 1
+ Points [-15, 0; 0, 110]
+ DstBlock "Integrator"
+ DstPort 2
+ }
+ Line {
SrcBlock "Integrator"
SrcPort 1
Points [20, 0]
Branch {
- Points [0, 85]
+ Labels [-1, 1]
+ Points [0, -165]
+ DstBlock "Subtract"
+ DstPort 2
+ }
+ Branch {
+ Labels [1, 0]
+ Points [0, -25]
+ Branch {
+ Points [0, 65]
+ Branch {
+ Points [0, 70]
+ Branch {
+ Points [0, 70]
+ Branch {
+ Points [0, 70]
+ DstBlock "Sum4"
+ DstPort 2
+ }
+ Branch {
+ DstBlock "Sum3"
+ DstPort 2
+ }
+ }
+ Branch {
+ DstBlock "Sum2"
+ DstPort 2
+ }
+ }
+ Branch {
+ DstBlock "Sum1"
+ DstPort 2
+ }
+ }
+ Branch {
+ DstBlock "Sum"
+ DstPort 2
+ }
+ }
+ }
+ Line {
+ SrcBlock "Subtract"
+ SrcPort 1
+ DstBlock "Hit \nCrossing"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Hit \nCrossing"
+ SrcPort 1
+ Points [20, 0]
+ Branch {
DstBlock "Selector"
DstPort 1
}
Branch {
- Labels [1, 0; 1, 1]
- DstBlock "Out1"
+ DstBlock "Demux"
DstPort 1
}
}
Line {
- SrcBlock "Mux1"
+ SrcBlock "Individual output"
SrcPort 1
- DstBlock "kinematics"
+ DstBlock "Individual"
DstPort 1
}
Line {
- Name "angle"
- Labels [0, 0]
- SrcBlock "Selector"
+ SrcBlock "Beacon Angles"
SrcPort 1
- Points [-60, 0; 0, -75]
- DstBlock "Mux1"
- DstPort 2
+ Points [70, 0]
+ Branch {
+ DstBlock "Subtract"
+ DstPort 1
+ }
+ Branch {
+ DstBlock "Sum5"
+ DstPort 1
+ }
+ }
+ Line {
+ SrcBlock "Beacon memory"
+ SrcPort 1
+ Points [40, 0; 0, 50]
+ DstBlock "Mux"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Beacon memory2"
+ SrcPort 1
+ Points [40, 0; 0, -50]
+ DstBlock "Mux"
+ DstPort 3
+ }
+ Line {
+ SrcBlock "Pos"
+ SrcPort 1
+ DstBlock "Beacon Angles"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Mux"
+ SrcPort 1
+ DstBlock "Rate Transition"
+ DstPort 1
}
Line {
- Name "rad/s"
Labels [0, 0]
- SrcBlock "In1"
+ SrcBlock "Rate Transition1"
SrcPort 1
- DstBlock "Wheel\nradius"
+ DstBlock "Ideal"
DstPort 1
}
Line {
- SrcBlock "max m/s"
+ SrcBlock "Clock"
SrcPort 1
- DstBlock "Mux1"
+ DstBlock "Compare\nTo Zero"
DstPort 1
}
Line {
- SrcBlock "Manual Switch"
+ SrcBlock "Rate Transition"
SrcPort 1
- DstBlock "max m/s"
+ DstBlock "Do not produce\ndata at start. We\nhave to "
+"wait one turn."
DstPort 1
}
Line {
- SrcBlock "max acc"
+ SrcBlock "Do not produce\ndata at start. We\nhave to "
+"wait one turn."
SrcPort 1
- DstBlock "Manual Switch"
+ DstBlock "Simultaneous"
DstPort 1
}
Line {
- Name "m/s"
- Labels [0, 0]
- SrcBlock "Wheel\nradius"
+ SrcBlock "Compare\nTo Zero"
SrcPort 1
- Points [20, 0]
+ Points [40, 0]
+ DstBlock "Do not produce\ndata at start. We\nhave to "
+"wait one turn."
+ DstPort enable
+ }
+ Line {
+ SrcBlock "Sum"
+ SrcPort 1
+ DstBlock "Individual output"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Measurement noise"
+ SrcPort 1
+ Points [-20, 0]
Branch {
- Points [0, -10]
- DstBlock "max acc"
+ DstBlock "Demux1"
DstPort 1
}
Branch {
- Points [0, 25; 75, 0]
- DstBlock "Manual Switch"
+ Points [-40, 0]
+ DstBlock "Sum5"
DstPort 2
}
}
+ Line {
+ SrcBlock "Sum3"
+ SrcPort 1
+ DstBlock "Beacon memory2"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Sum2"
+ SrcPort 1
+ DstBlock "Beacon memory1"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Sum1"
+ SrcPort 1
+ DstBlock "Beacon memory"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Sum5"
+ SrcPort 1
+ DstBlock "Rate Transition1"
+ DstPort 1
+ }
}
}
Block {
- BlockType Sum
- Name "Sum"
- Ports [2, 1]
- Position [305, 405, 325, 425]
- ShowName off
- IconShape "round"
- Inputs "|++"
- CollapseMode "All dimensions"
- InputSameDT off
- OutDataTypeMode "Inherit via internal rule"
- SaturateOnIntegerOverflow off
+ BlockType "S-Function"
+ Name "S-Function2"
+ Ports [0, 1]
+ Position [35, 124, 95, 156]
+ BackgroundColor "green"
+ NamePlacement "alternate"
+ FunctionName "sf_trgen"
+ Parameters "traj, initpos, constr, start"
+ MaskPromptString "Trajectory|Initial position|Trajectory constrai"
+"nts|Start time"
+ MaskStyleString "edit,edit,edit,edit"
+ MaskTunableValueString "off,off,off,off"
+ MaskCallbackString "|||"
+ MaskEnableString "on,on,on,on"
+ MaskVisibilityString "on,on,on,on"
+ MaskToolTipString "on,on,on,on"
+ MaskVarAliasString ",,,"
+ MaskVariables "traj=@1;initpos=@2;constr=@3;start=@4;"
+ MaskDisplay "disp('Trajectory\\ngenerator')"
+ MaskIconFrame on
+ MaskIconOpaque on
+ MaskIconRotate "none"
+ MaskIconUnits "autoscale"
+ MaskValueString "path|desc.initpos|desc.wayconstraints|3"
+ MaskTabNameString ",,,"
}
Block {
BlockType Reference
Name "Switch"
Ports [2, 1]
- Position [365, 387, 395, 423]
+ Position [567, 270, 603, 300]
+ Orientation "down"
+ NamePlacement "alternate"
SourceBlock "simulink/Signal\nRouting/Manual Switch"
SourceType "Manual Switch"
ShowPortLabels "FromPortIcon"
BlockType Reference
Name "Switch1"
Ports [2, 1]
- Position [345, 287, 375, 323]
+ Position [412, 270, 448, 300]
+ Orientation "down"
SourceBlock "simulink/Signal\nRouting/Manual Switch"
SourceType "Manual Switch"
ShowPortLabels "FromPortIcon"
RTWMemSecDataConstants "Inherit from model"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
- sw "0"
+ sw "1"
action "0"
varsize off
}
Block {
- BlockType ToWorkspace
- Name "To Workspace"
- Position [310, 45, 370, 75]
- VariableName "trajectory"
- MaxDataPoints "inf"
- SampleTime "-1"
- SaveFormat "Structure With Time"
+ BlockType TransportDelay
+ Name "Transport\nDelay"
+ Position [580, 190, 610, 220]
+ Orientation "down"
+ NamePlacement "alternate"
+ DelayTime "0"
}
Block {
BlockType "M-S-Function"
Name "draw probability bitmap"
Ports [1]
- Position [725, 393, 815, 427]
+ Position [725, 413, 815, 447]
ForegroundColor "gray"
BackgroundColor "black"
NamePlacement "alternate"
FunctionName "sfdrawbitmap"
}
+ Block {
+ BlockType SignalViewerScope
+ Name "Scope"
+ Ports []
+ Position [20, 15, 60, 55]
+ IOType "viewer"
+ List {
+ ListType IOSignalStrings
+ set0ParseKeys "9"
+ set0Sigs "Switch:o1"
+ }
+ Location [6, 641, 330, 998]
+ Open off
+ NumInputPorts "1"
+ List {
+ ListType AxesTitles
+ axes1 "%<SignalLabel>"
+ }
+ ShowDataMarkers off
+ ShowLegends off
+ TimeRange "1.5"
+ YMin "0"
+ YMax "7"
+ DataFormat "Array"
+ MaxDataPoints "7500"
+ RefreshTime 0.035000
+ Disabled off
+ }
+ Block {
+ BlockType SignalViewerScope
+ Name "Scope1"
+ Ports []
+ Position [20, 15, 60, 55]
+ IOType "viewer"
+ Location [6, 760, 772, 999]
+ Open off
+ NumInputPorts "1"
+ ZoomMode "yonly"
+ List {
+ ListType AxesTitles
+ axes1 "%<SignalLabel>"
+ }
+ ShowDataMarkers off
+ ShowLegends off
+ TimeRange "1.8"
+ YMin "-4"
+ YMax "4"
+ SaveName "ScopeData1"
+ DataFormat "Array"
+ MaxDataPoints "7500"
+ RefreshTime 0.035000
+ Disabled off
+ }
+ Block {
+ BlockType SignalViewerScope
+ Name "Scope2"
+ Ports []
+ Position [20, 15, 60, 55]
+ IOType "viewer"
+ List {
+ ListType IOSignalStrings
+ set0ParseKeys "13"
+ set0Sigs "Controller:o1"
+ }
+ Location [501, 172, 825, 411]
+ Open off
+ NumInputPorts "1"
+ ZoomMode "xonly"
+ List {
+ ListType AxesTitles
+ axes1 "%<SignalLabel>"
+ }
+ ShowDataMarkers off
+ ShowLegends on
+ LegendLocations "186.25 162.125 118.5 45.62"
+"5"
+ TimeRange "10"
+ YMax "10"
+ SaveName "ScopeData2"
+ DataFormat "Array"
+ MaxDataPoints "7500"
+ RefreshTime 0.035000
+ Disabled off
+ }
+ Block {
+ BlockType SignalViewerScope
+ Name "Scope3"
+ Ports []
+ Position [20, 15, 60, 55]
+ IOType "viewer"
+ Location [345, 294, 669, 533]
+ Open off
+ NumInputPorts "1"
+ ZoomMode "xonly"
+ List {
+ ListType AxesTitles
+ axes1 "%<SignalLabel>"
+ }
+ ShowDataMarkers off
+ ShowLegends off
+ TimeRange "10"
+ YMin "-1"
+ YMax "4"
+ SaveName "ScopeData3"
+ DataFormat "Array"
+ MaxDataPoints "7500"
+ RefreshTime 0.035000
+ Disabled off
+ }
Line {
SrcBlock "Controller"
SrcPort 1
- DstBlock "Gain"
+ DstBlock "Robot kinematics"
DstPort 1
}
Line {
Name "Real robot position"
- SrcBlock "Subsystem"
+ SrcBlock "Robot kinematics"
SrcPort 1
- Points [20, 0]
+ Points [10, 0]
Branch {
- Points [210, 0]
+ Labels [2, 0]
+ Points [0, 135]
+ DstBlock "Rate Transition2"
+ DstPort 1
+ }
+ Branch {
+ Labels [2, 0]
+ Points [0, -45; 300, 0]
Branch {
- Labels [-1, 1]
- DstBlock "Level-2 M-file\nS-Function"
+ Points [0, 90]
+ DstBlock "Graphs"
DstPort 1
}
Branch {
- Points [0, 90]
- DstBlock "Graphs"
+ DstBlock "Real\nposition"
DstPort 1
}
}
Branch {
- Labels [2, 0]
- Points [0, 70; -360, 0]
- Branch {
- Points [0, -40]
- DstBlock "Manual Switch"
- DstPort 2
- }
- Branch {
- Points [0, 100]
- Branch {
- Points [0, 110]
- DstBlock "Rate Transition"
- DstPort 1
- }
- Branch {
- Points [95, 0]
- DstBlock "Pos to deltas"
- DstPort 1
- }
- }
+ DstBlock "Rotating laser"
+ DstPort 1
}
}
Line {
Line {
SrcBlock "Rate Transition1"
SrcPort 1
- Points [-50, 0; 0, -370]
+ Points [-20, 0; 0, -155]
DstBlock "Manual Switch"
DstPort 1
}
Line {
- SrcBlock "Gain"
- SrcPort 1
- DstBlock "Subsystem"
+ SrcBlock "MCL"
+ SrcPort 2
+ Points [-75, 0; 0, 65]
+ DstBlock "draw probability bitmap"
DstPort 1
}
Line {
- SrcBlock "MCL"
+ SrcBlock "S-Function2"
SrcPort 1
- Points [50, 0]
+ Points [45, 0]
Branch {
- Points [0, -50]
- DstBlock "Graphs"
- DstPort 2
+ DstBlock "Controller"
+ DstPort 1
}
Branch {
- Points [0, 0]
- Branch {
- DstBlock "Level-2 M-file\nS-Function1"
- DstPort 1
- }
- Branch {
- Points [0, 210]
- DstBlock "Rate Transition1"
- DstPort 1
- }
+ Points [0, -115]
+ DstBlock "Reference\nposition"
+ DstPort 1
}
}
Line {
- SrcBlock "Measurement"
+ SrcBlock "Switch1"
SrcPort 1
- DstBlock "Sum"
+ Points [0, 25]
+ DstBlock "MCL"
DstPort 1
}
Line {
- SrcBlock "Sum"
+ SrcBlock "No odometry"
SrcPort 1
- DstBlock "Switch"
+ Points [-25, 0]
+ DstBlock "Switch1"
DstPort 2
}
Line {
- SrcBlock "Random\nNumber"
- SrcPort 1
- Points [0, 5]
- DstBlock "Measurement noise\nstd. deviation [deg]"
- DstPort 1
- }
- Line {
- SrcBlock "Measurement noise\nstd. deviation [deg]"
+ SrcBlock "Rate Transition2"
SrcPort 1
- DstBlock "Sum"
+ Points [0, -30]
+ DstBlock "Manual Switch"
DstPort 2
}
Line {
- SrcBlock "No laser"
- SrcPort 1
- Points [65, 0]
- DstBlock "Switch"
+ SrcBlock "Robot kinematics"
+ SrcPort 2
+ Points [30, 0]
+ DstBlock "Switch1"
DstPort 1
}
Line {
+ Name "Estimated position"
Labels [0, 0]
- SrcBlock "Rate Transition"
- SrcPort 1
- DstBlock "Measurement"
- DstPort 1
- }
- Line {
SrcBlock "MCL"
- SrcPort 2
- Points [15, 0; 0, 70]
- DstBlock "draw probability bitmap"
- DstPort 1
- }
- Line {
- SrcBlock "Switch"
- SrcPort 1
- Points [5, 0; 0, -65]
- Branch {
- DstBlock "MCL"
- DstPort 2
- }
- Branch {
- Points [0, -100; 205, 0; 0, -80]
- DstBlock "Level-2 M-file\nS-Function"
- DstPort 2
- }
- }
- Line {
- SrcBlock "S-Function2"
SrcPort 1
- Points [25, 0]
+ Points [-90, 0]
Branch {
- DstBlock "Controller"
+ DstBlock "Rate Transition1"
DstPort 1
}
Branch {
- Points [0, -70; 130, 0]
+ Labels [3, 0]
+ Points [0, 70; 580, 0; 0, -80]
Branch {
- DstBlock "Level-2 M-file\nS-Function2"
- DstPort 1
+ Points [0, -95]
+ DstBlock "Graphs"
+ DstPort 2
}
Branch {
- Points [0, 20]
- DstBlock "To Workspace"
+ DstBlock "Estimated\nposition"
DstPort 1
}
}
}
Line {
- SrcBlock "Switch1"
+ SrcBlock "No laser"
SrcPort 1
- DstBlock "MCL"
+ Points [20, 0]
+ DstBlock "Switch"
DstPort 1
}
Line {
- SrcBlock "Pos to deltas"
+ SrcBlock "Switch"
SrcPort 1
- DstBlock "Switch1"
- DstPort 2
+ Points [0, 15]
+ Branch {
+ Points [0, 45]
+ DstBlock "MCL"
+ DstPort 2
+ }
+ Branch {
+ Points [65, 0; 0, -190]
+ DstBlock "Real\nposition"
+ DstPort 2
+ }
}
Line {
- SrcBlock "No odometry"
- SrcPort 1
- Points [15, 0; 0, 15]
- DstBlock "Switch1"
+ SrcBlock "Rotating laser"
+ SrcPort 2
+ Points [20, 0]
+ DstBlock "Transport\nDelay"
DstPort 1
}
+ Line {
+ SrcBlock "Transport\nDelay"
+ SrcPort 1
+ DstBlock "Switch"
+ DstPort 2
+ }
}
}