Ananya's Portfolio

Lab 7: Kalman Filter

2026-03-23T00:00:00+00:00

Lab 7: Kalman Filter</h1>
The goal of this lab is to implement a Kalman filter so the robot, Meep, is able to drive at a high speed towards a wall and stop before hitting it. We do this by creating a physics model to predict Meep’s movement, then tuning a Kalman filter and using its predictions to estimate position while waiting for feedback from the time of flight.</p>

What is a Kalman Filter?</h2>
A Kalman filter is a way to combine a model for a system and sensor measurements to get an accurate estimate for the system. In this case, we have a time of flight that give accurate, but slow estimates for our position. By creating a physics model for our system, we can combine a calculated position with our sensor measurements to get quicker and more accurate location data for our robot.</p>

Estimating Drag and Momentum</h2>

To start, we need to have an accurate physical model for our bot. Let u</code> be the control signal (aka the PWM value or duty cycle) passed in to our motors), and let x</code> be Meep’s position, represented as distance from the wall infront of Meep. This leaves us with the equation below where d</code> is a constant representing drag while m</code> represents momentum.</p>

We can find d</code> by finding the steady speed for some u</code> (motor contorl input). I chose to operate my car at an analog value of 150 (duty cycle of 58.82%). I ran 3 trials and calculated the velocity from each as shown below.</p>

From there, I took the average of the three runs and attempted to fit a expoential decay function to the measurements.</p>

From here, we see the steady state velocity is -3176 mm/s. We can calculate d</code> to be 150/the steady state velocity so that the control input can be the passed in PWM value in the future. We calculate m</code> to be t_90/ln(1-0.9) where t_90 is the time it takes to get to 90% of the steady velocity. We have a rise time of 1.73 s giving us an m</code> of 0.75.</p>

d</span> = -</span>0.047227600035556115</span></span>
m</span> =</span> 0.7508005698913823</span></span>
A</span> =</span> np</span>.</span>array</span>([</span></span>
    [</span>0</span>,</span> 1</span>],</span></span>
    [</span>0</span>,</span> d</span>/</span>m</span>]</span></span>
])</span></span>
</span>
B</span> =</span> np</span>.</span>array</span>([</span></span>
    [</span>0</span>],</span></span>
    [</span>1</span>/</span>m</span>]</span></span>
])</span></span>
</span>
C</span> =</span> np</span>.</span>array</span>([[</span>1</span>,</span>0</span>]])</span></span>
</span>
def</span> kf</span>(</span>mu</span>,</span>sigma</span>,</span>u</span>,</span>y</span>,</span> dt</span>,</span> update</span> =</span> True</span>):</span></span>
    Ad</span> =</span> np</span>.</span>eye</span>(</span>len</span>(</span>A</span>))</span> +</span> dt</span> *</span> A  </span></span>
    Bd</span> =</span> dt</span> *</span> B</span></span>
    </span></span>
    mu_p</span> =</span> Ad</span>.</span>dot</span>(</span>mu</span>)</span> +</span> Bd</span>.</span>dot</span>(</span>u</span>)</span> </span></span>
    sigma_p</span> =</span> Ad</span>.</span>dot</span>(</span>sigma</span>.</span>dot</span>(</span>Ad</span>.</span>transpose</span>()))</span> +</span> sig_u</span></span>
</span>
    if</span> update</span>:</span></span>
        sigma_m</span> =</span> C</span>.</span>dot</span>(</span>sigma_p</span>.</span>dot</span>(</span>C</span>.</span>transpose</span>()))</span> +</span> sig_z</span></span>
        kkf_gain</span> =</span> sigma_p</span>.</span>dot</span>(</span>C</span>.</span>transpose</span>().</span>dot</span>(</span>np</span>.</span>linalg</span>.</span>inv</span>(</span>sigma_m</span>)))</span></span>
    </span></span>
        y_m</span> =</span> y</span>-</span>C</span>.</span>dot</span>(</span>mu_p</span>)</span></span>
        mu</span> =</span> mu_p</span> +</span> kkf_gain</span>.</span>dot</span>(</span>y_m</span>)</span>    </span></span>
        sigma</span>=</span>(</span>np</span>.</span>eye</span>(</span>2</span>)</span>-</span>kkf_gain</span>.</span>dot</span>(</span>C</span>)).</span>dot</span>(</span>sigma_p</span>)</span></span>
        return</span> mu</span>,</span>sigma</span></span>
    else</span>:</span></span>
        return</span> mu_p</span>,</span> sigma_p</span></span>
</span></code></pre>Tuning noise on simulated data</h2>
From here, we need to tune our uncertainity of our sensors and model. We have 4 parameters we can tune, the position and speed uncertainity from the physics model, the sensor noise, and the initial sigma.</p>
I started with a position and velocity noise of 100 and a sensor noise of 400. This noise was too large the sensor uncertainity. There was a major lag in the Kalman filter as sensor readings were bascially not trusted at all</p>

From there, I lowered the sensor noise as was able to see the filter follow the time of flight. I wanted the filter to be able to deviate from the time of flight so I chose a standard deviation of 10. This lead to the filter following the sensor almost exactly on all my datasets.</p>



I then had my kalman extrapolate predicted positions for the time steps where the time of flight was not ready with a new reading. This resulted in the spiky graph below.</p>

This made me reevalute my position and velocity uncertainity. I expect the physics model to be relavtively accurate for the position but not for the velocity. I updated the sigmas to reflect this (decreaing position to 10 which increasing the velcoity uncertainity), to get the graph below:</p>

sigma_1</span> =</span> np</span>.</span>sqrt</span>((</span>1</span>**</span>2</span>)</span>*</span>1</span>/</span>dt</span>)</span> # position variance</span></span>
sigma_2</span> =</span>  np</span>.</span>sqrt</span>((</span>50</span>**</span>2</span>)</span>*</span>1</span>/</span>dt</span>)</span> # speed variance</span></span>
sigma_3</span> =</span> 10</span> # sensor noise</span></span>
</span>
sig_u</span>=</span>np</span>.</span>array</span>([[</span>sigma_1</span>**</span>2</span>,</span>0</span>],[</span>0</span>,</span>sigma_2</span>**</span>2</span>]])</span></span>
sig_z</span>=</span>np</span>.</span>array</span>([[</span>sigma_3</span>**</span>2</span>]])</span></span></code></pre>Implement the Kalman Filter with PID</h2>
From there, I implemented the Kalman filter on the Artemis as shown below</p>
Matrix</span><</span>2</span>,</span>1</span>></span> mu </span>=</span> {</span>0</span>,</span>0</span>};</span> </span></span>
Matrix</span><</span>2</span>,</span>2</span>></span> sigma </span>=</span> {</span>100</span>,</span>0</span>,</span>100</span>,</span>0</span>};</span> </span></span>
</span>
float</span> d </span>= -</span>0.047227600035556115</span>;</span></span>
float</span> m </span>=</span> 0.7508005698913823</span>;</span></span>
</span>
</span>
Matrix</span><</span>2</span>,</span>2</span>></span> kA </span>=</span> {</span>0</span>,</span> 1</span>,</span></span>
                 0</span>,</span> d</span>/</span>m</span>};</span> </span></span>
</span>
Matrix</span><</span>2</span>,</span>1</span>></span> kB </span>=</span> {</span>0</span>,</span> 1</span>/</span>m</span>};</span> </span></span>
</span>
Matrix</span><</span>1</span>,</span>2</span>></span> kC </span>=</span> {</span>1</span>,</span> 0</span>};</span> </span></span>
</span>
float</span> var_position </span>=</span> 9.09</span>;</span></span>
float</span> var_speed </span>=</span> 227272.73</span>;</span></span>
float</span> var_sensor </span>=</span> 100</span>;</span></span>
</span>
Matrix</span><</span>2</span>,</span>2</span>></span> sigma_u </span>=</span> {</span>var_position</span>,</span> 0</span>,</span></span>
                        0</span>,</span> var_speed</span>};</span></span>
</span>
Matrix</span><</span>1</span>></span> sigma_z </span>=</span> {</span>var_sensor</span>};</span></span>
</span>
float</span> kf</span>(</span>float</span> control_in</span>,</span> float</span> sensor_reading</span>,</span> float</span> dt</span>,</span> int</span> tof_valid</span>){</span></span>
  Matrix</span><</span>1</span>></span> control </span>=</span> {</span>control_in</span>};</span></span>
  Matrix</span><</span>1</span>></span> sensor </span>=</span> {</span>sensor_reading</span>};</span></span>
  // Matrix<1> dt = {dt/1000.0};</span></span>
  Matrix</span><</span>2</span>,</span> 2</span>></span> I </span>=</span> {</span> 1</span>,</span> 0</span>,</span> 0</span>,</span> 1</span>};</span></span>
  Matrix</span><</span>2</span>,</span> 2</span>></span> Ad </span>=</span> I</span>;</span></span>
  Ad</span>(</span>0</span>,</span>0</span>)</span> +=</span> kA</span>(</span>0</span>,</span>0</span>)</span> *</span> (</span>dt</span>);</span></span>
  Ad</span>(</span>0</span>,</span>1</span>)</span> +=</span> kA</span>(</span>0</span>,</span>1</span>)</span> *</span> (</span>dt</span>);</span></span>
  Ad</span>(</span>1</span>,</span>0</span>)</span> +=</span> kA</span>(</span>1</span>,</span>0</span>)</span> *</span> (</span>dt</span>);</span></span>
  Ad</span>(</span>1</span>,</span>1</span>)</span> +=</span> kA</span>(</span>1</span>,</span>1</span>)</span> *</span> (</span>dt</span>);</span></span>
  Matrix</span><</span>2</span>,</span> 1</span>></span> Bd </span>=</span> kB</span>;</span></span>
  Bd</span>(</span>0</span>,</span>0</span>)</span> *=</span> (</span>dt</span>);</span></span>
  Bd</span>(</span>1</span>,</span>0</span>)</span> *=</span> (</span>dt</span>);</span></span>
</span>
  Matrix</span><</span>2</span>,</span> 1</span>></span> mu_p </span>=</span> Ad</span>*</span>mu </span>+</span> Bd</span>*</span>control</span>;</span></span>
  Matrix</span><</span>2</span>,</span> 2</span>></span> sigma_p </span>=</span> Ad</span>*</span>sigma</span>*~</span>Ad </span>+</span> sigma_u</span>;</span></span>
</span>
  if</span> (</span>tof_valid</span>) {</span></span>
    Matrix</span><</span>1</span>></span> sigma_m </span>=</span> kC </span>*</span> sigma_p </span>* ~</span>kC </span>+</span> sigma_z</span>;</span></span>
    Matrix</span><</span>2</span>,</span>1</span>></span> kff_gain </span>=</span> sigma_p</span>*</span>(</span>~</span>kC</span>*</span>Inverse</span>(</span>sigma_m</span>));</span></span>
</span>
    Matrix</span><</span>1</span>></span> y_m </span>=</span> sensor</span>-</span>kC</span>*</span>mu_p</span>;</span></span>
    mu </span>=</span> mu_p</span>+</span>kff_gain</span>*</span>y_m</span>;</span> </span></span>
    sigma </span>=</span> (</span>I </span>-</span> kff_gain</span>*</span>kC</span>)</span>*</span>sigma_p</span>;</span></span>
</span>
    return</span> mu</span>(</span>0</span>,</span>0</span>);</span></span>
  }</span></span>
  else</span> {</span></span>
    mu </span>=</span> mu_p</span>;</span></span>
    sigma </span>=</span> sigma_p</span>;</span></span>
    return</span> mu_p</span>(</span>0</span>,</span>0</span>);</span></span>
  }</span></span>
</span>
}</span></span></code></pre>
Then I integrated it with my pid code:</p>
void</span> pid_wall_dist</span>(</span>float</span> dist</span>){</span></span>
  float</span> curr_dist</span>;</span></span>
  time_stamps</span>[</span>i</span>]</span> =</span> millis</span>();</span></span>
  if</span> (</span>!</span>distanceSensor2</span>.</span>checkForDataReady</span>()) {</span></span>
    tof2_readings</span>[</span>i</span>]</span> = -</span>5</span>;</span></span>
</span>
    // no sensor data on the first time step</span></span>
    if</span> (</span>i </span>==</span> 0</span>) {</span></span>
      tof1_readings</span>[</span>i</span>]</span> = -</span>5</span>;</span></span>
      p_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      i_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      d_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      return</span> ;</span></span>
    }</span></span>
</span>
    // no valid readings yet</span></span>
    else if</span> (</span>tof2_readings</span>[</span>i</span>-</span>1</span>]</span> == -</span>5</span> &&</span> tof1_readings</span>[</span>i</span>-</span>1</span>]</span> == -</span>5</span>){</span></span>
      tof1_readings</span>[</span>i</span>]</span> =</span> tof1_readings</span>[</span>i</span>-</span>1</span>];</span></span>
      p_data</span>[</span>i</span>]</span> =</span> p_data</span>[</span>i</span>-</span>1</span>];</span></span>
      i_data</span>[</span>i</span>]</span> =</span> i_data</span>[</span>i</span>-</span>1</span>];</span></span>
      d_data</span>[</span>i</span>]</span> =</span> d_data</span>[</span>i</span>-</span>1</span>];</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> motor_data</span>[</span>i</span>-</span>1</span>];</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      return</span> ;</span></span>
    }</span></span>
</span>
</span>
    // Use kalman to predict position</span></span>
    else</span> {</span></span>
      int</span> dt </span>=</span> time_stamps</span>[</span>i</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>1</span>];</span></span>
</span>
      if</span> (</span>dt </span>==</span> 0</span>) {</span></span>
        curr_dist </span>=</span> tof1_readings</span>[</span>i</span>-</span>1</span>];</span>   </span></span>
      }</span></span>
      else</span> {</span></span>
        curr_dist </span>=</span> kf</span>(</span>motor_data</span>[</span>i</span>-</span>1</span>],</span> 0</span>, (</span>float</span>)</span> dt</span>/</span>1000.0</span>,</span> false</span>);</span></span>
      }</span></span>
      tof1_readings</span>[</span>i</span>]</span> =</span> curr_dist</span>;</span></span>
    }</span></span>
  }</span></span>
</span>
</span>
  // Use Kalman with TOF to predict position and update the filter</span></span>
  else</span> {</span></span>
    int</span> dt </span>=</span> time_stamps</span>[</span>i</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>1</span>];</span></span>
    tof2_readings</span>[</span>i</span>]</span> =</span> distanceSensor2</span>.</span>getDistance</span>();</span></span>
    tof1_readings</span>[</span>i</span>]</span> =</span> kf</span>(</span>motor_data</span>[</span>i</span>-</span>1</span>],</span> tof2_readings</span>[</span>i</span>], (</span>float</span>)</span> dt</span>/</span>1000.0</span>,</span> true</span>);</span></span>
    curr_dist </span>=</span>  tof1_readings</span>[</span>i</span>];</span></span>
    distanceSensor2</span>.</span>clearInterrupt</span>();</span></span>
    old_time </span>=</span> time_stamps</span>[</span>i</span>];</span></span>
  }</span></span>
  float</span> e </span>=</span> tof1_readings</span>[</span>i</span>]</span> -</span> dist</span>;</span></span>
  error_total </span>+=</span> e</span>;</span></span>
</span>
  // Integrator Windup code ///</span></span>
  if</span> (</span>error_total </span>></span> 2000</span>){</span></span>
    error_total </span>=</span> 2000</span>;</span></span>
  }</span></span>
  else if</span> (</span>error_total </span>< -</span>3000</span>){</span></span>
    error_total </span>= -</span>3000</span>;</span></span>
  }</span></span>
  /////////end//////////////</span></span>
</span>
  error_change </span>=</span> e</span>-</span>last_error</span>;</span></span>
  int</span> motor_out </span>=</span> (</span>int</span>)(</span>Kp</span>*</span>e </span>+</span> Ki</span>*</span>error_total </span>+</span> Kd</span>*</span>error_change</span>);</span></span>
</span>
  p_data</span>[</span>i</span>]</span> =</span> Kp</span>*</span>e</span>;</span></span>
  i_data</span>[</span>i</span>]</span> =</span> Ki</span>*</span>error_total</span>;</span></span>
  d_data</span>[</span>i</span>]</span> =</span> Kd</span>*</span>error_change</span>;</span></span>
</span>
</span>
  ///// Motor Updates //////////////</span></span>
  if</span> (</span>motor_out </span>> -</span>8</span> &&</span> motor_out </span><</span> 8</span>){</span></span>
    forward</span>(</span>0</span>,</span> 0</span>);</span></span>
    motor_out </span>=</span> 0</span>;</span> </span></span>
  }</span></span>
  else if</span> (</span>motor_out </span>></span> 5</span>){</span></span>
    if</span> (</span>motor_out </span><</span> 37</span>){</span></span>
      motor_out </span>=</span> 37</span>;</span> </span></span>
    }</span></span>
    else if</span> (</span>motor_out </span>></span> 255</span>){</span></span>
      motor_out </span>=</span> 255</span>;</span></span>
    }</span></span>
    forward</span>(</span>motor_out</span>-</span>3</span>,</span> motor_out</span>+</span>3</span>);</span>   </span></span>
  }</span></span>
  else</span> {</span></span>
    if</span> (</span>motor_out </span>> -</span>37</span>){</span></span>
      motor_out </span>= -</span>37</span>;</span> </span></span>
    }</span></span>
    else if</span> (</span>motor_out </span>< -</span>255</span>){</span></span>
      motor_out </span>= -</span>255</span>;</span></span>
    }</span></span>
    backward</span>(</span>-</span>motor_out</span>,</span> -</span>motor_out</span>);</span></span>
  }</span></span>
  motor_data</span>[</span>i</span>]</span> =</span> motor_out</span>;</span></span>
  ///////////end/////////////////</span></span>
  </span></span>
  // Step forward</span></span>
  last_error </span>=</span> e</span>;</span></span>
  i</span>++</span>;</span></span>
  if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
    i </span>=</span> 0</span>;</span></span>
  }</span></span>
}</span></span></code></pre>
I started with only allowing the robot to run for 1 second before stopping it to ensure that any initial implementation bugs could be worked out without damage being done to Meep. Once, the Kalman filter was showing expected results, I was ready to run the full PID.</p>

