NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
- ComputedModelVersion "1.294"
+ ComputedModelVersion "1.310"
NumModelReferences 0
NumTestPointedSignals 0
}
ModifiedByFormat "%<Auto>"
LastModifiedBy "wsh"
ModifiedDateFormat "%<Auto>"
- LastModifiedDate "Sat Apr 26 11:55:07 2008"
- ModelVersionFormat "1.%<AutoIncrement:294>"
+ LastModifiedDate "Sun Apr 27 12:09:33 2008"
+ ModelVersionFormat "1.%<AutoIncrement:310>"
ConfigurationManager "None"
SimulationMode "normal"
LinearizationMsg "none"
ZeroCross on
SampleTime "-1"
}
- Block {
- BlockType ToWorkspace
- VariableName "simulink_output"
- MaxDataPoints "1000"
- Decimation "1"
- SampleTime "0"
- FixptAsFi off
- }
Block {
BlockType TriggerPort
TriggerType "rising"
}
System {
Name "reg_mcl"
- Location [4, 431, 859, 1000]
+ Location [132, 385, 987, 862]
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_"
"mcl',0) "
MinAlgLoopOccurrences 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
}
}
}
- 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"
MaskIconUnits "autoscale"
MaskValueString "3000|0.01|2/180*pi|5/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"
Block {
BlockType Ground
Name "No laser"
- Position [295, 355, 315, 375]
+ Position [530, 220, 550, 240]
}
Block {
BlockType Ground
Name "No odometry"
- Position [265, 255, 285, 275]
+ 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"
+ X0 "desc.initpos"
+ }
+ Block {
+ 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 "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]
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]
- Open off
+ Name "Robot kinematics"
+ Location [4, 55, 1176, 412]
+ Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
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 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 [245, 260, 700, 398]
+ Open on
+ 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, 45, 55, 59]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Reference
+ Name "Manual Switch"
+ Ports [2, 1]
+ Position [345, 32, 375, 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 Gain
+ Name "Wheel\nradius"
+ Position [80, 32, 125, 68]
+ Gain "robot.wheelr"
+ ParameterDataTypeMode "Inherit via internal rule"
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ Port {
+ PortNumber 1
+ Name "m/s"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ }
+ 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 [400, 45, 430, 59]
+ IconDisplay "Port number"
+ }
+ Line {
+ SrcBlock "acc limit"
+ SrcPort 1
+ DstBlock "Manual Switch"
+ DstPort 1
+ }
+ Line {
+ Name "m/s"
+ Labels [0, 0]
+ SrcBlock "Wheel\nradius"
+ SrcPort 1
+ DstBlock "speed limit"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "speed limit"
+ SrcPort 1
+ Points [20, 0]
+ Branch {
+ Points [0, -10]
+ DstBlock "acc limit"
+ DstPort 1
+ }
+ Branch {
+ Points [0, 60; 100, 0]
+ DstBlock "Manual Switch"
+ DstPort 2
+ }
+ }
+ Line {
+ SrcBlock "Manual Switch"
+ SrcPort 1
+ DstBlock "(vl,vr)"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "ref omega"
+ SrcPort 1
+ DstBlock "Wheel\nradius"
+ 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 "Speed XY"
+ Name "Selector"
Ports [1, 1]
- Position [190, 91, 230, 129]
+ Position [546, 85, 584, 125]
+ Orientation "down"
+ NamePlacement "alternate"
IndexOptions "Index vector (dialog)"
- Indices "[1 2]"
+ Indices "[3]"
OutputSizes "1"
InputPortWidth "3"
Port {
PortNumber 1
- Name "dx, dy"
+ Name "angle"
RTWStorageClass "Auto"
DataLoggingNameMode "SignalName"
}
}
Block {
BlockType Selector
- Name "Speed angle"
+ Name "Speed XY"
Ports [1, 1]
- Position [190, 151, 230, 189]
+ Position [760, 221, 800, 259]
IndexOptions "Index vector (dialog)"
- Indices "3"
+ Indices "[1 2]"
+ OutputSizes "1"
+ InputPortWidth "3"
+ Port {
+ PortNumber 1
+ Name "dx, dy"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ }
+ Block {
+ BlockType Selector
+ Name "Speed angle"
+ Ports [1, 1]
+ Position [760, 281, 800, 319]
+ IndexOptions "Index vector (dialog)"
+ Indices "3"
OutputSizes "1"
InputPortWidth "3"
Port {
}
}
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 "(wl,wr)"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
}
Block {
BlockType Outport
- Name "Odometry"
- Position [500, 103, 530, 117]
+ Name "Pos"
+ Position [780, 48, 810, 62]
IconDisplay "Port number"
BusOutputAsStruct off
}
- Line {
- SrcBlock "Gain"
- SrcPort 1
- Points [0, 50]
- DstBlock "Mux1"
- DstPort 1
- }
- Line {
- SrcBlock "Angle"
- SrcPort 1
- DstBlock "Gain"
- DstPort 1
+ Block {
+ BlockType Outport
+ Name "Odometry"
+ Position [1090, 233, 1120, 247]
+ Port "2"
+ IconDisplay "Port number"
+ BusOutputAsStruct off
}
Line {
- SrcBlock "Unit Delay"
+ SrcBlock "kinematics"
SrcPort 1
- DstBlock "Angle"
+ DstBlock "Integrator"
DstPort 1
}
Line {
- SrcBlock "Difference"
+ Name "x,y,phi [m,m,rad]"
+ SrcBlock "Integrator"
SrcPort 1
- Points [5, 0]
+ Points [20, 0]
Branch {
- Points [0, 60]
- DstBlock "Speed angle"
+ DstBlock "Selector"
DstPort 1
}
Branch {
- DstBlock "Speed XY"
- DstPort 1
+ Points [45, 0]
+ Branch {
+ Labels [1, 1; 1, 0]
+ DstBlock "Pos"
+ DstPort 1
+ }
+ Branch {
+ DstBlock "Rate Transition2"
+ DstPort 1
+ }
}
}
Line {
- Name "dx, dy"
- Labels [0, 0]
- SrcBlock "Speed XY"
+ SrcBlock "Mux1"
SrcPort 1
- DstBlock "Mux1"
- DstPort 2
+ DstBlock "kinematics"
+ DstPort 1
}
Line {
- Name "dangle"
+ Name "angle"
Labels [0, 0]
- SrcBlock "Speed angle"
+ SrcBlock "Selector"
SrcPort 1
- Points [210, 0]
- DstBlock "Mux2"
+ Points [0, 15; -230, 0; 0, -80]
+ DstBlock "Mux1"
DstPort 2
}
Line {
- SrcBlock "Mux2"
+ Name "(vl,vr)"
+ Labels [0, 0]
+ SrcBlock "Motors"
SrcPort 1
- DstBlock "Odometry"
+ DstBlock "Mux1"
DstPort 1
}
Line {
- SrcBlock "Rotate XY"
+ Name "(wl,wr)"
+ Labels [0, 0]
+ SrcBlock "speed2wheel"
SrcPort 1
- DstBlock "Mux2"
+ DstBlock "Motors"
DstPort 1
}
Line {
- SrcBlock "Mux1"
+ Name "(v,w)"
+ Labels [0, 0]
+ SrcBlock "In1"
SrcPort 1
- DstBlock "Rotate XY"
+ DstBlock "speed2wheel"
DstPort 1
}
Line {
- SrcBlock "Pos"
- SrcPort 1
- Points [10, 0]
- Branch {
- Points [0, -70]
- DstBlock "Unit Delay"
- DstPort 1
- }
- Branch {
- DstBlock "Difference"
- DstPort 1
- }
- }
- }
- }
- Block {
- BlockType RateTransition
- Name "Rate Transition1"
- Position [520, 469, 560, 511]
- Orientation "left"
- X0 "desc.initpos"
- }
- Block {
- BlockType SubSystem
- Name "Robot kinematics"
- Ports [1, 1]
- Position [310, 111, 435, 159]
- BackgroundColor "yellow"
- 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)')"
- 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 "Robot kinematics"
- Location [121, 481, 955, 665]
- 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 [15, 38, 45, 52]
- IconDisplay "Port number"
- Port {
- PortNumber 1
- Name "rad/s"
- RTWStorageClass "Auto"
- DataLoggingNameMode "SignalName"
- }
- }
- Block {
- BlockType Integrator
- Name "Integrator"
- Ports [1, 1]
- Position [530, 40, 560, 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 Reference
- Name "Manual Switch"
- Ports [2, 1]
- Position [250, 27, 280, 63]
- 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 Mux
- Name "Mux1"
- Ports [2, 1]
- Position [380, 36, 385, 74]
- ShowName off
- Inputs "2"
- DisplayOption "bar"
- }
- Block {
- BlockType Selector
- Name "Selector"
- Ports [1, 1]
- Position [420, 121, 460, 159]
- Orientation "left"
- IndexOptions "Index vector (dialog)"
- Indices "[3]"
- OutputSizes "1"
- InputPortWidth "3"
- Port {
- PortNumber 1
- Name "angle"
- RTWStorageClass "Auto"
- DataLoggingNameMode "SignalName"
- }
- }
- Block {
- BlockType Gain
- Name "Wheel\nradius"
- Position [90, 27, 135, 63]
- Gain "robot.wheelr"
- ParameterDataTypeMode "Inherit via internal rule"
- OutDataTypeMode "Inherit via internal rule"
- 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"
- }
- Block {
- BlockType RateLimiter
- Name "max acc"
- Position [190, 20, 220, 50]
- RisingSlewLimit "3"
- FallingSlewLimit "-3"
- SampleTimeMode "inherited"
- }
- Block {
- BlockType Saturate
- Name "max m/s"
- Position [320, 30, 350, 60]
- UpperLimit "2"
- LowerLimit "-2"
- }
- Block {
- BlockType Outport
- Name "Out1"
- Position [725, 48, 755, 62]
- IconDisplay "Port number"
- BusOutputAsStruct off
- }
- Line {
- SrcBlock "kinematics"
+ SrcBlock "Gain"
SrcPort 1
- DstBlock "Integrator"
+ Points [0, 50]
+ DstBlock "Mux2"
DstPort 1
}
Line {
- Name "x,y,phi [m,m,rad]"
- SrcBlock "Integrator"
+ SrcBlock "Difference"
SrcPort 1
- Points [20, 0]
+ Points [5, 0]
Branch {
- Points [0, 85]
- DstBlock "Selector"
+ DstBlock "Speed XY"
DstPort 1
}
Branch {
- Labels [1, 0; 1, 1]
- DstBlock "Out1"
+ Points [0, 60]
+ DstBlock "Speed angle"
DstPort 1
}
}
Line {
- SrcBlock "Mux1"
+ Name "dx, dy"
+ Labels [0, 0]
+ SrcBlock "Speed XY"
SrcPort 1
- DstBlock "kinematics"
- DstPort 1
+ DstBlock "Mux2"
+ DstPort 2
}
Line {
- Name "angle"
+ Name "dangle"
Labels [0, 0]
- SrcBlock "Selector"
+ SrcBlock "Speed angle"
SrcPort 1
- Points [-60, 0; 0, -75]
- DstBlock "Mux1"
+ Points [210, 0]
+ DstBlock "Mux3"
DstPort 2
}
Line {
- Name "rad/s"
- Labels [0, 0]
- SrcBlock "In1"
+ SrcBlock "Mux3"
SrcPort 1
- DstBlock "Wheel\nradius"
+ DstBlock "Odometry"
DstPort 1
}
Line {
- SrcBlock "max m/s"
+ SrcBlock "Rotate XY"
SrcPort 1
- DstBlock "Mux1"
+ DstBlock "Mux3"
DstPort 1
}
Line {
- SrcBlock "Manual Switch"
+ SrcBlock "Mux2"
SrcPort 1
- DstBlock "max m/s"
+ DstBlock "Rotate XY"
DstPort 1
}
Line {
- SrcBlock "max acc"
+ SrcBlock "Angle"
SrcPort 1
- DstBlock "Manual Switch"
+ DstBlock "Gain"
DstPort 1
}
Line {
- Name "m/s"
- Labels [0, 0]
- SrcBlock "Wheel\nradius"
+ SrcBlock "Rate Transition2"
SrcPort 1
- Points [20, 0]
+ Points [0, 40]
Branch {
- Points [0, -10]
- DstBlock "max acc"
+ Points [0, 70]
+ DstBlock "Difference"
DstPort 1
}
Branch {
- Points [0, 25; 75, 0]
- DstBlock "Manual Switch"
- DstPort 2
+ DstBlock "Angle"
+ DstPort 1
}
}
}
BlockType SubSystem
Name "Rotating laser"
Ports [1, 3]
- Position [120, 394, 275, 456]
+ Position [430, 119, 575, 181]
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
FunctionWithSeparateData off
MaskIconOpaque on
MaskIconRotate "none"
MaskIconUnits "normalized"
- MaskValueString "3|1|on"
+ MaskValueString "0.1|0|on"
MaskTabNameString ",,"
System {
Name "Rotating laser"
BusOutputAsStruct off
}
Line {
- SrcBlock "Sum5"
- SrcPort 1
- DstBlock "Rate Transition1"
+ SrcBlock "Integrator"
+ SrcPort state
+ Points [0, -53]
+ DstBlock "Compare\nTo Constant"
DstPort 1
}
Line {
- SrcBlock "Demux1"
- SrcPort 3
- Points [0, 15]
- Branch {
- DstBlock "Sum3"
- DstPort 1
- }
- Branch {
- Points [35, 0]
- DstBlock "Sum"
- DstPort 1
- }
+ SrcBlock "Constant1"
+ SrcPort 1
+ DstBlock "Integrator"
+ DstPort 1
}
Line {
- SrcBlock "Demux1"
- SrcPort 2
- DstBlock "Sum2"
- DstPort 1
+ SrcBlock "Compare\nTo Constant"
+ SrcPort 1
+ Points [-30, 0; 0, 110]
+ DstBlock "Integrator"
+ DstPort 2
}
Line {
- SrcBlock "Demux1"
+ SrcBlock "Integrator"
SrcPort 1
- DstBlock "Sum1"
- DstPort 1
+ Points [20, 0]
+ Branch {
+ 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]
+ DstBlock "Sum3"
+ DstPort 2
+ }
+ Branch {
+ DstBlock "Sum2"
+ DstPort 2
+ }
+ }
+ Branch {
+ DstBlock "Sum1"
+ DstPort 2
+ }
+ }
+ Branch {
+ DstBlock "Sum"
+ DstPort 2
+ }
+ }
}
Line {
- SrcBlock "Sum1"
+ SrcBlock "Subtract"
SrcPort 1
- DstBlock "Beacon memory"
+ DstBlock "Hit \nCrossing"
DstPort 1
}
Line {
- SrcBlock "Sum2"
+ SrcBlock "Hit \nCrossing"
SrcPort 1
- DstBlock "Beacon memory1"
- DstPort 1
+ Points [15, 0]
+ Branch {
+ DstBlock "Demux"
+ DstPort 1
+ }
+ Branch {
+ Points [400, 0]
+ DstBlock "Individual output"
+ DstPort trigger
+ }
}
Line {
- SrcBlock "Sum3"
+ SrcBlock "Individual output"
SrcPort 1
- DstBlock "Beacon memory2"
+ DstBlock "Individual"
DstPort 1
}
Line {
- SrcBlock "Measurement noise"
+ SrcBlock "Beacon Angles"
SrcPort 1
- Points [-30, 0]
+ Points [70, 0]
Branch {
- Points [-30, 0]
- DstBlock "Sum5"
- DstPort 2
+ DstBlock "Subtract"
+ DstPort 1
}
Branch {
- DstBlock "Demux1"
+ DstBlock "Sum5"
DstPort 1
}
}
Line {
- SrcBlock "Sum"
+ SrcBlock "Beacon memory"
SrcPort 1
- DstBlock "Individual output"
+ Points [40, 0; 0, 50]
+ DstBlock "Mux"
DstPort 1
}
Line {
- SrcBlock "Compare\nTo Zero"
+ SrcBlock "Beacon memory1"
SrcPort 1
- Points [40, 0]
- DstBlock "Do not produce\ndata at start. We\nhave to "
-"wait one turn."
- DstPort enable
+ DstBlock "Mux"
+ DstPort 2
}
Line {
- SrcBlock "Do not produce\ndata at start. We\nhave to "
-"wait one turn."
+ SrcBlock "Beacon memory2"
SrcPort 1
- DstBlock "Simultaneous"
- DstPort 1
+ Points [40, 0; 0, -50]
+ DstBlock "Mux"
+ DstPort 3
}
Line {
- SrcBlock "Rate Transition"
+ SrcBlock "Demux"
SrcPort 1
- DstBlock "Do not produce\ndata at start. We\nhave to "
-"wait one turn."
- DstPort 1
+ Points [0, 219]
+ DstBlock "Beacon memory2"
+ DstPort trigger
}
Line {
- SrcBlock "Clock"
- SrcPort 1
- DstBlock "Compare\nTo Zero"
- DstPort 1
+ SrcBlock "Demux"
+ SrcPort 2
+ Points [0, 149]
+ DstBlock "Beacon memory1"
+ DstPort trigger
}
Line {
- Labels [0, 0]
- SrcBlock "Rate Transition1"
+ SrcBlock "Demux"
+ SrcPort 3
+ Points [0, 81; 150, 0]
+ DstBlock "Beacon memory"
+ DstPort trigger
+ }
+ Line {
+ SrcBlock "Pos"
SrcPort 1
- DstBlock "Ideal"
+ DstBlock "Beacon Angles"
DstPort 1
}
Line {
DstPort 1
}
Line {
- SrcBlock "Pos"
+ Labels [0, 0]
+ SrcBlock "Rate Transition1"
SrcPort 1
- DstBlock "Beacon Angles"
+ DstBlock "Ideal"
DstPort 1
}
Line {
- SrcBlock "Demux"
- SrcPort 3
- Points [0, 81; 150, 0]
- DstBlock "Beacon memory"
- DstPort trigger
- }
- Line {
- SrcBlock "Demux"
- SrcPort 2
- Points [0, 149]
- DstBlock "Beacon memory1"
- DstPort trigger
+ SrcBlock "Clock"
+ SrcPort 1
+ DstBlock "Compare\nTo Zero"
+ DstPort 1
}
Line {
- SrcBlock "Demux"
+ SrcBlock "Rate Transition"
SrcPort 1
- Points [0, 219]
- DstBlock "Beacon memory2"
- DstPort trigger
+ DstBlock "Do not produce\ndata at start. We\nhave to "
+"wait one turn."
+ DstPort 1
}
Line {
- SrcBlock "Beacon memory2"
+ SrcBlock "Do not produce\ndata at start. We\nhave to "
+"wait one turn."
SrcPort 1
- Points [40, 0; 0, -50]
- DstBlock "Mux"
- DstPort 3
+ DstBlock "Simultaneous"
+ DstPort 1
}
Line {
- SrcBlock "Beacon memory1"
+ SrcBlock "Compare\nTo Zero"
SrcPort 1
- DstBlock "Mux"
- DstPort 2
+ Points [40, 0]
+ DstBlock "Do not produce\ndata at start. We\nhave to "
+"wait one turn."
+ DstPort enable
}
Line {
- SrcBlock "Beacon memory"
+ SrcBlock "Sum"
SrcPort 1
- Points [40, 0; 0, 50]
- DstBlock "Mux"
+ DstBlock "Individual output"
DstPort 1
}
Line {
- SrcBlock "Beacon Angles"
+ SrcBlock "Measurement noise"
SrcPort 1
- Points [70, 0]
+ Points [-30, 0]
Branch {
- DstBlock "Sum5"
+ DstBlock "Demux1"
DstPort 1
}
Branch {
- DstBlock "Subtract"
- DstPort 1
+ Points [-30, 0]
+ DstBlock "Sum5"
+ DstPort 2
}
}
Line {
- SrcBlock "Individual output"
+ SrcBlock "Sum3"
SrcPort 1
- DstBlock "Individual"
+ DstBlock "Beacon memory2"
DstPort 1
}
Line {
- SrcBlock "Hit \nCrossing"
+ SrcBlock "Sum2"
SrcPort 1
- Points [15, 0]
- Branch {
- Points [400, 0]
- DstBlock "Individual output"
- DstPort trigger
- }
- Branch {
- DstBlock "Demux"
- DstPort 1
- }
+ DstBlock "Beacon memory1"
+ DstPort 1
}
Line {
- SrcBlock "Subtract"
+ SrcBlock "Sum1"
SrcPort 1
- DstBlock "Hit \nCrossing"
+ DstBlock "Beacon memory"
DstPort 1
}
Line {
- SrcBlock "Integrator"
+ SrcBlock "Demux1"
SrcPort 1
- Points [20, 0]
+ DstBlock "Sum1"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Demux1"
+ SrcPort 2
+ DstBlock "Sum2"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Demux1"
+ SrcPort 3
+ Points [0, 15]
Branch {
- Labels [1, 0]
- Points [0, -25]
- Branch {
- DstBlock "Sum"
- DstPort 2
- }
- Branch {
- Points [0, 65]
- Branch {
- DstBlock "Sum1"
- DstPort 2
- }
- Branch {
- Points [0, 70]
- Branch {
- DstBlock "Sum2"
- DstPort 2
- }
- Branch {
- Points [0, 70]
- DstBlock "Sum3"
- DstPort 2
- }
- }
- }
+ Points [35, 0]
+ DstBlock "Sum"
+ DstPort 1
}
Branch {
- Labels [-1, 1]
- Points [0, -165]
- DstBlock "Subtract"
- DstPort 2
+ DstBlock "Sum3"
+ DstPort 1
}
}
Line {
- SrcBlock "Compare\nTo Constant"
- SrcPort 1
- Points [-30, 0; 0, 110]
- DstBlock "Integrator"
- DstPort 2
- }
- Line {
- SrcBlock "Constant1"
+ SrcBlock "Sum5"
SrcPort 1
- DstBlock "Integrator"
- DstPort 1
- }
- Line {
- SrcBlock "Integrator"
- SrcPort state
- Points [0, -53]
- DstBlock "Compare\nTo Constant"
+ DstBlock "Rate Transition1"
DstPort 1
}
}
BlockType "S-Function"
Name "S-Function2"
Ports [0, 1]
- Position [55, 94, 115, 126]
+ Position [35, 124, 95, 156]
BackgroundColor "green"
+ NamePlacement "alternate"
FunctionName "sf_trgen"
Parameters "traj, initpos, constr, start"
MaskPromptString "Trajectory|Initial position|Trajectory constrai"
BlockType Reference
Name "Switch"
Ports [2, 1]
- Position [350, 377, 380, 413]
+ Position [567, 270, 603, 300]
+ Orientation "down"
+ NamePlacement "alternate"
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
}
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"
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"
- }
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"
set0ParseKeys "9"
set0Sigs "Switch:o1"
}
- Location [764, 98, 1088, 337]
- Open on
+ Location [864, 1368, 1160, 1660]
+ Open off
NumInputPorts "1"
List {
ListType AxesTitles
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"
+ List {
+ ListType AxesTitles
+ axes1 "%<SignalLabel>"
+ }
+ ShowDataMarkers off
+ ShowLegends off
+ YMin "-1.25e+60"
+ YMax "7.5e+59"
+ SaveName "ScopeData2"
+ 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 "Robot kinematics"
SrcPort 1
- Points [20, 0]
+ Points [10, 0]
Branch {
- Points [210, 0]
- Branch {
- Labels [-1, 1]
- DstBlock "Level-2 M-file\nS-Function"
- DstPort 1
- }
- Branch {
- Points [0, 90]
- DstBlock "Graphs"
- DstPort 1
- }
+ Labels [2, 0]
+ Points [0, 135]
+ DstBlock "Rate Transition2"
+ DstPort 1
+ }
+ Branch {
+ DstBlock "Rotating laser"
+ DstPort 1
}
Branch {
Labels [2, 0]
- Points [0, 70; -360, 0]
+ Points [0, -45; 300, 0]
Branch {
- Points [0, -40]
- DstBlock "Manual Switch"
- DstPort 2
+ Points [0, 90]
+ DstBlock "Graphs"
+ DstPort 1
}
Branch {
- Points [0, 110]
- Branch {
- DstBlock "Pos to deltas"
- DstPort 1
- }
- Branch {
- Points [0, 110]
- DstBlock "Rotating laser"
- DstPort 1
- }
+ DstBlock "Real\nposition"
+ DstPort 1
}
}
}
Line {
SrcBlock "Rate Transition1"
SrcPort 1
- Points [-435, 0; 0, -345]
+ Points [-20, 0; 0, -155]
DstBlock "Manual Switch"
DstPort 1
}
Line {
- SrcBlock "Gain"
- SrcPort 1
- DstBlock "Robot kinematics"
+ 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, 185]
- DstBlock "Rate Transition1"
- DstPort 1
- }
+ Points [0, -115]
+ DstBlock "Reference\nposition"
+ DstPort 1
}
}
Line {
- SrcBlock "No laser"
+ SrcBlock "Switch1"
SrcPort 1
- Points [15, 0]
- DstBlock "Switch"
+ Points [0, 25]
+ DstBlock "MCL"
DstPort 1
}
Line {
- SrcBlock "MCL"
- SrcPort 2
- Points [15, 0; 0, 70]
- DstBlock "draw probability bitmap"
- DstPort 1
+ SrcBlock "No odometry"
+ SrcPort 1
+ Points [-25, 0]
+ DstBlock "Switch1"
+ DstPort 2
}
Line {
- SrcBlock "Switch"
+ SrcBlock "Rate Transition2"
SrcPort 1
- Points [20, 0; 0, -55]
- Branch {
- Points [0, -100; 205, 0; 0, -80]
- DstBlock "Level-2 M-file\nS-Function"
- DstPort 2
- }
- Branch {
- DstBlock "MCL"
- DstPort 2
- }
+ Points [0, -30]
+ DstBlock "Manual Switch"
+ DstPort 2
}
Line {
- SrcBlock "S-Function2"
+ SrcBlock "Robot kinematics"
+ SrcPort 2
+ Points [30, 0]
+ DstBlock "Switch1"
+ DstPort 1
+ }
+ Line {
+ Name "Estimated position"
+ Labels [0, 0]
+ SrcBlock "MCL"
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"
- SrcPort 1
- DstBlock "Switch1"
+ SrcBlock "Rotating laser"
+ SrcPort 2
+ Points [15, 0]
+ DstBlock "Switch"
DstPort 2
}
Line {
- SrcBlock "No odometry"
+ SrcBlock "Switch"
SrcPort 1
- Points [35, 0; 0, 30]
- DstBlock "Switch1"
- DstPort 1
- }
- Line {
- SrcBlock "Rotating laser"
- SrcPort 3
- Points [25, 0; 0, -40]
- DstBlock "Switch"
- DstPort 2
+ Points [0, 15]
+ Branch {
+ Points [0, 45]
+ DstBlock "MCL"
+ DstPort 2
+ }
+ Branch {
+ Points [65, 0; 0, -190]
+ DstBlock "Real\nposition"
+ DstPort 2
+ }
}
}
}