]> rtime.felk.cvut.cz Git - eurobot/public.git/commitdiff
projects / eurobot / public.git / commitdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | patch | inline | side by side (parent: 7b3d1de)
Matlab: Fixed bug in 4 beacon laser model
authorMichal Sojka <sojkam1@fel.cvut.cz>
Wed, 30 Apr 2008 05:46:06 +0000 (07:46 +0200)
committerMichal Sojka <sojkam1@fel.cvut.cz>
Wed, 30 Apr 2008 05:46:06 +0000 (07:46 +0200)
src/mcl/matlab/mcl_test.mdl patch | blob | history
src/mcl/matlab/reg_mcl.mdl patch | blob | history
src/mcl/matlab/sf_mcl.c patch | blob | history

diff --git a/src/mcl/matlab/mcl_test.mdl b/src/mcl/matlab/mcl_test.mdl
index 7175b50883173b62286f6843a1b3e258adb1ff5c..21825b8cba6417bd54222ac2b587e77d1cf379fd 100644 (file)
--- a/src/mcl/matlab/mcl_test.mdl
+++ b/src/mcl/matlab/mcl_test.mdl
@@ -6,7 +6,7 @@ Model {
     NumRootInports         0
     NumRootOutports        0
     ParameterArgumentNames  ""
-    ComputedModelVersion    "1.129"
+    ComputedModelVersion    "1.131"
     NumModelReferences     0
     NumTestPointedSignals   0
   }