Using my original PID settings, we can see that Meep is able to start at its max duty cycle and stop just before hitting the wall.</p>
</iframe>


Collaboration and Sources</h2>
I referenced the lecture slides when setting up the Kalman filter. I also referenced Trevor Dales and Aidan Derocher’s lab writeups.</p>



Lab 6: Orientation Control
2026-03-16T00:00:00+00:00
Lab 6: Orientation Control</h1>
The goal of this lab was to get the robot, Meep, to be able to turn to a specified orientation, and maintain that position when encountering disturbances. I implement and tune a PI controller that shows success in varying terain conditions. A PID or Proportional, Inetegral, and Derivative controller is a controller that adjusts the PWM signal based on the bots current error from a set point. In this lab, the setpoint is a given orientation angle and the error is the difference between estimated angle (via IMU readings) and the setpoint. The proportion term is important for obvious reasons. I also implemented the integral term to help with steady error and make the controller more effective on varying floor conditions (as discussed below).</p>
Prelab: Data transfer</h2>
One of the major decisions of this lab way how to tranfer data. Transferring each piece of data in the control loop would slow the loop down, resulting in slower updates and slower reaction time. Trying to store all the data is limited by the maximum storage of the Artemis. I decide to store all my values and transmit after PID has stopped. This results in a long waiting time when PID is stopped, but allows the code to loop fast while in the PID loop. I allow the buffer to override the inital values if the controller tries to save more values than the set max buffer length but set the buffer length to the largest number possible to prevent this case from occurring. I parse this data into python lists like I have in previous labs and save them to a CSV for the future.</p>
case PID_STOP: </span>{</span></span>
  // Stop PID and motors</span></span>
  pid_running </span>=</span> false</span>;</span></span>
  analogWrite</span>(</span>AB1_left</span>,</span>0</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_left</span>,</span>0</span>);</span></span>
  analogWrite</span>(</span>AB1_right</span>,</span>0</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_right</span>,</span>0</span>);</span></span>
</span>
  // Transfer Data</span></span>
  for</span> (</span>int</span> j </span>=</span> 0</span>;</span> j </span><</span> i</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>roll_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>pitch_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>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>p_data</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>i_data</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>d_data</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>motor_data</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>
</span>
    // drain the DMP FIFO so the chip doesn't crash</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>
</span>
  // Signal that the data is done sending</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>
  break</span>;</span></span>
</span>
  Serial</span>.</span>println</span>(</span>"Done"</span>);</span></span>
}</span></span></code></pre>
I also added the SET_ANGLE</code> command, which allows the user to change the setpoint while PID is running.</p>
case SET_ANGLE: </span>{</span></span>
    success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>curr_setpoint</span>);</span></span>
    if</span> (</span>!</span>success</span>)</span></span>
        return</span>;</span></span>
    break</span>;</span></span>
}</span></span></code></pre>Range/Sampling time and DMP</h2>
I enabled the DMP (Digital Motion Processing) to get calibrated accurate IMU angles. The DMP fills a Queue (First In, First Out) with values from the desired report. To prevent the DMP FIFO from filling, I had to make sure to pop vaules while waiting for comamnds and while transfering data. I chose to get the Game Rotation Vector which return the quaterion of the angle facing, calcultated by combinding the accelerometer and gyroscope values. I chose to not use the magnetometer since Meep only needs to know its relative orientation and I wasn’t sure how the megnetometer would perform in different environments. I was able to run its max frequency of about 55 Hz which resulted in an average of 17.6 ms</strong> between motor updates.</p>
void</span> pid_angle</span>(</span>float</span> angle</span>){</span></span>
  myICM</span>.</span>readDMPdataFromFIFO</span>(</span>&</span>imu_data</span>);</span></span>
  // Skip everything if valid data wasn't available</span></span>
  if</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></span>
  {</span></span>
      time_stamps</span>[</span>i</span>]</span> =</span> millis</span>();</span></span>
      get_roll_pitch_yaw</span>(</span>i</span>);</span></span>
</span>
</span>
      // Calcualate error, total error, and error change</span></span>
      float</span> e </span>=</span> yaw_readings</span>[</span>i</span>]</span> -</span> angle</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>
      error_total </span>+=</span> e</span>;</span></span>
</span>
      // Integrator Windup code</span></span>
      if</span> (</span>error_total </span>></span> 1000</span>){</span></span>
        error_total </span>=</span> 1000</span>;</span></span>
      }</span></span>
      else if</span> (</span>error_total </span>< -</span>1000</span>){</span></span>
        error_total </span>= -</span>1000</span>;</span></span>
      }</span></span>
</span>
      error_change </span>=</span> e</span>-</span>last_error</span>;</span></span>
</span>
</span>
      // Motor Update</span></span>
      int</span> motor_out </span>=</span> (</span>int</span>)(</span>Kp</span>*</span>e </span>+</span> Ki</span>*</span>error_total </span>+</span> Kd</span>*</span>error_change</span>);</span></span>
</span>
      // Record Data</span></span>
      p_data</span>[</span>i</span>]</span> =</span> Kp</span>*</span>e</span>;</span></span>
      i_data</span>[</span>i</span>]</span> =</span> Ki</span>*</span>error_total</span>;</span></span>
      d_data</span>[</span>i</span>]</span> =</span> Kd</span>*</span>error_change</span>;</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> motor_out</span>;</span></span>
      setpoint_angle</span>[</span>i</span>]</span> =</span> angle</span>;</span></span>
</span>
      // Set motor values while adjusting for the deadband region</span></span>
      if</span> (</span>motor_out </span><</span> 5</span> &&</span> motor_out </span>> -</span>5</span>){</span></span>
        right</span>(</span>0</span>,</span> 0</span>);</span></span>
      }</span></span>
      else if</span> (</span>motor_out </span>></span> 0</span>){</span></span>
        if</span> (</span>motor_out </span><</span> 140</span>){</span></span>
          motor_out </span>=</span> 140</span>;</span></span>
        }</span></span>
        right</span>(</span>motor_out</span>,</span> motor_out</span>);</span>   </span></span>
      }</span></span>
      else</span> {</span></span>
        if</span> (</span>motor_out </span>> -</span>140</span>){</span></span>
          motor_out </span>= -</span>140</span>;</span></span>
        }</span></span>
        left</span>(</span>-</span>motor_out</span>,</span> -</span>motor_out</span>);</span></span>
      }</span></span>
      last_error </span>=</span> e</span>;</span></span>
      </span></span>
      // Increment data index</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      </span></span>
  </span></span>
  }</span></span>
}</span></span></code></pre>Tuning process and results</h2>
I started with just a proportional controller on a smooth but grippy floor. With proportion drive, Meep was able to quickly turn to the desire angle with minimal overshoot and negliable steady state error.</p>
</iframe>
I adjusted the P term to be 1.9</strong> and tested it on a set sequence. I had the robot reach 0, 90, -90, 45, and 0 degrees with 5 seconds between setpoint changes. I did this to test accuracy, speed, and ability to adjust to a changing setpoint. I saw very accurate and quick positioning.</p>
</iframe>


Next, I tried running the robot on carpet and immediately ran into problems. The robot struggled to turn with the currently set deadband region.</p>
</iframe>

Since I didn’t want to adjust the deadband region but wanted Meep to be able to turn accurately on all floor types, I decided to add the integral term. An integral term of 0.1</strong> allowed Meep to turn accurately on carpet.</p>
</iframe>

We can see the addition of the I</code> term allowed Meep to get enough power to turn successfully!</p>

Finally, I tested Meep with its newly tuned PI controller on a tile floor.</p>


I gave Meep some light kicks to demonstrate its ability to readjust.</p>
</iframe>


Grad Task: Integrator Term</h2>
The i</code> term is proportional to the cummalative error. Without being capped, the error term can greatly accumulate causing a high amount of overshoot once the robot is unstuck. In this lab, this causes the robot to be stuck spinning since the error in unable to decrease so the PWM is always maxed out.</p>
</iframe>

We can fix this by capping the max value. Earlier in the lab, I capped the cummlative error to 2000. I decided to decrement the cap to 1000 so that Meep would not be stuck overshooting (osciallating) for as long.</p>
</iframe>

Collaboration and Sources</h2>
I did the lab with very light discussion with Dyllan Hofflich.</p>


