Lab 9: Mapping

2026-04-14T00:00:00+00:00
Lab 9: Mapping</h1>
The goal of this lab is to use the IMU and time of flight sensors on Meep to create a map of the environment. We sample distances from multiple points in the area, use transformation matricies to translate the measurements from the robot frame to the world frame, then convert this scan to a Line-Based Map that can be used in the simulation for lab 10.</p>

Orientation Control Mapping</h2>

Implementation</h3>
To get a full map of the environment, we get a 360 degrees of distance measurements from set points in the environment. We then compile these readings together to make a map. In order to get the 360 degree mapping, Meep must read distance values while spinning in place. I decided to do this using the orientation control I tuned in lab 6</a>. I created the command MAP</code> which allows to caller to determine how many datapoints they want from the turn. Meep will calculate the setpoint angles based on the number of readings desired.</p>
case MAP: </span>{</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>x_coord</span>);</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>y_coord</span>);</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>num_angles</span>);</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>Kp</span>);</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>Ki</span>);</span></span>
  success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>Kd</span>);</span></span>
  </span></span>
  angle_step </span>=</span> 360</span>/</span>num_angles</span>;</span></span>
  angle_idx </span>=</span> 0</span>;</span></span>
  i </span>=</span> 0</span>;</span></span>
  mapping </span>=</span> true</span>;</span></span>
}</span></span></code></pre>
From there, we simply record and transmit mapping data as shown below:</p>
void</span> make_mapping</span>(){</span></span>
  </span></span>
  // We start by getting a yaw value</span></span>
  if</span> (</span>myICM</span>.</span>status</span> =</span> ICM_20948_Stat_FIFOMoreDataAvail</span>) {</span></span>
    myICM</span>.</span>readDMPdataFromFIFO</span>(</span>&</span>imu_data</span>);</span></span>
  }</span></span>
  while</span>(</span>!</span>((</span>myICM</span>.</span>status</span> ==</span> ICM_20948_Stat_Ok</span>)</span> ||</span> (</span>myICM</span>.</span>status</span> ==</span> ICM_20948_Stat_FIFOMoreDataAvail</span>))){</span></span>
    if</span> (</span>myICM</span>.</span>status</span> =</span> ICM_20948_Stat_FIFOMoreDataAvail</span>) {</span></span>
      myICM</span>.</span>readDMPdataFromFIFO</span>(</span>&</span>imu_data</span>);</span></span>
    }</span></span>
  }</span></span>
  get_roll_pitch_yaw</span>(</span>0</span>);</span></span>
</span>
  // We set the first yaw value as our first setpoint</span></span>
  curr_setpoint </span>=</span> yaw_readings</span>[</span>0</span>];</span></span>
  // and record values from both our time of flights</span></span>
  record_tof_vals</span>(</span>num_datapoints</span>,</span> curr_setpoint</span>);</span></span>
  </span></span>
</span>
  for</span> (</span>int</span> j </span>=</span> 0</span>;</span> j </span><</span> num_angles</span>;</span> j</span>++</span>){</span></span>
    // we head to the next angle</span></span>
    curr_setpoint </span>+=</span> angle_step</span>;</span></span>
    if</span> (</span>curr_setpoint </span>></span> 180</span>){</span></span>
      curr_setpoint </span>-=</span> 360</span>;</span></span>
    }</span></span>
    else if</span> (</span>curr_setpoint </span>< -</span>180</span>) {</span></span>
      curr_setpoint </span>+=</span> 360</span>;</span></span>
    }</span></span>
    float</span> e </span>=</span> yaw_readings</span>[</span>i</span>-</span>1</span>]</span> -</span> curr_setpoint</span>;</span></span>
    if</span> (</span>e </span>></span> 180</span>){</span></span>
      e </span>-=</span> 360</span>;</span></span>
    }</span></span>
    else if</span> (</span>e </span>< -</span>180</span>) {</span></span>
      e </span>+=</span> 360</span>;</span></span>
    }</span></span>
    </span></span>
    // while we aren't at the desired angle, we call `pid_angle`</span></span>
    while</span> (</span>e </span>></span> 5</span> ||</span> e </span>< -</span>5</span> ) {</span></span>
      pid_angle</span>(</span>curr_setpoint</span>);</span></span>
      e </span>=</span> yaw_readings</span>[</span>i</span>-</span>1</span>]</span> -</span> curr_setpoint</span>;</span></span>
      if</span> (</span>e </span>></span> 180</span>){</span></span>
        e </span>-=</span> 360</span>;</span></span>
      }</span></span>
      else if</span> (</span>e </span>< -</span>180</span>) {</span></span>
        e </span>+=</span> 360</span>;</span></span>
      }</span></span>
    }</span></span>
    </span></span>
    // once we get there, we stop and record the time of flight and angle data</span></span>
    brake</span>();</span></span>
    tx_estring_value</span>.</span>clear</span>();</span></span>
    tx_estring_value</span>.</span>append</span>(</span>"GETTING DATA"</span>);</span></span>
    tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span>  </span></span>
    record_tof_vals</span>(</span>num_datapoints</span>,</span> curr_setpoint</span>);</span>  </span></span>
  }</span></span>
</span>
</span>
  ////////////////// Start Transmition ////////////////////////////</span></span>
</span>
  // Once we finish, we send the data over</span></span>
  tx_estring_value</span>.</span>clear</span>();</span></span>
  tx_estring_value</span>.</span>append</span>(</span>"Transmitting"</span>);</span></span>
  tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span>  </span></span>
</span>
  for</span> (</span>int</span> j </span>=</span> 0</span>;</span> j </span><</span> angle_idx</span>;</span> j</span>++</span>){</span></span>
    tx_estring_value</span>.</span>clear</span>();</span></span>
    tx_estring_value</span>.</span>append</span>(</span>time_stamps</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>tof1_readings</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>tof2_readings</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>gyro_yaw_readings</span>[</span>j</span>]);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
    tx_estring_value</span>.</span>append</span>(</span>setpoint_angle</span>[</span>j</span>]);</span></span>
    tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span></span>
    if</span> (</span>myICM</span>.</span>status</span> =</span> ICM_20948_Stat_FIFOMoreDataAvail</span>) {</span></span>
      myICM</span>.</span>readDMPdataFromFIFO</span>(</span>&</span>imu_data</span>);</span></span>
    }</span></span>
  }</span></span>
  tx_estring_value</span>.</span>clear</span>();</span></span>
  tx_estring_value</span>.</span>append</span>(</span>"Done"</span>);</span></span>
  tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span>          </span></span>
  Serial</span>.</span>println</span>(</span>"Done"</span>);</span></span>
  mapping </span>=</span> false</span>;</span></span></code></pre>
When actually recording the data, I record num_vals</code> datapoints to try to offset the error and noise discussed below.</p>
void</span> record_tof_vals</span>(</span>int</span> num_vals</span>,</span> int</span> angle</span>){</span></span>
  for</span> (</span>int</span> j </span>=</span> 0</span>;</span> j </span><</span> num_vals</span>;</span> j</span>++</span>){</span></span>
    while</span> (</span>!</span>distanceSensor2</span>.</span>checkForDataReady</span>()) {</span></span>
      if</span> (</span>myICM</span>.</span>status</span> =</span> ICM_20948_Stat_FIFOMoreDataAvail</span>) {</span></span>
        myICM</span>.</span>readDMPdataFromFIFO</span>(</span>&</span>imu_data</span>);</span></span>
      }</span></span>
    }</span></span>
    while</span> (</span>!</span>distanceSensor1</span>.</span>checkForDataReady</span>()) {</span></span>
      if</span> (</span>myICM</span>.</span>status</span> =</span> ICM_20948_Stat_FIFOMoreDataAvail</span>) {</span></span>
        myICM</span>.</span>readDMPdataFromFIFO</span>(</span>&</span>imu_data</span>);</span></span>
      }</span></span>
    }</span></span>
    if</span> (</span>myICM</span>.</span>status</span> =</span> ICM_20948_Stat_FIFOMoreDataAvail</span>) {</span></span>
      myICM</span>.</span>readDMPdataFromFIFO</span>(</span>&</span>imu_data</span>);</span></span>
    }</span></span>
    while</span>(</span>!</span>((</span>myICM</span>.</span>status</span> ==</span> ICM_20948_Stat_Ok</span>)</span> ||</span> (</span>myICM</span>.</span>status</span> ==</span> ICM_20948_Stat_FIFOMoreDataAvail</span>))){</span></span>
      if</span> (</span>myICM</span>.</span>status</span> =</span> ICM_20948_Stat_FIFOMoreDataAvail</span>) {</span></span>
        myICM</span>.</span>readDMPdataFromFIFO</span>(</span>&</span>imu_data</span>);</span></span>
      }</span></span>
    }</span></span>
    time_stamps</span>[</span>angle_idx</span>]</span> =</span> millis</span>();</span></span>
    get_roll_pitch_yaw</span>(</span>angle_idx</span>);</span></span>
    gyro_yaw_readings</span>[</span>angle_idx</span>]</span> =</span> yaw_readings</span>[</span>angle_idx</span>];</span></span>
    tof1_readings</span>[</span>angle_idx</span>]</span> =</span> distanceSensor1</span>.</span>getDistance</span>();</span></span>
    tof2_readings</span>[</span>angle_idx</span>]</span> =</span> distanceSensor2</span>.</span>getDistance</span>();</span></span>
    setpoint_angle</span>[</span>angle_idx</span>]</span> =</span> angle</span>;</span></span>
</span>
    distanceSensor1</span>.</span>clearInterrupt</span>();</span></span>
    distanceSensor2</span>.</span>clearInterrupt</span>();</span></span>
    angle_idx</span>++</span>;</span></span>
    if</span> (</span>angle_idx </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
      angle_idx </span>=</span> 0</span>;</span></span>
    }</span></span>
  }</span></span>
}</span></span></code></pre>Testing</h3>
To test this, I created a box for Meep to try mapping</p>
</iframe>
That resulted in the map below.</p>

Since I noticed the side time of flight having some much smaller readings, I adjusted its angle. That resulted in the graph below</p>

My sensor readings mostly agree. The discrepency between them is likely the result of having different distances due to their physical placement. As the car rotates, the front sensor will typically get closer to the wall it is reading from than the side sensor. Even in this small space, we already see quite a bit of error in both the sensors. I would reason that, in an empty room, its likely that one of the sensors would hallunicate some sort of wall or blockage that does not exist. It is likely that we are going to get a lot of noise data points and will have to look for where walls appear on mulitple trials to confirm where the real walls are located.</p>
Real Environment</h3>

This is the environment we are trying to map. I measured the distance at 20 different angles, taking 5 measurements at each to try to offset bad readings. I got data from several locations in the environment that were a set distance apart. Specifically, I grabed data from (0,0), (-3,-2), (5,3), (0,3), and (5,-3). At each point, Meep would rotate and gather data.</p>
</iframe>
The mapping from each point can easily be seen in polar coordinates as shown below.</p>

I continued mapping, getting the data below.



</p>
Transforming the data</h2>
From here, we have data in the robot frame. To combine our data together, we can use a tranformation matrix to rotate and translate our data to the world frame, which will allow us to combine the data together. Our rotation matrix consists of a rotation (in blue) and translation (in purple). Since I started every trial with the bot facing along the positive x in our environment, we can map the inital angle to theta below and adjust from there.</p>

While using the inital theta as the rotation works for some of the data, there are many places where I was not precise with placing the robot’s facing angle. I adjust these by graphing the transformed data, then measuring the angle offset.</p>

The location of the sensors in the robot frame is not precisely (0,0). To be more accurate, I should have used a translation matrix to adjust for the location of the sensors. I found that the readings from the sensors mostly lined up so I decided to forgo this step when processing the data.</p>
Results</h2>
After adjusting the rotation values, I ended up with the maps below</p>



These mappings roughly map to the real environment as shown below


</p>

# locations in mm</span></span>
x_off</span> =</span> [</span>0</span>,</span> 0</span>,</span> 1524</span>,</span> 1524</span>,</span> -</span>914.4</span>]</span></span>
y_off</span> =</span> [</span>0</span>,</span> 914.4</span>,</span> 914.4</span>,</span> -</span>914.4</span>,</span> -</span>609.6</span>]</span></span>
</span>
# adjusted offsets for the rotation matrix</span></span>
theta_off</span> =</span> np</span>.</span>deg2rad</span>(</span>np</span>.</span>array</span>([</span>68.586</span>,</span> 54.65656937979465</span>,</span> 64.538</span>,</span>  115.60284796050428</span>,</span> 60.6559914284864</span>]))</span></span>
</span>
# plot everything</span></span>
plt</span>.</span>figure</span>()</span></span>
</span>
for</span> i</span> in</span> range</span>(</span>5</span>):</span></span>
    # convert degrees to radians</span></span>
    theta</span> =</span> np</span>.</span>deg2rad</span>(</span>data_arrays</span>[</span>f</span>"</span>yaw_</span>{</span>i</span>}</span>"</span>]</span> +</span> 90</span>)</span> # add 90 to the side sensor so that it aligns with the front sensor</span></span>
    theta2</span> =</span> np</span>.</span>deg2rad</span>(</span>data_arrays</span>[</span>f</span>"</span>yaw_</span>{</span>i</span>}</span>"</span>])</span></span>
</span>
    theta_offset</span> =</span> theta_off</span>[</span>i</span>]</span></span>
    </span></span>
    rotation_matrix</span> =</span> np</span>.</span>array</span>([[</span>np</span>.</span>cos</span>(</span>theta_offset</span>),</span> -</span>np</span>.</span>sin</span>(</span>theta_offset</span>),</span> x_off</span>[</span>i</span>]],</span> </span></span>
                                [</span>np</span>.</span>sin</span>(</span>theta_offset</span>),</span> np</span>.</span>cos</span>(</span>theta_offset</span>),</span> -</span>y_off</span>[</span>i</span>]],</span> </span></span>
                                [</span>   0</span>,</span>                  0</span>,</span>                   1</span>]])</span></span>
</span>
    </span></span>
    r</span> =</span> data_arrays</span>[</span>f</span>"</span>tof1_</span>{</span>i</span>}</span>"</span>]</span></span>
    r2</span> =</span> data_arrays</span>[</span>f</span>"</span>tof2_</span>{</span>i</span>}</span>"</span>]</span></span>
    </span></span>
    # polar to cartesian</span></span>
    x</span> =</span> r</span> *</span> np</span>.</span>cos</span>(</span>theta</span>)</span></span>
    y</span> =</span> r</span> *</span> np</span>.</span>sin</span>(</span>theta</span>)</span></span>
    x2</span> =</span> r2</span> *</span> np</span>.</span>cos</span>(</span>theta2</span>)</span></span>
    y2</span> =</span> r2</span> *</span> np</span>.</span>sin</span>(</span>theta2</span>)</span></span>
</span>
    # transform the readings</span></span>
    roatated</span> =</span> rotation_matrix</span> @</span> [</span>x</span>,</span> y</span>,</span> np</span>.</span>ones_like</span>(</span>x</span>)]</span></span>
    roatated2</span> =</span> rotation_matrix</span> @</span> [</span>x2</span>,</span> y2</span>,</span> np</span>.</span>ones_like</span>(</span>x2</span>)]</span></span>
</span>
    # then graph them</span></span>
    plt</span>.</span>scatter</span>(</span>roatated</span>[</span>0</span>],</span> roatated</span>[</span>1</span>],</span> s</span>=</span>1</span>,</span> label</span> =</span> f</span>"tof 1 data </span>{</span>i</span>}</span>"</span>)</span></span>
    plt</span>.</span>scatter</span>(</span>roatated2</span>[</span>0</span>],</span> roatated2</span>[</span>1</span>],</span> s</span>=</span>1</span>,</span> label</span> =</span> f</span>"tof 2 data </span>{</span>i</span>}</span>"</span>)</span></span>
</span>
plt</span>.</span>gca</span>().</span>set_aspect</span>(</span>'equal'</span>)</span></span>
plt</span>.</span>xlabel</span>(</span>"X"</span>)</span></span>
plt</span>.</span>ylabel</span>(</span>"Y"</span>)</span></span>
plt</span>.</span>title</span>(</span>"2D Map from Side TOF"</span>)</span></span>
# plt.legend() # The legend blocks the plot so I commented it out for the screenshots. Its mostly useful for debugging</span></span>
plt</span>.</span>show</span>()</span></span></code></pre>Collaboration and Sources</h2>
I consulted Jack Long’s website and discussed rotation vector mapping with Dyllan Hofflich.</p>
Ananya's Portfolio - Mapping

Lab 9: Mapping

Orientation Control Mapping</h2>

Collaboration and Sources</h2> I consulted Jack Long’s website and discussed rotation vector mapping with Dyllan Hofflich.</p>

Collaboration and Sources</h2>
I consulted Jack Long’s website and discussed rotation vector mapping with Dyllan Hofflich.</p>