@@ -46,8 +46,8 @@ Model {
   ModifiedByFormat       "%<Auto>"
   LastModifiedBy         "wsh"
   ModifiedDateFormat     "%<Auto>"
-  LastModifiedDate       "Wed Apr 30 02:08:24 2008"
-  ModelVersionFormat     "1.%<AutoIncrement:129>"
+  LastModifiedDate       "Wed Apr 30 07:39:51 2008"
+  ModelVersionFormat     "1.%<AutoIncrement:131>"
   ConfigurationManager   "None"
   SimulationMode         "normal"
   LinearizationMsg       "none"
@@ -794,7 +794,7 @@ Model {
       System {
        Name                    "Graphs"
        Location                [962, 152, 1482, 471]
-       Open                    on
+       Open                    off
        ModelBrowserVisibility  off
        ModelBrowserWidth       200
        ScreenColor             "white"
@@ -1050,7 +1050,7 @@ Model {
       MaskIconOpaque         on
       MaskIconRotate         "none"
       MaskIconUnits          "autoscale"
-      MaskValueString        "3000|0.01|5/180*pi|1/180*pi"
+      MaskValueString        "3000|0.01|5/180*pi|5/180*pi"
       MaskTabNameString              ",,,"
     }
     Block {
@@ -1341,7 +1341,7 @@ Model {
       System {
        Name                    "Rotating laser"
        Location                [651, 66, 1675, 656]
-       Open                    off
+       Open                    on
        ModelBrowserVisibility  off
        ModelBrowserWidth       200
        ScreenColor             "white"
@@ -1848,7 +1848,7 @@ Model {
          BlockType               Sum
          Name                    "Sum1"
          Ports                   [2, 1]
-         Position                [415, 305, 435, 325]
+         Position                [460, 305, 480, 325]
          ShowName                off
          IconShape               "round"
          Inputs                  "++|"
@@ -1861,7 +1861,7 @@ Model {
          BlockType               Sum
          Name                    "Sum2"
          Ports                   [2, 1]
-         Position                [430, 375, 450, 395]
+         Position                [445, 375, 465, 395]
          ShowName                off
          IconShape               "round"
          Inputs                  "++|"
@@ -1874,7 +1874,7 @@ Model {
          BlockType               Sum
          Name                    "Sum3"
          Ports                   [2, 1]
-         Position                [445, 445, 465, 465]
+         Position                [430, 445, 450, 465]
          ShowName                off
          IconShape               "round"
          Inputs                  "++|"
@@ -1887,7 +1887,7 @@ Model {
          BlockType               Sum
          Name                    "Sum4"
          Ports                   [2, 1]
-         Position                [460, 515, 480, 535]
+         Position                [415, 515, 435, 535]
          ShowName                off
          IconShape               "round"
          Inputs                  "++|"
@@ -1938,24 +1938,6 @@ Model {
          DstBlock                "Rate Transition1"
          DstPort                 1
        }
-       Line {
-         SrcBlock                "Demux1"
-         SrcPort                 3
-         DstBlock                "Sum3"
-         DstPort                 1
-       }
-       Line {
-         SrcBlock                "Demux1"
-         SrcPort                 2
-         DstBlock                "Sum2"
-         DstPort                 1
-       }
-       Line {
-         SrcBlock                "Demux1"
-         SrcPort                 1
-         DstBlock                "Sum1"
-         DstPort                 1
-       }
        Line {
          SrcBlock                "Sum1"
          SrcPort                 1
@@ -2042,27 +2024,6 @@ Model {
          DstBlock                "Beacon Angles"
          DstPort                 1
        }
-       Line {
-         SrcBlock                "Demux"
-         SrcPort                 3
-         Points                  [0, 81; 155, 0]
-         DstBlock                "Beacon memory"
-         DstPort                 trigger
-       }
-       Line {
-         SrcBlock                "Demux"
-         SrcPort                 2
-         Points                  [0, 149]
-         DstBlock                "Beacon memory1"
-         DstPort                 trigger
-       }
-       Line {
-         SrcBlock                "Demux"
-         SrcPort                 1
-         Points                  [0, 219]
-         DstBlock                "Beacon memory2"
-         DstPort                 trigger
-       }
        Line {
          SrcBlock                "Beacon memory2"
          SrcPort                 1
@@ -2210,7 +2171,7 @@ Model {
            DstPort                 1
          }
          Branch {
-           DstBlock                "Sum4"
+           DstBlock                "Sum1"
            DstPort                 1
          }
        }
@@ -2221,6 +2182,52 @@ Model {
          DstBlock                "Mux"
          DstPort                 4
        }
+       Line {
+         SrcBlock                "Demux"
+         SrcPort                 4
+         Points                  [0, 79]
+         DstBlock                "Beacon memory"
+         DstPort                 trigger
+       }
+       Line {
+         SrcBlock                "Demux"
+         SrcPort                 3
+         Points                  [0, 149]
+         DstBlock                "Beacon memory1"
+         DstPort                 trigger
+       }
+       Line {
+         SrcBlock                "Demux"
+         SrcPort                 2
+         Points                  [0, 219]
+         DstBlock                "Beacon memory2"
+         DstPort                 trigger
+       }
+       Line {
+         SrcBlock                "Demux"
+         SrcPort                 1
+         Points                  [0, 289]
+         DstBlock                "Beacon memory3"
+         DstPort                 trigger
+       }
+       Line {
+         SrcBlock                "Demux1"
+         SrcPort                 3
+         DstBlock                "Sum2"
+         DstPort                 1
+       }
+       Line {
+         SrcBlock                "Demux1"
+         SrcPort                 2
+         DstBlock                "Sum3"
+         DstPort                 1
+       }
+       Line {
+         SrcBlock                "Demux1"
+         SrcPort                 1
+         DstBlock                "Sum4"
+         DstPort                 1
+       }
       }
     }
     Block {
@@ -2239,7 +2246,7 @@ Model {
       RTWMemSecDataInternal   "Inherit from model"
       RTWMemSecDataParameters "Inherit from model"
       low                    "0"
-      gain                   "360"
+      gain                   "280.8"
       high                   "360"
     }
     Block {
@@ -2258,7 +2265,7 @@ Model {
       RTWMemSecDataInternal   "Inherit from model"
       RTWMemSecDataParameters "Inherit from model"
       low                    "0"
-      gain                   "0.7156"
+      gain                   "1.1656"
       high                   "3"
     }
     Block {
@@ -2315,7 +2322,7 @@ Model {
       RTWMemSecDataConstants  "Inherit from model"
       RTWMemSecDataInternal   "Inherit from model"
       RTWMemSecDataParameters "Inherit from model"
-      sw                     "1"
+      sw                     "0"
       action                 "0"
       varsize                off
     }
diff --git a/src/mcl/matlab/reg_mcl.mdl b/src/mcl/matlab/reg_mcl.mdl
index 0d33cf260a39c91174f0577670f8bdb2b9b1aaa4..efc30f975c8192ba72b5d7b01e0b1ccdd0d2dae7 100644 (file)
--- a/src/mcl/matlab/reg_mcl.mdl
+++ b/src/mcl/matlab/reg_mcl.mdl
@@ -6,7 +6,7 @@ Model {
     NumRootInports         0
     NumRootOutports        0
     ParameterArgumentNames  ""
-    ComputedModelVersion    "1.327"
+    ComputedModelVersion    "1.329"
     NumModelReferences     0
     NumTestPointedSignals   0
   }
@@ -48,8 +48,8 @@ Model {
   ModifiedByFormat       "%<Auto>"
   LastModifiedBy         "wsh"
   ModifiedDateFormat     "%<Auto>"
-  LastModifiedDate       "Wed Apr 30 02:09:53 2008"
-  ModelVersionFormat     "1.%<AutoIncrement:327>"
+  LastModifiedDate       "Wed Apr 30 07:42:20 2008"
+  ModelVersionFormat     "1.%<AutoIncrement:329>"
   ConfigurationManager   "None"
   SimulationMode         "normal"
   LinearizationMsg       "none"
@@ -791,7 +791,7 @@ Model {
   }
   System {
     Name                   "reg_mcl"
-    Location               [666, 262, 1521, 739]
+    Location               [118, 523, 973, 1000]
     Open                   on
     ModelBrowserVisibility  off
     ModelBrowserWidth      200
@@ -1287,7 +1287,7 @@ Model {
       System {
        Name                    "Robot kinematics"
        Location                [4, 55, 1176, 412]
-       Open                    on
+       Open                    off
        ModelBrowserVisibility  off
        ModelBrowserWidth       200
        ScreenColor             "white"
@@ -1772,7 +1772,7 @@ Model {
       BlockType                      SubSystem
       Name                   "Rotating laser"
       Ports                  [1, 3]
-      Position               [430, 119, 575, 181]
+      Position               [425, 119, 570, 181]
       BackgroundColor        "[0.912000, 0.763112, 0.484696]"
       MinAlgLoopOccurrences   off
       RTWSystemCode          "Auto"
@@ -2308,7 +2308,7 @@ Model {
          BlockType               Sum
          Name                    "Sum1"
          Ports                   [2, 1]
-         Position                [415, 305, 435, 325]
+         Position                [460, 305, 480, 325]
          ShowName                off
          IconShape               "round"
          Inputs                  "++|"
@@ -2321,7 +2321,7 @@ Model {
          BlockType               Sum
          Name                    "Sum2"
          Ports                   [2, 1]
-         Position                [430, 375, 450, 395]
+         Position                [445, 375, 465, 395]
          ShowName                off
          IconShape               "round"
          Inputs                  "++|"
@@ -2334,7 +2334,7 @@ Model {
          BlockType               Sum
          Name                    "Sum3"
          Ports                   [2, 1]
-         Position                [445, 445, 465, 465]
+         Position                [430, 445, 450, 465]
          ShowName                off
          IconShape               "round"
          Inputs                  "++|"
@@ -2347,7 +2347,7 @@ Model {
          BlockType               Sum
          Name                    "Sum4"
          Ports                   [2, 1]
-         Position                [460, 515, 480, 535]
+         Position                [415, 515, 435, 535]
          ShowName                off
          IconShape               "round"
          Inputs                  "++|"
@@ -2392,6 +2392,52 @@ Model {
          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
@@ -2404,7 +2450,7 @@ Model {
          SrcPort                 4
          Points                  [0, 10]
          Branch {
-           DstBlock                "Sum4"
+           DstBlock                "Sum1"
            DstPort                 1
          }
          Branch {
@@ -2550,27 +2596,6 @@ Model {
          DstBlock                "Mux"
          DstPort                 3
        }
-       Line {
-         SrcBlock                "Demux"
-         SrcPort                 1
-         Points                  [0, 219]
-         DstBlock                "Beacon memory2"
-         DstPort                 trigger
-       }
-       Line {
-         SrcBlock                "Demux"
-         SrcPort                 2
-         Points                  [0, 149]
-         DstBlock                "Beacon memory1"
-         DstPort                 trigger
-       }
-       Line {
-         SrcBlock                "Demux"
-         SrcPort                 3
-         Points                  [0, 81; 155, 0]
-         DstBlock                "Beacon memory"
-         DstPort                 trigger
-       }
        Line {
          SrcBlock                "Pos"
          SrcPort                 1
@@ -2657,24 +2682,6 @@ Model {
          DstBlock                "Beacon memory"
          DstPort                 1
        }
-       Line {
-         SrcBlock                "Demux1"
-         SrcPort                 1
-         DstBlock                "Sum1"
-         DstPort                 1
-       }
-       Line {
-         SrcBlock                "Demux1"
-         SrcPort                 2
-         DstBlock                "Sum2"
-         DstPort                 1
-       }
-       Line {
-         SrcBlock                "Demux1"
-         SrcPort                 3
-         DstBlock                "Sum3"
-         DstPort                 1
-       }
        Line {
          SrcBlock                "Sum5"
          SrcPort                 1
@@ -2707,7 +2714,7 @@ Model {
       MaskIconOpaque         on
       MaskIconRotate         "none"
       MaskIconUnits          "autoscale"
-      MaskValueString        "path|desc.initpos|desc.wayconstraints|1"
+      MaskValueString        "path|desc.initpos|desc.wayconstraints|3"
       MaskTabNameString              ",,,"
     }
     Block {
@@ -3027,7 +3034,7 @@ Model {
     Line {
       SrcBlock               "Rotating laser"
       SrcPort                2
-      Points                 [15, 0]
+      Points                 [20, 0]
       DstBlock               "Switch"
       DstPort                2
     }
diff --git a/src/mcl/matlab/sf_mcl.c b/src/mcl/matlab/sf_mcl.c
index ffbb2c2ee63cda948fabe2487118b0d1fea03b38..c8e27cb71904dacc50b52acebb3e7ef77c597e4d 100644 (file)
--- a/src/mcl/matlab/sf_mcl.c
+++ b/src/mcl/matlab/sf_mcl.c
@@ -367,6 +367,8 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     struct mcl_model *mcl = &l->mcl;
     struct mcl_laser_measurement *last_angles = ssGetPWork(S)[1];
     struct mcl_laser_state *parts = l->parts;
+    bool nonzero = false;
+    bool changed = false;
     
     double *pos = ssGetOutputPortRealSignal(S,OUTPUT_EST_POS);
     uint8_t *bitmap = ssGetOutputPortSignal(S,OUTPUT_BITMAP);
@@ -393,8 +395,6 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     if (ssIsSampleHit(S, measureTid, tid)) {
            struct mcl_laser_measurement angles;
            int i;
-           bool nonzero = false;
-           bool changed = false;
 
 /*         ssPrintf("measure \n"); */
            angles.count = iMeasureWidth;
@@ -428,8 +428,10 @@ static void mdlOutputs(SimStruct *S, int_T tid)
     }
 
     /* Update probability bitmap */
-    /* parts = l->resampled; */
-    parts = l->parts;;
+    if (changed && nonzero)
+           parts = l->resampled; /* Draw particles before resampling */
+    else
+           parts = l->parts;;
     for (i=0; i<BITMAP_WIDTH*BITMAP_HEIGHT*3; i++)
            bitmap[i] = 0;
     double max = 0;
@@ -442,11 +444,11 @@ static void mdlOutputs(SimStruct *S, int_T tid)
            y = parts[i].y / l->height * BITMAP_HEIGHT;
            y = BITMAP_HEIGHT - 1 - y;
            if (x>=0 && x<BITMAP_WIDTH && y>=0 && y<BITMAP_HEIGHT) {
-                   #if 0
-                   color = log(1+mcl->weight[i])/log(1+max);
-                   /* color = 0.3+0*mcl->weight[i]/max; */
-                   color = fmax(color, 0.2);
-                   #endif
+                   if (changed && nonzero) {
+                           color = log(1+mcl->weight[i])/log(1+max);
+                           /* color = 0.3+0*mcl->weight[i]/max; */
+                           color = fmax(color, 0.2);
+                   }
                    uint8_t rgb[3];
                    float hsv[3] = {fmod(parts[i].angle, 2*M_PI)/2/M_PI, 1, color};
                    hsv2rgb(rgb, hsv);
Robotic team Flamingos [formerly CTU Dragons] - main SW repository