Lab 5: Linear PID control and Linear Interpolation
2026-03-09T00:00:00+00:00
Lab 5: Linear PID control and Linear interpolation</h1>
The goal of this lab was to get the robot, Meep, to drive forward and stop extactly 304 mm from a wall via PID controls. This lab explores tuning a PID controller, extrapolating sensor data, and handling integrator windup.</p>
Why PID?</h2>
PID or Proportional, Inetegral, and Derivative controller is a controller that adjusts the PWM signal based on the bots current error from a set point. In this lab, the setpoint is the distance away from the wall and the error is determined to be the difference between the car’s estimated position (via Time of Flight) and the setpoint.</p>
I chose to implement a full PID controller for this lab. I started with just a proportional term, which resulted in either heavy overshoot or stopping a great distance from the target. While I added the I term (required since I am a grad student), this overshoot issue got worse. To correct for this, the derivative term was added. I implemented a PID_START</code> and PID_STOP</code> command to signal to the robot to start and stop the PID cotnroller. The PID_START</code> command takes in values for Kp</code>, Ki</code>, and Kd</code> and triggers the pid_wall_dist</code> function to run as shown below:</p>
case PID_START: </span>{</span></span>
  Serial</span>.</span>println</span>(</span>"Starting PID"</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>
  if</span> (</span>!</span>success</span>)</span></span>
      return</span>;</span></span>
</span>
  pid_running </span>=</span> true</span>;</span> //tell loop to call pid_wall_dist</span></span>
  </span></span>
  // reset variables</span></span>
  i </span>=</span> 0</span>;</span></span>
  error_total </span>=</span> 0</span>;</span> </span></span>
  last_error </span>=</span> 0</span>;</span></span>
  error_change </span>=</span> 0</span>;</span></span>
  break</span>;</span></span>
}</span></span></code></pre>// Takes in [dist] which is the setpoint in mm</span></span>
void</span> pid_wall_dist</span>(</span>float</span> dist</span>){</span></span>
  float</span> curr_dist</span>;</span></span>
  time_stamps</span>[</span>i</span>]</span> =</span> millis</span>();</span></span>
  </span></span>
  // if there is no sensor reading</span></span>
  if</span> (</span>!</span>distanceSensor2</span>.</span>checkForDataReady</span>()) {</span></span>
    tof2_readings</span>[</span>i</span>]</span> = -</span>5</span>;</span></span>
</span>
    // no data to extrapolate with</span></span>
    if</span> (</span>i </span>==</span> 0</span>) {</span></span>
      tof1_readings</span>[</span>i</span>]</span> = -</span>5</span>;</span></span>
      p_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      i_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      d_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      return</span> ;</span></span>
    }</span></span>
</span>
    // only one reading or no valid readings</span></span>
    else if</span> (</span>i </span>==</span> 1</span> ||</span> tof1_readings</span>[</span>i</span>-</span>1</span>]</span> == -</span>5</span> ||</span> tof1_readings</span>[</span>i</span>-</span>2</span>]</span> == -</span>5</span>){</span></span>
      tof1_readings</span>[</span>i</span>]</span> =</span> tof1_readings</span>[</span>i</span>-</span>1</span>];</span></span>
      p_data</span>[</span>i</span>]</span> =</span> p_data</span>[</span>i</span>-</span>1</span>];</span></span>
      i_data</span>[</span>i</span>]</span> =</span> i_data</span>[</span>i</span>-</span>1</span>];</span></span>
      d_data</span>[</span>i</span>]</span> =</span> d_data</span>[</span>i</span>-</span>1</span>];</span></span>
      motor_data</span>[</span>i</span>]</span> =</span> motor_data</span>[</span>i</span>-</span>1</span>];</span></span>
      i</span>++</span>;</span></span>
      if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
        i </span>=</span> 0</span>;</span></span>
      }</span></span>
      return</span> ;</span></span>
    }</span></span>
</span>
    // Can extrapolate next tof value</span></span>
    else</span> {</span></span>
      int</span> dt </span>=</span> time_stamps</span>[</span>i</span>-</span>1</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>2</span>];</span></span>
</span>
      if</span> (</span>dt </span>==</span> 0</span>) {</span></span>
        curr_dist </span>=</span> tof1_readings</span>[</span>i</span>-</span>1</span>];</span>   // fallback</span></span>
      }</span></span>
      else</span> {</span></span>
        curr_dist </span>=</span> (</span>old_slope</span>)</span> *</span>                    //  m</span></span>
              (</span>time_stamps</span>[</span>i</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>1</span>])</span> +</span>  // *x</span></span>
              tof1_readings</span>[</span>i</span>-</span>1</span>];</span>                    // +b</span></span>
      }</span></span>
      if</span> (</span>curr_dist </span><</span> 0</span>) {</span></span>
        tof1_readings</span>[</span>i</span>]</span> =</span> dist</span>;</span></span>
      }</span></span>
      else if</span> (</span>curr_dist </span>></span> 5000</span>){</span></span>
        tof1_readings</span>[</span>i</span>]</span> =</span> 4000</span>;</span></span>
      }</span></span>
      else</span> {</span></span>
        tof1_readings</span>[</span>i</span>]</span> =</span> curr_dist</span>;</span></span>
      }</span></span>
    }</span></span>
  }</span></span>
</span>
  // if there is a sensor reading</span></span>
  else</span> {</span></span>
    tof1_readings</span>[</span>i</span>]</span> =</span> distanceSensor2</span>.</span>getDistance</span>();</span></span>
    tof2_readings</span>[</span>i</span>]</span> =</span> tof1_readings</span>[</span>i</span>];</span></span>
    curr_dist </span>=</span>  tof1_readings</span>[</span>i</span>];</span></span>
    distanceSensor2</span>.</span>clearInterrupt</span>();</span></span>
    old_slope </span>=</span> (</span>curr_dist </span>-</span> old_tof</span>)</span>/</span>(</span>time_stamps</span>[</span>i</span>]</span>-</span>old_time</span>);</span></span>
    old_tof </span>=</span> curr_dist</span>;</span></span>
    old_time </span>=</span> time_stamps</span>[</span>i</span>];</span></span>
  }</span></span>
</span>
  // update errors and controller</span></span>
  float</span> e </span>=</span> curr_dist </span>-</span> dist</span>;</span></span>
  error_total </span>+=</span> e</span>;</span></span>
  error_change </span>=</span> e</span>-</span>last_error</span>;</span></span>
  int</span> motor_out </span>=</span> (</span>int</span>)(</span>Kp</span>*</span>e </span>+</span> Ki</span>*</span>error_total </span>+</span> Kd</span>*</span>error_change</span>);</span></span>
</span>
  p_data</span>[</span>i</span>]</span> =</span> Kp</span>*</span>e</span>;</span></span>
  i_data</span>[</span>i</span>]</span> =</span> Ki</span>*</span>error_total</span>;</span></span>
  d_data</span>[</span>i</span>]</span> =</span> Kd</span>*</span>error_change</span>;</span></span>
  motor_data</span>[</span>i</span>]</span> =</span> motor_out</span>;</span></span>
</span>
  // update motors (corrected for straight drive)</span></span>
  if</span> (</span>motor_out </span>></span> 0</span>){</span></span>
    forward</span>(</span>motor_out</span>-</span>3</span>,</span> motor_out</span>+</span>3</span>);</span>   </span></span>
  }</span></span>
  else</span> {</span></span>
    backward</span>(</span>-</span>motor_out</span>,</span> -</span>motor_out</span>);</span></span>
  }</span></span>
  last_error </span>=</span> e</span>;</span></span>
  </span></span>
  i</span>++</span>;</span></span>
  if</span> (</span>i </span>>=</span> TIME_ARR_SIZE</span>){</span></span>
    i </span>=</span> 0</span>;</span></span>
  }</span></span>
}</span></span></code></pre>Prelab: Data transfer</h2>
One of the major decisions of this lab way how to tranfer data. Transferring each piece of data in the control loop
would slow the loop down, resulting in slower updates and slower reaction time. Trying to store all the data is limited by the maximum storage of the Artemis. I decide to store all my values and transmit after PID has stopped. This results in a long waiting time after running my controller, but allows the code to loop fasting during the original run time. I allows the buffer to override the inital values if the controller tries to save more values than the set max buffer length. I parse this data in python lists like I have in previous labs and save them to a CSV for the future.</p>
case PID_STOP: </span>{</span></span>
  // Stop PID controller and the motors</span></span>
  pid_running </span>=</span> false</span>;</span></span>
  analogWrite</span>(</span>AB1_left</span>,</span>0</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_left</span>,</span>0</span>);</span></span>
  analogWrite</span>(</span>AB1_right</span>,</span>0</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_right</span>,</span>0</span>);</span></span>
</span>
  // tranfer the data written</span></span>
  for</span> (</span>int</span> j </span>=</span> 0</span>;</span> j </span><</span> i</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>p_data</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>i_data</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>d_data</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>motor_data</span>[</span>j</span>]);</span></span>
    tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span></span>
  }</span></span>
  </span></span>
  // Signal the data tranfer is done and it is safe to stop the listener</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>
  break</span>;</span></span>
}</span></span></code></pre>Range/Sampling time</h2>
I chose to swap my time of flight from short range mode to long range so that the max distance would be 4m instead of 1.5m. I originally decided to use short range mode since it takes far less time to be ready with a distance reading. However, we work on extrapolating values so that we can update the motors with an educated guess of our distance readings. This prevents the polling delay on the time of flight from being a limiting factor in the speed between motor updates. Since the delay becomes a non-issue, the benefits of long range mode outweigh the cons.</p>
With the original loop waiting for the time of flight to be ready, the average time between motor updates was 100.29 ms</strong>. I modified the loop to skip when the time of flight was not ready. This sped the loop up to and average of 2.4 ms</strong> between updates. Iterations that required processing a time of flight value took much longer, while iterations with no new time of flight reading typically ran very fast. Finally, when data extrapolation was added (as dsicussed below) the loop ran updating the motor values every 7.68 ms</strong> on average.</p>
Data Extrapolation</h3>
We use linear extrapolation to estimate the next time of flight value when one isn’t available. The way I decided to do this was to use the previous two valid time of flight readings to make a slope. I orginally used the last two readings to calcualte the slope (including the last two estimated readings). This was problematic since the estimated readings were often smaller than the real distaces since the bot tends to slow down as it approaches the wall. When an accurate ToF reading came in, the slope would become postive and the code would assume Meep was moving away from the wall rather than towards it. To find the next value, we simply use slope-intercept with the change in time and previous estimated distance.</p>

// Code to calculate slope when a new distance reading is available</span></span>
tof1_readings</span>[</span>i</span>]</span> =</span> distanceSensor2.</span>getDistance</span>();</span></span>
tof2_readings</span>[</span>i</span>]</span> =</span> tof1_readings</span>[</span>i</span>];</span></span>
curr_dist </span>=</span>  tof1_readings</span>[</span>i</span>];</span></span>
distanceSensor2.</span>clearInterrupt</span>();</span></span>
old_slope </span>=</span> (</span>curr_dist </span>-</span> old_tof</span>)</span>/</span>(</span>time_stamps</span>[</span>i</span>]</span>-</span>old_time</span>);</span></span>
old_tof </span>=</span> curr_dist</span>;</span></span>
old_time </span>=</span> time_stamps</span>[</span>i</span>];</span></span></code></pre>// using point slope to estimate when data isn't available</span></span>
curr_dist </span>=</span> (</span>old_slope</span>)</span> *</span>                    //  m</span></span>
      (</span>time_stamps</span>[</span>i</span>]</span> -</span> time_stamps</span>[</span>i</span>-</span>1</span>])</span> +</span>  // *x</span></span>
      tof1_readings</span>[</span>i</span>-</span>1</span>];</span>                    // +b</span></span>
</span>
// Sanity checks to make sure we aren't making up a crazy distance </span></span>
if</span> (</span>curr_dist </span><</span> 0</span>) {</span></span>
  tof1_readings</span>[</span>i</span>]</span> =</span> 0</span>;</span></span>
}</span></span>
else if</span> (</span>curr_dist </span>></span> 5000</span>){</span></span>
  tof1_readings</span>[</span>i</span>]</span> =</span> 4000</span>;</span></span>
}</span></span>
else</span> {</span></span>
  tof1_readings</span>[</span>i</span>]</span> =</span> curr_dist</span>;</span></span>
}</span></span></code></pre>Tuning process and results</h2>
I started with just a proportional controller. Since my max distance from the time of flight was 4000 mm and the setpoint was 304 mm, the max error was This resulted in some successfully runs, but the controller tended to overshoot and hit the wall and have steady state error in both the negative and positive direction. Here’s a video of the most successful run on the gain only controller.</p>
</iframe>
From there, I decided to add the integral term, since I wanted to see if PID was needed or if PI would work well. I was reminded that Ki should typically be a very small value as the cummlative error can get to a large number fairly quickly.</p>
</iframe>
Since I was experiencing a large amount of overshoot, I decided to add the derivative term and use a full PID controller. I restarted my tuning process, finding a Kp that resulted in the car being stable with steady state error (0.06). I added a small Ki to try to correct for this (0.0001). At this point, the car began to overshoot its set point, so I added Kd (1). I was able to successfully stop at the set point in these conditions.</p>
</iframe>


The PID controller should also be able to find the wall again when displaced. I pulled the car back to show that it was able to recorrect.</p>
</iframe>
Grad Task: Integrator Windup</h2>
As previously mentioned, the cummalitive error can get very large very quickly, especially when first starting the controller. Integrator windup occurs when the integrator term gets overpopulated very quickly when the setpoint is set.</p>
</iframe>
We can avoid this by limiting the max value of the total error. This prevents the error accumulation from blowing up, even if we keep the bot away from the set point for an extended period of time.</p>
if</span> (</span>error_total </span>></span> 2000</span>){</span></span>
  error_total </span>=</span> 2000</span>;</span></span>
}</span></span>
else if</span> (</span>error_total </span>< -</span>3000</span>){</span></span>
  error_total </span>= -</span>3000</span>;</span></span>
}</span></span></code></pre></iframe>
Collaboration and Sources</h2>
I referenced the lecture slides when setting up PID. I discussed ideas for code structure with Dyllan Hofflich.</p>


Lab 4: Motors and Open Loop Control
2026-03-02T00:00:00+00:00
Lab 4: Motors and Open Loop Control</h1>
In this lab, we work on controlling the robot, Meep, with open-loop pwm signals. We start by soldering the motor driver and testing the pwm signal generated via analogWrite</code>. Meep has two motors and operates on differential drive.</p>
Wire Set Up</h1>
Full Circuit Diagram</h2>
To start, this lab involves soldering the final componenets needed to assemble Meep. The circuit diagram below shows all the connections and GPIOs used by Meep. I chose to use the 850 mAh battery to power both the motors since I wanted to just have two batteries in the robot (one to power the Artemis and one to power both the motors). The 850 mAh battery should be able to handle the current draw without depleting too quickly.</p>

I assembled the bot as shown below. I placed the IMU on a flat service and secured it by drilling holes and putting in zipties (idea from Shao Stassen). The ToF is secured with orange tape in this photo, but will be secured with a piece of double sided tape in the near future. Since it is important the ToFs are facing straight, I made sure to secure them onto a flat surface. The Artemis faces with the bluetooth antenna facing outward in the hope of minizing EMI interference with the bluetooth. The motor drivers are placed on either wall on either side of the Artemis, keeping the wires short. I also expanded the hole that the battery wire comes through to store the Artemis battery with the motor battery on the other side of the bot.</p>


Motor drivers</h2>
I started by soldering by motor drivers.</p>

I decided to connect the motor drivers to A14, A15, A2, and A3. I did this for serval reasons:</p>


I wanted each motor drivers to be on analog pins since that would be the most compatiable with the analogWrite command used to generate the PWM signals.</p>
</li>

We needed to choose pins that can generate a pwm signal. By looking at the datasheet, we can see that pins with a tilde (~) can generate pwm signals.</p>
</li>
</ol>


Since the wires to connect to the motors are on opposite sides of the bot, I chose pins that were on opposites sides of the Artemis. I wanted the wires to avoid crossing eachother and be as short as possible to reduce possible electromagnetic interference (EMI).</li>
</ol>
PWM and Oscilloscope</h1>
Power Supply Settings</h2>
I set my power supply to 3.70 V since I planned to use the 850 mAH lipo which has a nominal voltage of 3.7 V. I set the max current to 2 A so that the driver could run without tripping the power supply. After soldering the motor drivers to the Artemis, I generated a PWM signal with analogWrite</code> and probed them as shown below.</p>
  #define</span> AB2_left</span> 2</span></span>
  #define</span> AB1_left</span> 3</span></span>
  #define</span> AB2_right</span> 14</span></span>
  #define</span> AB1_right</span> 15</span></span>
  Serial</span>.</span>begin</span>(</span>115200</span>);</span></span>
  pinMode</span>(</span>AB1_left</span>,</span>OUTPUT</span>);</span></span>
  pinMode</span>(</span>AB2_left</span>,</span>OUTPUT</span>);</span></span>
  pinMode</span>(</span>AB1_right</span>,</span>OUTPUT</span>);</span></span>
  pinMode</span>(</span>AB2_right</span>,</span>OUTPUT</span>);</span></span>
  analogWrite</span>(</span>AB1_left</span>,</span>0</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_left</span>,</span>60</span>);</span></span>
  analogWrite</span>(</span>AB1_right</span>,</span>60</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_right</span>,</span>0</span>);</span></span></code></pre></iframe>
PWM, or pulse width modulation, is frequently used to control motor speeds. The PWM has a duty cycle, or a percentage of time when the signal is set to high. By lowering the duty cycle, the average voltage that the motor sees decreases, causing the motors to have a lower rpm. An example of the readings for 25%, 33%, 50%, and 100% duty cycle are shown below.</p>
</iframe>
Testing Drive</h2>
I started with testing each side individually by connecting the driver I was testing to the power supply. Note that the code that is running is listed under the Power Supply Settings section.</p>
</iframe>
Then, I connected the VINs and Grounds together and ran both motors.</p>
</iframe>
Lowest PWM</h2>
To find the lowest PWM that Meep will move from rest with, I started with loop</code> code that would try to drive forward at a set PWM for 3 seconds, then increase the pulse width by one. The robot started moving in the 35-39 range. I tested individual pulse widths from there till the robot began to move at 37 or 14.5% duty cycle.</p>
</iframe>
void</span> loop</span>() {</span></span>
  forward</span>(</span>pwm</span>);</span> // Or left(pwm) depedning on which we want to control</span></span>
</span>
  // SEND ANYTHING ACROSS THE SERIAL PORT TO STOP THE BOT</span></span>
  if</span> (</span>Serial</span>.</span>available</span>()</span> ></span> 0</span>){</span></span>
    a </span>=</span> Serial</span>.</span>readString</span>();</span></span>
    Serial</span>.</span>println</span>(</span>"COMMAND TO WAIT SENT"</span>);</span></span>
    wait </span>=</span> 1</span>;</span></span>
    while</span>(</span>wait</span>){</span></span>
      if</span> (</span>!</span>Serial</span>.</span>available</span>()</span> ></span> 0</span>){</span></span>
        // Serial.println("Waiting");</span></span>
        stop</span>();</span></span>
      }</span></span>
      </span></span>
      // RESTART WITH NEW PWM WHEN SENT A NEW MESSAGE</span></span>
      else</span> {</span></span>
        Serial</span>.</span>println</span>(</span>"Reading"</span>);</span></span>
        pwm </span>=</span> Serial</span>.</span>parseInt</span>();</span></span>
        wait </span>=</span> 0</span>;</span></span>
        Serial</span>.</span>print</span>(</span>"PWM IS NOW: "</span>);</span></span>
        Serial</span>.</span>println</span>(</span>pwm</span>);</span></span>
      }</span></span>
    }</span></span>
    Serial</span>.</span>println</span>(</span>"COMMAND TO CONTINUE SENT"</span>);</span></span>
    while</span> (</span>Serial</span>.</span>available</span>()</span> ></span> 0</span>) {</span></span>
      Serial</span>.</span>read</span>();</span> // Clear Serial Input so we dont end up waiting </span></span>
    }</span></span>
  }</span></span>
}</span></span>
</span></code></pre>
For turning, the robot started moving at 93 with very jittery motion.</p>
</iframe>
We see that the turning is more smooth starting at 100. It is good to note that turning requires a higher duty cycle than simple forward motion since there is more friction working against turning the wheels than just moving forward/backward in the direction the wheels are already facing.</p>
</iframe>
Tuning the straight drive</h2>
To test how straight my robot drove, I ran both motors with 60 (23.5% duty cycle) for 6ft. Without calibration, Meep naturally drifted to the left.</p>
</iframe>
To fix this, I simply slowly lowered the right pwm so that the robot would stay on course. This fixed the straight drive as demonstrated in the video below.</p>
ble</span>.</span>send_command</span>(</span>CMD</span>.</span>FORWARD</span>,</span> "55|60"</span>)</span></span></code></pre>  case</span> FORWARD: </span>{</span></span>
      int</span> flspeed</span>,</span> frspeed</span>;</span></span>
</span>
      success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>flspeed</span>);</span></span>
      if</span> (</span>!</span>success</span>)</span></span>
          return</span>;</span></span>
</span>
      success </span>=</span> robot_cmd</span>.</span>get_next_value</span>(</span>frspeed</span>);</span></span>
      if</span> (</span>!</span>success</span>)</span></span>
          return</span>;</span></span>
          </span></span>
      analogWrite</span>(</span>AB1_left</span>,</span>0</span>);</span> </span></span>
      analogWrite</span>(</span>AB2_left</span>,</span>flspeed</span>);</span></span>
      analogWrite</span>(</span>AB2_right</span>,</span>0</span>);</span></span>
      analogWrite</span>(</span>AB1_right</span>,</span>frspeed</span>);</span> </span></span>
      </span></span>
      // Added so I can record the straight drive without the robot running away</span></span>
      delay</span>(</span>4000</span>);</span></span>
      analogWrite</span>(</span>AB1_left</span>,</span>0</span>);</span> </span></span>
      analogWrite</span>(</span>AB2_left</span>,</span>0</span>);</span></span>
      analogWrite</span>(</span>AB1_right</span>,</span>0</span>);</span> </span></span>
      analogWrite</span>(</span>AB2_right</span>,</span>0</span>);</span></span>
      //////////////////////////////////////</span></span>
      </span></span>
      break</span>;</span></span>
  }</span></span></code></pre></iframe>
Open Loop Robot Control</h2>
I ran open loop control with the robot to try to get it to head towards a wall and turn before it hit it. I did this with manual motion commands. This would be much easier to do with closed loop control. Meep would be able to use the time of flight sensor to tell when to turn and use the IMU to determine how much to turn by. We will be exploring this concept in the next lap.</p>
</iframe>
Grad Tasks</h1>
5000: Analog Write</h2>
We can see from the PWM oscilloscope reading that period of the pwm signal is 5.460 ms. Using this, we can calculate the frquency to be 1/0.005460 or 183.15 Hz.</p>

void</span> forward</span>(</span>int</span> ls</span>,</span> int</span> rs</span>) {</span></span>
  analogWrite</span>(</span>AB1_left</span>,</span>ls</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_left</span>,</span>0</span>);</span></span>
  analogWrite</span>(</span>AB1_right</span>,</span>0</span>);</span> </span></span>
  analogWrite</span>(</span>AB2_right</span>,</span>rs</span>);</span></span>
}</span></span>
</span>
void</span> loop</span>(){</span></span>
  for</span> (</span>i </span>=</span> 1</span>;</span> i </span><</span>5</span>;</span> i</span>++</span>){</span></span>
    forward</span>(</span>255</span>/</span>i</span>,</span> 255</span>/</span>i</span>);</span></span>
    delay</span>(</span>1000</span>);</span></span>
  }</span></span>
}</span></span></code></pre>5000: Lowest PWM with Motion</h2>
I found the lowest PWM that kept the bot in motion by lowering the PWM one by one until the bot stopped movement. The bot stopped moving at 44 which is odd since this is higher than the pwm needed to get it to move from rest. Upon rerunning my moving the vehicle from rest, I got a similar result as prior. I would expect this PWM to be lower since it takes less force to keep the bot in motion when it already has inertia in the direction we are trying to move.</p>
</iframe>
Collaboration and Sources</h2>
I collobrated with Dyllan and Shao on bot building ideas. I referenced Aiden Derocher’s</a> and Tyler Wisniewski’s</a> lab reports when building my robot.</p>


Lab 3: ToF
2026-02-24T00:00:00+00:00
Lab 3: Time of Flight Sensors</h1>
This lab explores adding two time of flight sensors to an RC car (Meep). We explore how to handle I2C communication when devices share the same address, the pros and cons of different configuration modes, and the various sensitivities of infared sensors.</p>
Time of Flight Setup</h2>
Vehicle Planning</h3>
First, we need to chose where we will be placing our ToF sensors on the car (Meep). I chose to place one sensor in the front and one sensor on the right of the car. This will allow Meep to see if there is something in front of it and avoid a collision. At the same time, having a sensor to the right gives potenital for Meep to follow a wall to help it determine if it is drifting in any direction. This also give for potential for maze naviagtion in the future.</p>
The disadvantages of this placement are the sensor will be on the back if the robot flips. For mapping our environment in the future, we would have to spin 270 degrees to get a complete mapping. Meep will also not be able to follow or see obstacles on its left.</p>

Soldering and initial testing</h3>
We start by soldering the ToF pins to a QWiic connect cable for easy use. As seen in the digram above, the 4 pins of the qwiic connect are soldered to their corresponding pins. Note that red is power, black is ground, blue is SDA (I2C data wire), and yellow is SCL (I2C clock wire).</p>


By running the Apollo Example05_wire_I2C demo code, we can see the I2C addresses of our devices. The I2C address is the memory location where sensor data will be written on our Artemis as determined by the sensor manufacturer.</p>

We can see that the IMU is on 0x69 and that the ToF is on 0x029. An important note is that both ToFs are on the same I2C address by default. This means that the information from the time of flights will conflict and likely corrupt eachother if we allow both to try to write at the same time. We fix this by using the XSHUT pin and the setI2CAddress</code> function. The XSHUT pin allows us to turn on and off the ToF sensors. By keeping one of the ToF sensors off, we can change the I2C address of the other sensor without conflict. We change the I2C address of the second sensor to 0x30 since we see that it is not in use. This allows us to have both sensors on the same I2C bus without conflict.</p>
// Create Objects for both ToF sensors</span></span>
SFEVL53L1X</span> distanceSensor1</span>(</span>Wire</span>,</span> SHUTDOWN_PIN</span>);</span></span>
SFEVL53L1X distanceSensor2</span>;</span></span>
</span>
pinMode</span>(</span>SHUTDOWN_PIN</span>,</span> OUTPUT</span>);</span> </span></span>
digitalWrite</span>(</span>SHUTDOWN_PIN</span>,</span> LOW</span>);</span> // shut down the sensor so we can change its I2C address</span></span>
distanceSensor1</span>.</span>setI2CAddress</span>(</span>0x</span>30</span>);</span> // Set an address that won't conflict</span></span>
digitalWrite</span>(</span>SHUTDOWN_PIN</span>,</span> HIGH</span>);</span> // reboot sensor with new address</span></span></code></pre></iframe>

For futher testing, I taped both ToFs to a box so that they would be in the same position and facing the same direction. This way, we can compare the data from both sensors to see if they are consistent with eachother.</p>

Distance Modes</h2>
Next, we explore the different distance modes of the ToF sensor. The ToF sensor has three different distance modes: short, medium, and long. Each mode has different accuracy and execution speed. I speficially chose to compare the short and long distance modes. The short distance mode has a range of 0.1m to 1.2m and an accuracy of +/- 1mm. The long distance mode has a range of 0.1m to 4m and an accuracy of +/- 3mm. The short distance mode has an execution time of 20ms while the long distance mode has an execution time of 50ms.</p>
I experiemented with this by setting one sensor to short distance mode and the other sensor to long distance mode. I then moved the sensors backwards slowly and recorded the distance data from both sensors.</p>

As shown, the sensors are very accurate in their respective ranges. The short distance mode is more accurate at close distances while the long distance mode is more accurate at longer distances. As soon as the range is exceeded, the sensors become noisy before spitting out erroneous data. This could be a problem in the future since these erroneous data look like valid data so Meep might think it is close to or colliding with an object when it is not.</p>
The long distance mode is also much slower than the short distance mode. This would be a problem for Meep since it needs to be able to react quickly to its environment.</p>

With this in mind, we expect Meep to only need to see objects that are close to it in order to avoid collisions. Meep can also drive close to walls for mapping in the future. With this in mind, I will be using the short distance mode for both sensors.</p>
Accuracy</h2>
Using both sensors, we can determine how accurate the sensors are in short distance mode. I measured the distance from the sensor to a wall using a tape measure and compared it to the distance reported by the sensor. I did this for distances of 0.2m, 0.5m, 0.75m, 1m, and 1.51m. We can see that the sensors output different values for the same distance. This is likely due to a difference in facing angle of the sensor to the wall. Overall, we see the sensors are very accurate with a maximum percent error of 8.57% for sensor 2 at 0.2m. Sensor 1 is more accurate than sensor 2 at all distances. This could be due to a difference in the quality of the sensors or a difference in the facing angle of the sensors to the wall.</p>

  
    
      Real Value (mm)</th>
      Sensor 1 (mm)</th>
      Sensor 2 (mm)</th>
      Difference</th>
      Percent Error 1</th>
      Percent Error 2</th>
    </tr>
  </thead>
  

    
      200.0</td>
      193.51</td>
      217.14</td>
      -23.63</td>
      -0.0325</td>
      0.0857</td>
    </tr>
    

      500.0</td>
      494.00</td>
      522.64</td>
      -28.64</td>
      -0.0120</td>
      0.04528</td>
    </tr>
    

      750.0</td>
      747.37</td>
      772.05</td>
      -24.68</td>
      -0.0035</td>
      0.0294</td>
    </tr>
    

      1000.0</td>
      994.37</td>
      1024.96</td>
      -30.59</td>
      -0.0056</td>
      0.02496</td>
    </tr>
    

      1510.0</td>
      1517.00</td>
      1548.50</td>
      -31.50</td>
      0.0046</td>
      0.0255</td>
    </tr>
  </tbody>
</table>


Execution Time</h2>
Next, I attempted to see how quickly I could get data from one of the sensors to print to the Serial Monitor. I decided to have the sensor print data as fast as possible and using the built in time stamps on the serial monitor to determine the execution time so that recoding the time would not slow it down. The average time between serial prints was 3.87 ms.</p>

void</span> loop</span>(</span>void</span>)</span></span>
{</span></span>
  if</span> (</span>distanceSensor1</span>.</span>checkForDataReady</span>())</span></span>
  {</span></span>
    distance </span>=</span> distanceSensor1</span>.</span>getDistance</span>();</span></span>
</span>
    Serial</span>.</span>print</span>(</span>"TOF1: "</span>);</span></span>
    Serial</span>.</span>print</span>(</span>distance</span>);</span></span>
    Serial</span>.</span>println</span>();</span></span>
  }</span></span>
}</span></span></code></pre>Integration with IMU and Bluetooth</h2>
Finally, I integrated the ToF sensors with the IMU and Bluetooth. I created the command SENSOR_START</code> for the Artemis to begin saving and the transmitting the data from the sensors. I then used the SENSOR_STOP</code> command to stop transmitting data. I demonstrate this by sldiing a box with the sensors taped on back and forth in front of a wall. The data is read and saved by a notification handler (see previous labs for more details on this).</p>
The average time bewtween data points was 6.41 ms.</p>


// This function is called in loop when the SENSOR_START command has been sent</span></span>
void</span> transmit_sensor_data</span>(){</span></span>
    int</span> dropped_data </span>=</span> 0</span>;</span></span>
    int</span> j </span>=</span> 0</span>;</span></span>
    time_stamps</span>[</span>0</span>]</span> =</span> (</span>int</span>)</span> millis</span>();</span></span>
</span>
    for</span> (</span>i </span>=</span> 0</span>;</span> i </span><</span> TIME_ARR_SIZE</span>;</span> i</span>++</span>) {</span></span>
        if</span> (</span>myICM</span>.</span>dataReady</span>()) {</span></span>
          myICM</span>.</span>getAGMT</span>();</span></span>
          time_stamps</span>[</span>i</span>]</span> =</span> (</span>int</span>)</span> millis</span>();</span></span>
          roll_readings</span>[</span>i</span>]</span> =</span> roll_calc_accel</span>(</span>&</span>myICM</span>);</span></span>
          pitch_readings</span>[</span>i</span>]</span> =</span> pitch_calc_accel</span>(</span>&</span>myICM</span>);</span></span>
          gyro_roll_readings</span>[</span>i</span>]</span> =</span> myICM</span>.</span>gyrX</span>();</span></span>
          gyro_pitch_readings</span>[</span>i</span>]</span> =</span> myICM</span>.</span>gyrY</span>();</span></span>
          gyro_pitch_readings</span>[</span>i</span>]</span> =</span> myICM</span>.</span>gyrY</span>();</span></span>
          gyro_yaw_readings</span>[</span>i</span>]</span> =</span> myICM</span>.</span>gyrZ</span>();</span></span>
        }</span></span>
        else</span>{</span></span>
          dropped_data</span>++</span>;</span></span>
        }</span></span>
        if</span> (</span>distanceSensor2</span>.</span>checkForDataReady</span>()) {</span></span>
          tof2_readings</span>[</span>i</span>]</span> =</span> distanceSensor2</span>.</span>getDistance</span>();</span></span>
        }</span></span>
        else</span>{</span></span>
          tof2_readings</span>[</span>i</span>]</span> = -</span>100</span>;</span></span>
          dropped_data</span>++</span>;</span></span>
        }</span></span>
        if</span> (</span>distanceSensor1</span>.</span>checkForDataReady</span>()) {</span></span>
          tof1_readings</span>[</span>i</span>]</span> =</span> distanceSensor1</span>.</span>getDistance</span>();</span></span>
        }</span></span>
        else</span>{</span></span>
          tof1_readings</span>[</span>i</span>]</span> = -</span>100</span>;</span></span>
          dropped_data</span>++</span>;</span></span>
        }</span></span>
        read_data</span>();</span> // check if the SENSOR_STOP command is sent</span></span>
        if</span> (</span>!</span>sensor_trans</span>){</span></span>
          break</span>;</span></span>
        }</span></span>
    }</span>         </span></span>
    // Transmit the data when the buffer is full or when the SENSOR_STOP command is sent  </span></span>
    for</span> (</span>j </span>=</span> 0</span>;</span> j </span><</span> i</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>roll_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>pitch_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_roll_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_pitch_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>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>dropped_data</span>);</span></span>
        tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span></span>
    }</span>       </span></span>
}</span></span></code></pre>
5000: IMU Color and Texture Sensitivity</h2>
I tested the IMU’s sensitivity to different colors and textures by placing different materials in front of the IMU at 200 mm and taking the average of the measureuments for a set period of time. I found that the IMU was sensitive to different materials but not overly so. I noticed the most discrepancy with the glass door which was expected since the most light passes through it. Suprisingly, the black garbage bag and the black expomarker also performed poorly. This could indicate a sensitivity to black or a sensitivity to the texture of the materials.</p>

  
    
      Real Value (mm)</th>
      Surface</th>
      Sensor 1 (mm)</th>
      Sensor 2 (mm)</th>
      Difference</th>
      Percent Error 1</th>
      Percent Error 2</th>
    </tr>
  </thead>
  

    
      200.0</td>
      
      White door</td>
      193.51</td>
      217.14</td>
      -23.63</td>
      -0.03245</td>
      0.0857</td>
    </tr>
    

      200.0</td>
      Paper</td>
      184.36</td>
      200.53</td>
      -16.17</td>
      -0.0782</td>
      0.00265</td>
    </tr>
    

      200.0</td>
      White Wall</td>
      193.46</td>
      207.47</td>
      -14.01</td>
      -0.0327</td>
      0.03735</td>
    </tr>
    

      200.0</td>
      Whiteboard</td>
      178.97</td>
      194.64</td>
      -15.67</td>
      -0.10515</td>
      -0.0268</td>
    </tr>
    

      200.0</td>
      Expomarker</td>
      167.77</td>
      191.2</td>
      -23.43</td>
      -0.16115</td>
      -0.044</td>
    </tr>
    

      200.0</td>
      Orange Tape</td>
      178.89</td>
      197.8</td>
      -18.91</td>
      -0.10555</td>
      -0.011</td>
    </tr>
    

      200.0</td>
      Green Microfiber</td>
      183.18</td>
      203.14</td>
      -19.96</td>
      -0.0841</td>
      0.0157</td>
    </tr>
    

      200.0</td>
      Pink Polyester</td>
      175.82</td>
      199.84</td>
      -24.02</td>
      -0.1209</td>
      -0.0008</td>
    </tr>
    

      200.0</td>
      Glass Door</td>
      118.57</td>
      188.69</td>
      -70.12</td>
      -0.40715</td>
      -0.05655</td>
    </tr>
    

      200.0</td>
      Garbage Bag</td>
      169.3</td>
      190.86</td>
      -21.56</td>
      -0.1535</td>
      -0.0457</td>
    </tr>
  </tbody>
</table>
5000: Other Infared Sensors</h2>
There are two main types of infared distance sensors. The first is like the time of flight we are using, which measures distance based on the time it takes for a light signal to be recieved back after it is emitted. The second type is a triangulation sensor, which measures distance based on the angle of the reflected light signal. The triangulation sensor is more accurate at close distances but time of flight sensors have a longer range. Time of flights also tend to be faster than triangulation sensors. This makes time of flights better for applications where you need to see objects that are farther away or need to react quickly to your environment. Triangulation sensors seem to be best for when you need a very precise measurement of distance at close range.</p>
Collaboration and Sources</h2>
I worked with Shao Stassen and Dyllan Hofflich when soldering the ToF sensors. I worked with Shao when gathering data for accuracy. Aiden Derocher’s lab 3 report was also referenced.</p>
Short vs Long Distance Mode information</a></p>
Infared Sensor Types</a></p>


Lab 2: IMU
2026-02-10T00:00:00+00:00
Lab Tasks</h1>
This lab explores an Inertial Measurement Unit (IMU), specifically the Sparkfun breakout board</a>. This board has an accelerometer, gyrocrope, and magnetometer on the boards X, Y, and Z axis. This lab explores use of the accelerometer and gyroscope, noise analysis for low pass filtering, and using a complementary filter for optimal angle estimation.</p>
IMU Set Up</h2>
The IMU is connected to the Artemis with a Qwiic connect able with 4 wires: Power (3.3 V), Ground, Clock (SCL), and Data (SDA).</p>


AD0_VAL and I2C</h2>
I2C commincation is used fairly often with sensors and other devices that need to write data to an area where it can be read by a microcontroller. I2C uses a clock to synchronize data reads from a specified memory location on the IMU. The IMU will transmit data on the SDA line synchronized with SCL.</p>
The IMU example code itself describes the AD0_VAL</code> as "the value of the last bit of the I2C address"</code>. This value defaults as 1 but can be changed to 0 to allow multiple IMUs (or I2C devices in general) to attach to different I2C commincation addresses.</p>
IMU Example Code</h2>
We run the IMU example code. I could see the acceleration raw readings range from about -1000 to 1000 as the board was spun 360 degrees on all its axis.</p>
</iframe>
Accelerometer</h1>
Calculating and Calibrating Roll and Pitch</h2>
The acceleraometer reads acceleration, or force per mass, being applied to the IMU. When the IMU is stationary, the only force applied to it is gravity. We can use 2 of the raw acceleromter readings to determine the angle of the board for both pitch and roll. In the case of roll, as shown below, we can use the obervation that $a_z = g*\cos(\theta)$ and that $a_x = g*\sin(\theta)$ to determine that the angle $\theta = tan^{-1}(a_x/a_z)$. Since we want $\theta$ to be the correct sign, we use the function $atan2$ to get an angle in radians between $-\pi/2$ and $\pi/2$. The same logic applies to finding the pitch, where $\phi = atan2(a_y, a_z)$. We use these equations with the accelerometer readings to determine roll and pitch.</p>


#ifdef</span> USE_SPI</span></span>
float</span> roll_calc_accel</span>(</span>ICM_20948_SPI </span>*</span>sensor</span>) {</span></span>
#else</span></span>
float</span> roll_calc_accel</span>(</span>ICM_20948_I2C </span>*</span>sensor</span>) {</span></span>
#endif</span></span>
  return</span> atan2</span>(</span>sensor</span>-></span>accX</span>(),</span> sensor</span>-></span>accZ</span>())</span> /</span> M_PI </span>*</span> 180.0</span>;</span> </span></span>
}</span></span>
</span>
</span>
#ifdef</span> USE_SPI</span></span>
float</span> pitch_calc_accel</span>(</span>ICM_20948_SPI </span>*</span>sensor</span>) {</span></span>
#else</span></span>
float</span> pitch_calc_accel</span>(</span>ICM_20948_I2C </span>*</span>sensor</span>) {</span></span>
#endif</span></span>
  return</span> atan2</span>(</span>sensor</span>-></span>accY</span>(),</span> sensor</span>-></span>accZ</span>())</span> /</span> M_PI </span>*</span> 180.0</span>;</span></span>
}</span></span></code></pre>
To callibrate, I started by observing the IMU roll and pitch readings for -90°, 0°, and 90°. I used the edge of the lab table as reference for ground truth degree measurements. I observed that the accelerometer readings were about one degree off of expected. I chose to calibrate the IMU by shifting based on the 0° output, then scaling the values based on the -90° and 90° readings. For example, for the pitch, I added 1° to shift 0 into the right location, then scaled by $\frac{180}{91.1-(-89.1)} \approx .999$. My calibration equations are as follows:</p>
Roll: $\phi_{actual} = 1.01294(\phi_{measured}+0.21)$</p>
Pitch: $\theta_{actual} = 0.998890(\theta_{measured}+1.0)$</p>
















Data Speed and Accuracy</h2>
Overall, the IMU is able to output accelermoter values very quickly and decently accurately. Raw acceleromter values are constantly changing and very noise. Even slight bumps in the table are detected with extreme changed in the accelerometer readings. An easy way to fix this is to low pass the accelerometer to filter out the noise. We start by observing stationary accelerometer readings in the frequency domain. We apply a fast fourier transformation to transform the data into the frequency domain. I attempted to sample 2048 accelerometer readings at a semi constant rate which ended up being about 1329 microseconds between data reads.</p>




f</span> =</span> np</span>.</span>linspace</span>(</span>0</span>,</span> fs</span>/</span>2</span>,</span> int</span>(</span>N</span>/</span>2</span>),</span> endpoint</span>=</span>False</span>)</span> </span></span>
roll_fft</span> =</span> fft</span>(</span>rolls</span>)</span></span>
pitch_fft</span> =</span> fft</span>(</span>pitches</span>)</span></span>
roll_y</span> =</span> 2.0</span> /</span> N</span> *</span> np</span>.</span>abs</span>(</span>roll_fft</span>[:</span>N</span>//</span>2</span>])</span></span>
pitch_y</span> =</span> 2.0</span> /</span> N</span> *</span> np</span>.</span>abs</span>(</span>pitch_fft</span>[:</span>N</span>//</span>2</span>])</span></span></code></pre>
I noticed that all data seemed to be contained within 10 Hz so I chose to make this my cutoff. Sticking with the SciPy library, I used the built in butter</code> and filtfilt</code> functions to low pass the accelerometer values. This showed accelerometer readings that were far more resistant to noise as shown in the graph below.</p>




Gyroscope</h1>
Raw Gyroscope Readings</h2>
A gyroscope measures angular velocity. By updating out angle estimate based on time, the gyroscope readings, and the previous angle, we can accurately estimate the angle with far less noise. To estimate angle $\theta$ with the gyroscope, we can use the following equation $\theta_t = \theta_{t-1} + $ gyro_reading $ * dt$. This updates the angle based on the change between the last angle and the current. We can observe that when the imu is stable, we see little to no fluctuation in the gyroscope angle estimates. However, a major issue with relying only on the gyroscope is the value drifting from reality. Since we are estimating the position through the deriviative, we can easily stray from reality as the same estimates in our error compound.</p>





Here’s an example of gyroscope readings as I spin the IMU 360 degrees on the yaw axis.</p>
</iframe>

Sample Time</h2>
One factor in gyroscope angle estimation is the time between samples. If we have a long time between samples, we can easily drift from reality since we are estimating the angle based on the change in angle. If we have a short time between samples, we can get more accurate readings since we are updating our angle estimate more frequently. I attempted to observe the difference between a short and long sample time. I noticed that quick movements could not be picked up if the gyroscope is already stationary when sampled again, leading to larger drift.</p>


Complementary Filter Readings</h2>
The solution to account for both the noise in the accelerometer and the drift in the gyroscope is to use a complementary filter. A complementary filter is simply a way of combinding the output for both the accelerometer and the gyrpscope to get non-noisy and accurate angle readings. We do this by taking the weighted average of the accelerometer and gyroscope angle estimates. I chose to take 99% of the gyroscopes reading since it would update quickly and not be noisy. I used 1% of the angle to be the acceleromter’s reading to correct any drift introduced by the gyroscope. We can see in the graphs below that the complementary filter greatly improved both angle drift and resistanct to noise for the IMU readings. Light vibrations of the table no longer cause the angle to fluctuate and the gyroscope’s drift is adjusted for (as demonstrated clearly in the pitch graph below).</p>




fs</span> =</span> 1</span>/</span>2827e-6</span></span>
cutoff_freq</span> =</span> 10</span> </span></span>
Wn</span> =</span> cutoff_freq</span> /</span> (</span>fs</span> /</span> 2</span>)</span></span>
b</span>,</span> a</span> =</span> butter</span>(</span>N</span>=</span>4</span>,</span> Wn</span>=</span>Wn</span>,</span> btype</span>=</span>'low'</span>)</span></span>
comp_accel_roll_filtered</span> =</span> filtfilt</span>(</span>b</span>,</span> a</span>,</span> comp_acc_rolls</span>)</span> # filter the acceleration roll</span></span>
comp_accel_pitch_filtered</span> =</span> filtfilt</span>(</span>b</span>,</span> a</span>,</span> comp_acc_pitches</span>)</span> # filter the acceleration pitch</span></span>
comp_roll</span> =</span> np</span>.</span>zeros</span>(</span>2048</span>)</span></span>
comp_pitch</span> =</span> np</span>.</span>zeros</span>(</span>2048</span>)</span></span>
comp_roll</span>[</span>0</span>]</span> =</span> comp_accel_roll_filtered</span>[</span>0</span>]</span></span>
comp_pitch</span>[</span>0</span>]</span> =</span> comp_accel_pitch_filtered</span>[</span>0</span>]</span></span>
</span>
# calculate the complementary filter angles</span></span>
for</span> i</span> in</span> range</span>(</span>1</span>,</span> 2048</span>):</span></span>
    dt</span> =</span> (</span>comp_times</span>[</span>i</span>]</span> -</span> comp_times</span>[</span>i</span>-</span>1</span>])</span> *</span> 0.000001</span></span>
    comp_roll</span>[</span>i</span>]</span> =</span> (</span>comp_roll</span>[</span>i</span>-</span>1</span>]</span>+</span> comp_gyr_rolls</span>[</span>i</span>]</span>*</span>dt</span>)</span>*</span>(</span>0.99</span>)</span> +</span> comp_acc_rolls</span>[</span>i</span>]</span>*</span>0.01</span></span>
    comp_pitch</span>[</span>i</span>]</span> =</span> (</span>comp_pitch</span>[</span>i</span>-</span>1</span>]</span>+</span> comp_gyr_pitches</span>[</span>i</span>]</span>*</span>dt</span>)</span>*</span>(</span>0.99</span>)</span> +</span> comp_acc_pitches</span>[</span>i</span>]</span>*</span>0.01</span></span></code></pre>
One design choice I made was to do the complementary filter math in python rather than on the Artemis. I chose to do this because I wanted to transmit IMU data as quickly as possible and I wanted to be able to low pass and calibrate the accelerometer readings before passing them into the complementary filter.</p>
Sample Data</h1>
To continously sample data, I created the commands IMU_START</code> and IMU_STOP</code>. The IMU will record and send out data in batches until it receives the stop command. I chose a batch size of 10 to minimize latency while transmitting mostly up to data readings. This resulted in a sampling rate of about 48.6 milliseconds between data points.</p>
</iframe>


// START STORING VALUES. TRANSMIT WHEN THE BUFFER IS FULL</span></span>
void</span> transmit_imu_data</span>(){</span></span>
    int</span> dropped_data </span>=</span> 0</span>;</span></span>
    int</span> j </span>=</span> 0</span>;</span></span>
    time_stamps</span>[</span>0</span>]</span> =</span> (</span>int</span>)</span> millis</span>();</span></span>
</span>
    for</span> (</span>i </span>=</span> 0</span>;</span> i </span><</span> TIME_ARR_SIZE</span>;</span> i</span>++</span>) {</span></span>
        if</span> (</span>myICM</span>.</span>dataReady</span>()) {</span></span>
          myICM</span>.</span>getAGMT</span>();</span></span>
          time_stamps</span>[</span>i</span>]</span> =</span> (</span>int</span>)</span> millis</span>();</span></span>
          roll_readings</span>[</span>i</span>]</span> =</span> roll_calc_accel</span>(</span>&</span>myICM</span>);</span></span>
          pitch_readings</span>[</span>i</span>]</span> =</span> pitch_calc_accel</span>(</span>&</span>myICM</span>);</span></span>
          gyro_roll_readings</span>[</span>i</span>]</span> =</span> myICM</span>.</span>gyrX</span>();</span></span>
          gyro_pitch_readings</span>[</span>i</span>]</span> =</span> myICM</span>.</span>gyrY</span>();</span></span>
          gyro_pitch_readings</span>[</span>i</span>]</span> =</span> myICM</span>.</span>gyrY</span>();</span></span>
          gyro_yaw_readings</span>[</span>i</span>]</span> =</span> myICM</span>.</span>gyrZ</span>();</span></span>
        }</span></span>
        else</span>{</span></span>
          dropped_data</span>++</span>;</span></span>
        }</span></span>
        read_data</span>();</span></span>
        if</span> (</span>!</span>imu_trans</span>){</span></span>
          break</span>;</span></span>
        }</span></span>
    }</span>         </span></span>
      </span></span>
    // Serial.println("SENDING IMU DATA");</span></span>
</span>
    for</span> (</span>j </span>=</span> 0</span>;</span> j </span><</span> i</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>roll_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>pitch_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_roll_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_pitch_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>dropped_data</span>);</span></span>
        tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span></span>
    }</span>       </span></span>
</span>
    // Serial.println("COMP DATA TRANFER Complete");</span></span>
}</span></span></code></pre>Stunt</h1>
After playing around with my RC car, I observed the car’s natural drifting, turning speed/accuracy, and how easy it was to flip the bot over (either by driving into a wall or rapidally switching directions). I noted that telling the bot to drive ‘straight’ resulted in the car taking a pretty heavy curve. It was pretty easy to get the car to only have 1 or 2 wheel touching the ground causing a straight command to result in the car spinning in a circle (as shown in the video).</p>
</iframe>
Shao and I also (accidently) observed that the RC car can drive on its side. I’m not sure what to do with this information at this time, but it could be used for a stunt in the future. Driving on the side seemed much more controlled and better for slower motion. (Please ignore my cackling as I found this driving method to be quite funny).</p>
</iframe>
Collaboration</h1>
This lab was worked on with consultation from Shao Stassen. We worked together more closely for the Stunt section. Aidan McNay’s student page was also consulted.</p>
Resources</h2>
Qwiic Connect Guide</a></p>
ArduinoDocs I2C Guide</a></p>
Alphabold FFT Tutorial</a></p>
SciPy Butter</a></p>
SciPy Filtfilt</a></p>


Lab 1: Artemis and Bluetooth
2026-01-23T00:00:00+00:00

This lab introduces programming the RedBoard Artemis Nano and explores communicating with the board via Bluetooth.</p>
Lab 1A</h2>
The first part of this lab demostrates how to program the RedBoard Artemis Nano and explores some of the features via demo code files.</p>
Basic Setup</h3>
To start this lab, the Artemis Redboard Nano is connected to a laptop. We use the Arduino IDE to program it.</p>
Running Blink</h3>
Like with most microntroller projects, we start with making the onboard led blink. The demo code initializes the built-in led as the output, then repeated turns it on and off with a second delay in between each action.</p>
</iframe>
Running Serial</h3>
Next, we run the Serial demo code to demonstate communication with the Artemis via the serial port. The serial monitor allows us to send and recieve messages when the Artemis is connected to a serial port on our laptop. The demo code takes a message you sent and echos it back as demontrated in the video below.</p>
</iframe>
Testing the Temperature Sensor</h3>
The “analog read” demo code allows us to read the output of the Artemis builtin temperature sensor.</p>

Testing the Microphone</h3>
Finally, the Microphone Ouptut demo allows use to see the loudest frequency detected by the onboard microphone. You can see the result of my high pitch whisper screaming into the microphone below.</p>

Grad Task: Frequency Detector</h3>
The grad task for this assignment builds on the Microphone and Serial demos to create a electronic tuner that can detect 3 different frequencies. I chose to detect the musical notes A, C and F. I did this by playing the middle pitches of these notes (A4, C4, and F4) and setting the standard to the output of the program. To detect the pitch, I tranform the heard pitch to be in the correct octave (by multiplying or dividing by 2), then find the pitch it is closest to and print that as the note.</p>
void</span> pitch_analysis</span>(</span>uint32_t</span> real_pitch</span>) {</span></span>
</span>
  // standard frequencies to match </span></span>
  uint32_t</span> freq_A </span>=</span> 434</span>;</span></span>
  uint32_t</span> freq_C </span>=</span> 263</span>;</span></span>
  uint32_t</span> freq_F </span>=</span> 343</span>;</span></span>
</span>
  uint32_t</span> pitch </span>=</span> real_pitch</span>;</span></span>
</span>
  // We normalize the pitch to be in the middle octave</span></span>
  while</span> (</span>pitch </span><</span> 256</span>) {</span></span>
    pitch </span>*=</span> 2</span>;</span></span>
  }</span></span>
</span>
  while</span> (</span>pitch </span>></span> 512</span>){</span></span>
    pitch </span>/=</span> 2</span>;</span></span>
  }</span></span>
</span>
  uint32_t</span> diff_A </span>=</span> (</span>uint32_t</span>)</span> abs</span>((</span>int</span>)</span> pitch </span>-</span> (</span>int</span>)</span> freq_A</span>);</span></span>
  uint32_t</span> diff_C </span>=</span> (</span>uint32_t</span>)</span> abs</span>((</span>int</span>)</span> pitch </span>-</span> (</span>int</span>)</span> freq_C</span>);</span></span>
  uint32_t</span> diff_F </span>=</span> (</span>uint32_t</span>)</span> abs</span>((</span>int</span>)</span> pitch </span>-</span> (</span>int</span>)</span> freq_F</span>);</span></span>
</span>
  // We choose the closest pitch and print it</span></span>
  if</span> (</span>diff_A </span><</span> diff_C</span>) {</span></span>
    if</span> (</span>diff_F </span><</span> diff_A</span>) {</span></span>
      Serial</span>.</span>printf</span>(</span>"THE NOTE IS AN F (%d)         </span>\n</span>"</span>,</span> real_pitch</span>);</span></span>
    }</span></span>
    else</span> {</span></span>
      Serial</span>.</span>printf</span>(</span>"THE NOTE IS AN A (%d)         </span>\n</span>"</span>,</span> real_pitch</span>);</span></span>
    }</span></span>
  }</span></span>
  else if</span> (</span>diff_F </span><</span> diff_C</span>) {</span></span>
    Serial</span>.</span>printf</span>(</span>"THE NOTE IS AN F (%d)         </span>\n</span>"</span>,</span> real_pitch</span>);</span></span>
  }</span>  </span></span>
  else</span> {</span></span>
    Serial</span>.</span>printf</span>(</span>"THE NOTE IS A C (%d)         </span>\n</span>"</span>,</span> real_pitch</span>);</span></span>
  }</span></span>
}</span></span></code></pre></iframe>
Lab 1B</h2>
The second part of this lab works on communicated with the Artemis via bluetooth. Since we won’t have a serial connection with the board when it is in a robot, our primary communication method will be Bluetooth. In this lab, we experiement with thie bluetooth connection and create commands to use for communication.</p>
Prelab</h3>
We set up bluetooth connection with the Artemis board. The board has a unique MAC address that we search for to create a bluetooth connection to our laptop. We also generate a unique UUID to prevent any unplanned connections. Through the bluetooth library, we are able to communicate in many ways, as explored in the lab below.</p>

ECHO Command</h3>
The ECHO</code> command takes in a string s</code> and prints “Robot says: s</code> :)” on the serial port.</p>
case ECHO:</span></span>
    char</span> char_arr</span>[</span>MAX_MSG_SIZE</span>];</span></span>
</span>
    // Extract the next value from the command string as a character array</span></span>
    success </span>=</span> robot_cmd.</span>get_next_value</span>(</span>char_arr</span>);</span></span>
    if</span> (</span>!</span>success</span>)</span></span>
        return</span>;</span></span>
</span>
    Serial.</span>print</span>(</span>"Robot says: "</span>);</span></span>
    Serial.</span>print</span>(</span>char_arr</span>);</span></span>
    Serial.</span>println</span>(</span>" :)"</span>);</span></span>
    </span></span>
    break</span>;</span></span></code></pre>
SEND_THREE_FLOATS command</h3>
The SEND_THREE_FLOATS</code> command takes in an input of 3 floats each separated by a |</code>. The robot prints to the serial port Three floats: [float1], [float2], [float3]</code>.</p>
case SEND_THREE_FLOATS:</span></span>
    float</span> float_a</span>,</span> float_b</span>,</span> float_c</span>;</span></span>
</span>
    // Extract the next value from the command string as a float</span></span>
    success </span>=</span> robot_cmd.</span>get_next_value</span>(</span>float_a</span>);</span></span>
    if</span> (</span>!</span>success</span>)</span></span>
        return</span>;</span></span>
</span>
    // Extract the next value from the command string as a float</span></span>
    success </span>=</span> robot_cmd.</span>get_next_value</span>(</span>float_b</span>);</span></span>
    if</span> (</span>!</span>success</span>)</span></span>
        return</span>;</span></span>
</span>
    // Extract the next value from the command string as a float</span></span>
    success </span>=</span> robot_cmd.</span>get_next_value</span>(</span>float_c</span>);</span></span>
    if</span> (</span>!</span>success</span>)</span></span>
        return</span>;</span></span>
</span>
    Serial.</span>print</span>(</span>"Three Floats: "</span>);</span></span>
    Serial.</span>print</span>(</span>float_a</span>);</span></span>
    Serial.</span>print</span>(</span>", "</span>);</span></span>
    Serial.</span>print</span>(</span>float_b</span>);</span></span>
    Serial.</span>print</span>(</span>", "</span>);</span></span>
    Serial.</span>println</span>(</span>float_c</span>);</span></span>
    </span></span>
    break</span>;</span></span></code></pre>GET_TIME_MILLIS command</h3>
The GET_TIME MILLIS</code> command takes in no parameters, gets the time since Artemis boot in milliseconds, and writes T: [time]</code> to the string characteristic and the serial port.</p>
case GET_TIME_MILLIS:</span></span>
    tx_estring_value.</span>clear</span>();</span></span>
    tx_estring_value.</span>append</span>(</span>"T:"</span>);</span></span>
    tx_estring_value.</span>append</span>((</span>int</span>)</span> millis</span>());</span></span>
    tx_characteristic_string.</span>writeValue</span>(</span>tx_estring_value.</span>c_str</span>());</span></span>
</span>
    Serial.</span>println</span>(</span>tx_estring_value.</span>c_str</span>());</span></span>
    break</span>;</span></span></code></pre></iframe>
Notification Handler</h3>
The notification handler takes in a function with the UUID and a bytearray as inputs. It is set up to call the function when the string characteristic is written. This notification handler waits for the time to be written, parsed the data, and prints it.</p>
def</span> time_extractor</span>(</span>uuid</span>,</span> byte_time</span>):</span></span>
    str_time</span> =</span> ble</span>.</span>bytearray_to_string</span>(</span>byte_time</span>)</span></span>
    i</span> =</span> str_time</span>.</span>find</span>(</span>"T:"</span>)</span></span>
    if</span> i</span>  == -</span>1</span>:</span></span>
        print</span>(</span>f</span>"Time is </span>{</span>str_time</span>}</span>"</span>)</span></span>
    else</span>:</span></span>
        print</span>(</span>f</span>"Time is </span>{</span>str_time</span>[</span>i</span>+</span>2</span>:]</span>}</span>"</span>)</span> </span></span>
</span>
ble</span>.</span>start_notify</span>(</span>ble</span>.</span>uuid</span>[</span>'</span>RX_STRING</span>'</span>],</span> time_extractor</span>)</span></span></code></pre></iframe>
Sending Time in a Loop</h3>
We start by sending time in a loop. I do this by making a command that gets the time and transmits it in a loop. From there, the notification handler parses and prints the data.</p>
case LOOP_TIME:</span></span>
    Serial.</span>println</span>(</span>"Sending Loop Time"</span>);</span></span>
    for</span> (</span>i </span>=</span> 0</span>;</span> i </span><</span> TIME_ARR_SIZE</span>;</span> i</span>++</span>) {</span></span>
        tx_estring_value</span>.</span>clear</span>();</span></span>
        tx_estring_value</span>.</span>append</span>((</span>int</span>)</span> millis</span>());</span></span>
        tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span></span>
    }</span></span>
    Serial.</span>println</span>(</span>"Loop Complete"</span>);</span></span>
    break</span>;</span></span></code></pre></iframe>
SEND_TIME_DATA (Sending Time in and Array)</h3>
Next, we transmit the data by storing the values in an array, then transmitting all at once.</p>
case SEND_TIME_DATA:</span></span>
    Serial.</span>println</span>(</span>"Filling Time"</span>);</span></span>
    for</span> (</span>i </span>=</span> 0</span>;</span> i </span><</span> TIME_ARR_SIZE</span>;</span> i</span>++</span>) {</span></span>
        time_stamps</span>[</span>i</span>]</span> =</span> (</span>int</span>)</span> millis</span>();</span></span>
    }</span>            </span></span>
    Serial.</span>println</span>(</span>"Time Array Complete"</span>);</span></span>
    Serial.</span>println</span>(</span>"Sending Array Time"</span>);</span></span>
    for</span> (</span>i </span>=</span> 0</span>;</span> i </span><</span> TIME_ARR_SIZE</span>;</span> i</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>i</span>]);</span></span>
        tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span></span>
    }</span>            </span></span>
    Serial.</span>println</span>(</span>"Array Complete"</span>);</span></span>
    break</span>;</span></span></code></pre></iframe>
GET_TEMP_READINGS (Sending Temperature)</h3>
Similar to SEND_TIME_DATA</code>, we fill 2 arrays with data for the time stamp and the current temperature reading. Then, we transmit the data.</p>
case GET_TEMP_READINGS:</span></span>
    Serial.</span>println</span>(</span>"Filling Readings"</span>);</span></span>
    for</span> (</span>i </span>=</span> 0</span>;</span> i </span><</span> TIME_ARR_SIZE</span>;</span> i</span>++</span>) {</span></span>
        float</span> temp_f </span>=</span> getTempDegF</span>();</span></span>
        time_stamps</span>[</span>i</span>]</span> =</span> (</span>int</span>)</span> millis</span>();</span></span>
        temp_readings</span>[</span>i</span>]</span> =</span> getTempDegF</span>();</span></span>
    }</span>            </span></span>
    Serial.</span>println</span>(</span>"Time Array Complete"</span>);</span></span>
    Serial.</span>println</span>(</span>"Sending Array Time"</span>);</span></span>
    for</span> (</span>i </span>=</span> 0</span>;</span> i </span><</span> TIME_ARR_SIZE</span>;</span> i</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>i</span>]);</span></span>
        tx_estring_value</span>.</span>append</span>(</span>":"</span>);</span></span>
        tx_estring_value</span>.</span>append</span>(</span>temp_readings</span>[</span>i</span>]);</span></span>
        tx_characteristic_string</span>.</span>writeValue</span>(</span>tx_estring_value</span>.</span>c_str</span>());</span></span>
    }</span>            </span></span>
    Serial.</span>println</span>(</span>"Array Complete"</span>);</span></span>
    break</span>;</span></span></code></pre></iframe>
Discussion</h3>
Loop vs Array</h4>
We can clearly see that the time stamps are much closer together when the data is first stored in array, and then transmitted, like in SEND_TIME_DATA</code>. The LOOP_TIME</code> transmission shows a delay of about 66.6 ms. Meanwhile, all 10 datapoints from SEND_TIME_DATA</code> and in the same millisecond. This shows that if we want to collect data quickly, we should be storing it in an array. But, if we want to transmit the most up-to-date data, then sending data in a loop makes more sense.</p>
Amount of Data</h4>
We transmit time as an int</code> which is 4B. If all the Artemis RAM is used to store time, we could store at most 96000 time values. This is assuming no other data is in RAM and no other overhead is needed be to stored.</p>
Effective Data Rate, Overhead, and Reliablility</h3>
To observe the data rate of messages of different sizes, I observe the time between transmitting and recieving a string of various lengths. Since each character is 1 byte, the string length is the same as the byte size of the message. I form the string first, record the start time, then transmit. I tested 100 transmission of each byte size and then graphed the average time.</p>

From this graph, we see that smallest and largest packet size show a similar transmission time. This indicates the small packets do introduce a lot of overhead. We see the round trip time spike when the data packets are 100-120 bytes. Overall, the data rate doesn’t seem to change too much when the data size changes, showing that there is limited overhead on the messages in general.</p>
Even as the data rate changed, no packets were dropped at any point. This shows that the computer does read all the data from the Artemis.</p>
Implementation</h4>
This command was used to send back the recieved data.</p>
case DATA_RATE_ANALYSIS:</span></span>
    char</span> message_arr</span>[</span>MAX_MSG_SIZE</span>];</span></span>
    // Extract the next value from the command string as a character array</span></span>
    success </span>=</span> robot_cmd.</span>get_next_value</span>(</span>message_arr</span>);</span></span>
    if</span> (</span>!</span>success</span>)</span></span>
        return</span>;</span></span>
    </span></span>
    tx_estring_value.</span>clear</span>();</span></span>
    tx_estring_value.</span>append</span>(</span>message_arr</span>);</span></span>
    tx_characteristic_string.</span>writeValue</span>(</span>tx_estring_value.</span>c_str</span>());</span></span>
    break</span>;</span></span></code></pre>
On the python side, we choose to take 100 samples of strings from 1 byte to 140 bytes.</p>
max_byte_size</span> =</span> 141</span></span>
samples</span> =</span> 100</span></span>
</span>
start_times</span> =</span> [[]</span> for</span> _</span> in</span> range</span>(</span>max_byte_size</span>)]</span></span>
end_times</span> =</span> [[]</span> for</span> _</span> in</span> range</span>(</span>max_byte_size</span>)]</span></span></code></pre>
A notification handler is set up to recieve messages and add the end time to the appropiate array spot.</p>
def</span> recieved_time_getter</span>(</span>uuid</span>,</span> byte_message</span>):</span>    </span></span>
    end_time</span> =</span> time</span>.</span>time</span>()</span></span>
    mess</span> =</span> ble</span>.</span>bytearray_to_string</span>(</span>byte_message</span>)</span></span>
    end_times</span>[</span>len</span>(</span>mess</span>)].</span>append</span>(</span>end_time</span>)</span></span>
 </span></span>
        </span></span>
ble</span>.</span>start_notify</span>(</span>ble</span>.</span>uuid</span>[</span>'</span>RX_STRING</span>'</span>],</span> recieved_time_getter</span>)</span></span></code></pre>
Then, we transmit messages of various sizes. We record the time they’re transmitted and sleep to ensure transmissions don’t conflict with eachother.</p>
for</span> i</span> in</span> range</span>(</span>1</span>,</span> max_byte_size</span>-</span>1</span>):</span></span>
    for</span> _</span> in</span> range</span>(</span>samples</span>):</span></span>
        message</span> =</span> "a"</span>*</span>i</span></span>
        start</span> =</span> time</span>.</span>time</span>()</span></span>
        ble</span>.</span>send_command</span>(</span>CMD</span>.</span>DATA_RATE_ANALYSIS</span>,</span> message</span>)</span></span>
        start_times</span>[</span>i</span>].</span>append</span>(</span>start</span>)</span></span>
        time</span>.</span>sleep</span>(</span>.05</span>)</span></span>
print</span>(</span>"done"</span>)</span></span></code></pre>
Finally, we compute the rtt and graph the mean.</p>
ble</span>.</span>stop_notify</span>(</span>ble</span>.</span>uuid</span>[</span>'</span>RX_STRING</span>'</span>])</span></span>
</span>
start</span> =</span> np</span>.</span>array</span>(</span>start_times</span>[</span>1</span>:</span>max_byte_size</span>-</span>1</span>])</span></span>
end</span> =</span> np</span>.</span>array</span>(</span>end_times</span>[</span>1</span>:</span>max_byte_size</span>-</span>1</span>])</span></span>
</span>
durations</span> =</span> end</span> -</span> start</span></span>
rtt</span> =</span> durations</span>.</span>mean</span>(</span>axis</span>=</span>1</span>)</span></span>
</span>
plt</span>.</span>plot</span>(</span>rtt</span>)</span></span>
plt</span>.</span>xlabel</span>(</span>"Message Size (bytes)"</span>)</span></span>
plt</span>.</span>ylabel</span>(</span>"Round Trip Time (ms)"</span>)</span></span>
plt</span>.</span>show</span>()</span></span></code></pre>Collaboration</h2>
This lab was complete with discussion with Dyllan Hofflich and Shao Stassen. Lab writeups from previous students such as Lucca Correia, Aiden McNay, and Sana Chawla were refferenced.</p>


Cucumber the Robot Dog
2025-05-16T00:00:00+00:00
Check out the project website here for details!</a></p>
</iframe>

Collaboration</h1>
This lab was worked on with consultation from Shao Stassen. We worked together more closely for the Stunt section. Aidan McNay’s student page was also consulted.</p>

Resources</h2>
Qwiic Connect Guide</a></p>
ArduinoDocs I2C Guide</a></p>
Alphabold FFT Tutorial</a></p>
SciPy Butter</a></p>
SciPy Filtfilt</a></p>

Ananya's Portfolio

Lab 7: Kalman Filter

Collaboration and Sources</h2>
I referenced the lecture slides when setting up the Kalman filter. I also referenced Trevor Dales and Aidan Derocher’s lab writeups.</p>

Lab 6: Orientation Control

`Collaboration and Sources</h2> I did the lab with very light discussion with Dyllan Hofflich.</p>`

Lab 5: Linear PID control and Linear Interpolation

Lab 5: Linear PID control and Linear interpolation</h1>
The goal of this lab was to get the robot, Meep, to drive forward and stop extactly 304 mm from a wall via PID controls. This lab explores tuning a PID controller, extrapolating sensor data, and handling integrator windup.</p>

Collaboration and Sources</h2>
I referenced the lecture slides when setting up PID. I discussed ideas for code structure with Dyllan Hofflich.</p>

Lab 4: Motors and Open Loop Control

Lab 4: Motors and Open Loop Control</h1>
In this lab, we work on controlling the robot, Meep, with open-loop pwm signals. We start by soldering the motor driver and testing the pwm signal generated via `analogWrite</code>. Meep has two motors and operates on differential drive.</p>`

PWM and Oscilloscope</h1>

Grad Tasks</h1>

Collaboration and Sources</h2>
I collobrated with Dyllan and Shao on bot building ideas. I referenced Aiden Derocher’s</a> and Tyler Wisniewski’s</a> lab reports when building my robot.</p>

Lab 3: ToF

Lab 3: Time of Flight Sensors</h1>
This lab explores adding two time of flight sensors to an RC car (Meep). We explore how to handle I2C communication when devices share the same address, the pros and cons of different configuration modes, and the various sensitivities of infared sensors.</p>

Time of Flight Setup</h2>

Lab 2: IMU

Accelerometer</h1>

Lab 1: Artemis and Bluetooth

Lab 1A</h2>
The first part of this lab demostrates how to program the RedBoard Artemis Nano and explores some of the features via demo code files.</p>

Basic Setup</h3>
To start this lab, the Artemis Redboard Nano is connected to a laptop. We use the Arduino IDE to program it.</p>

Running Blink</h3>
Like with most microntroller projects, we start with making the onboard led blink. The demo code initializes the built-in led as the output, then repeated turns it on and off with a second delay in between each action.</p> </iframe>

Testing the Temperature Sensor</h3>
The “analog read” demo code allows us to read the output of the Artemis builtin temperature sensor.</p>

Testing the Microphone</h3>
Finally, the Microphone Ouptut demo allows use to see the loudest frequency detected by the onboard microphone. You can see the result of my high pitch whisper screaming into the microphone below.</p>

Discussion</h3>

Collaboration</h2>
This lab was complete with discussion with Dyllan Hofflich and Shao Stassen. Lab writeups from previous students such as Lucca Correia, Aiden McNay, and Sana Chawla were refferenced.</p>

Cucumber the Robot Dog

Ananya's Portfolio

Lab 7: Kalman Filter

Collaboration and Sources</h2> I referenced the lecture slides when setting up the Kalman filter. I also referenced Trevor Dales and Aidan Derocher’s lab writeups.</p>

Lab 6: Orientation Control

Collaboration and Sources</h2> I did the lab with very light discussion with Dyllan Hofflich.</p>

Lab 5: Linear PID control and Linear Interpolation

Lab 5: Linear PID control and Linear interpolation</h1> The goal of this lab was to get the robot, Meep, to drive forward and stop extactly 304 mm from a wall via PID controls. This lab explores tuning a PID controller, extrapolating sensor data, and handling integrator windup.</p>

Collaboration and Sources</h2> I referenced the lecture slides when setting up PID. I discussed ideas for code structure with Dyllan Hofflich.</p>

Lab 4: Motors and Open Loop Control

Lab 4: Motors and Open Loop Control</h1> In this lab, we work on controlling the robot, Meep, with open-loop pwm signals. We start by soldering the motor driver and testing the pwm signal generated via analogWrite</code>. Meep has two motors and operates on differential drive.</p>

PWM and Oscilloscope</h1>

Grad Tasks</h1>

Collaboration and Sources</h2> I collobrated with Dyllan and Shao on bot building ideas. I referenced Aiden Derocher’s</a> and Tyler Wisniewski’s</a> lab reports when building my robot.</p>

Lab 3: ToF

Lab 3: Time of Flight Sensors</h1> This lab explores adding two time of flight sensors to an RC car (Meep). We explore how to handle I2C communication when devices share the same address, the pros and cons of different configuration modes, and the various sensitivities of infared sensors.</p>

Time of Flight Setup</h2>

Lab 2: IMU

Accelerometer</h1>

Collaboration</h1> This lab was worked on with consultation from Shao Stassen. We worked together more closely for the Stunt section. Aidan McNay’s student page was also consulted.</p>

Resources</h2> Qwiic Connect Guide</a></p> ArduinoDocs I2C Guide</a></p> Alphabold FFT Tutorial</a></p> SciPy Butter</a></p> SciPy Filtfilt</a></p>

Lab 1: Artemis and Bluetooth

Lab 1A</h2> The first part of this lab demostrates how to program the RedBoard Artemis Nano and explores some of the features via demo code files.</p>

Basic Setup</h3> To start this lab, the Artemis Redboard Nano is connected to a laptop. We use the Arduino IDE to program it.</p>

Running Blink</h3> Like with most microntroller projects, we start with making the onboard led blink. The demo code initializes the built-in led as the output, then repeated turns it on and off with a second delay in between each action.</p> </iframe>

Testing the Temperature Sensor</h3> The “analog read” demo code allows us to read the output of the Artemis builtin temperature sensor.</p>

Testing the Microphone</h3> Finally, the Microphone Ouptut demo allows use to see the loudest frequency detected by the onboard microphone. You can see the result of my high pitch whisper screaming into the microphone below.</p>

Discussion</h3>

Collaboration</h2> This lab was complete with discussion with Dyllan Hofflich and Shao Stassen. Lab writeups from previous students such as Lucca Correia, Aiden McNay, and Sana Chawla were refferenced.</p>

Cucumber the Robot Dog

Collaboration and Sources</h2>
I referenced the lecture slides when setting up the Kalman filter. I also referenced Trevor Dales and Aidan Derocher’s lab writeups.</p>

`Collaboration and Sources</h2> I did the lab with very light discussion with Dyllan Hofflich.</p>`

Lab 5: Linear PID control and Linear interpolation</h1>
The goal of this lab was to get the robot, Meep, to drive forward and stop extactly 304 mm from a wall via PID controls. This lab explores tuning a PID controller, extrapolating sensor data, and handling integrator windup.</p>

Collaboration and Sources</h2>
I referenced the lecture slides when setting up PID. I discussed ideas for code structure with Dyllan Hofflich.</p>

Lab 4: Motors and Open Loop Control</h1>
In this lab, we work on controlling the robot, Meep, with open-loop pwm signals. We start by soldering the motor driver and testing the pwm signal generated via `analogWrite</code>. Meep has two motors and operates on differential drive.</p>`

Collaboration and Sources</h2>
I collobrated with Dyllan and Shao on bot building ideas. I referenced Aiden Derocher’s</a> and Tyler Wisniewski’s</a> lab reports when building my robot.</p>

Lab 3: Time of Flight Sensors</h1>
This lab explores adding two time of flight sensors to an RC car (Meep). We explore how to handle I2C communication when devices share the same address, the pros and cons of different configuration modes, and the various sensitivities of infared sensors.</p>

Collaboration</h1>
This lab was worked on with consultation from Shao Stassen. We worked together more closely for the Stunt section. Aidan McNay’s student page was also consulted.</p>

Resources</h2>
Qwiic Connect Guide</a></p>
ArduinoDocs I2C Guide</a></p>
Alphabold FFT Tutorial</a></p>
SciPy Butter</a></p>
SciPy Filtfilt</a></p>

Lab 1A</h2>
The first part of this lab demostrates how to program the RedBoard Artemis Nano and explores some of the features via demo code files.</p>

Basic Setup</h3>
To start this lab, the Artemis Redboard Nano is connected to a laptop. We use the Arduino IDE to program it.</p>

Running Blink</h3>
Like with most microntroller projects, we start with making the onboard led blink. The demo code initializes the built-in led as the output, then repeated turns it on and off with a second delay in between each action.</p> </iframe>

Testing the Temperature Sensor</h3>
The “analog read” demo code allows us to read the output of the Artemis builtin temperature sensor.</p>

Testing the Microphone</h3>
Finally, the Microphone Ouptut demo allows use to see the loudest frequency detected by the onboard microphone. You can see the result of my high pitch whisper screaming into the microphone below.</p>

Collaboration</h2>
This lab was complete with discussion with Dyllan Hofflich and Shao Stassen. Lab writeups from previous students such as Lucca Correia, Aiden McNay, and Sana Chawla were refferenced.</p>