S2_IMU/S2_IMU.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# `S2`: Sensor Lab 2: Inertial Measurement Unit (IMU)\n",
    "\n",
    "\n",
    "An inertial measurement unit (IMU) is a device consisting of an accelerometer, gyroscope, and (often) a magnetometer. By combining the information from the accelerometer--gravitational acceleration--with the data from the gyroscope--rotational velocity--the orientation of the device can be determined. The magnetometer is used to track the magnetic-north which determines the heading of the IMU.\n",
    "\n",
    "There are many commercially available IMUs on the market (e.g. [Human Motion Capture](https://human-motioncapture.com/measurement-systems/inertial-sensor-measurement-systems/)). When combined, the sensors in an IMU can also be used to detect alterations in human motions. For example, the accelerometer can be used to track body sway.\n",
    "\n",
    "In this lab we will work with the Sense HAT. This HAT is specially designed for the Raspberry Pi Zero. In addition to the accelerometer, gyroscope, and magnetometer, the Sense HAT also has a barometer, and temperature and humidity sensors. More information on the board and example code can be found [here](https://www.waveshare.com/wiki/Sense_HAT_(B)).\n",
    "\n",
    "\n",
    "## Outline\n",
    "\n",
    "* [1. Setup Hardware](#Ch1)\n",
    "  * [1.1 Connect the SenseHAT Module](#Ch11)\n",
    "  * [1.2 Connect the UPS HAT](#Ch12)\n",
    "  * [1.3 Power Up the Pi](#Ch13)\n",
    "* [2. Setup Software](#Ch2)\n",
    "* [3. Control the IMU with Python code](#Ch3)\n",
    "* [4. Create an Interface to Visualize the IMU's data](#Ch4)\n",
    "* [5. IMU measurements](#Ch5)\n",
    "  * [5.1 What is an IMU?](#Ch51)\n",
    "  * [5.2 Saving IMU data to a CSV file](#Ch52)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Setup Hardware <a class=\"anchor\" id=\"Ch1\"></a>\n",
    "\n",
    "\n",
    "### 1.1 Connect the SenseHAT Module <a class=\"anchor\" id=\"Ch11\"></a>\n",
    "\n",
    "1. Ensure your Raspberry Pi is turned off (not plugged into power).\n",
    "2. Gently push the SenseHAT module onto the Pi's header pins.\n",
    "\n",
    "<div>\n",
    "    <img src=\"images/Sense_HAT.jpg\" width=\"500\" />\n",
    "</div>\n",
    "\n",
    "\n",
    "### 1.2 Connect the UPS HAT <a class=\"anchor\" id=\"Ch12\"></a>\n",
    "\n",
    "The IMU will run with a battery board. Therefore, we will also connect the UPS HAT:\n",
    "\n",
    "1. Ensure your Raspberry Pi and your battery are turned off\n",
    "2. Connect the battery header to your Raspberry Pi:\n",
    "\n",
    "    * Connect the battery to the header board, move the wires out of the way of the connector pins\n",
    "    * Make sure the switch on the end of the board is __OFF__ before connecting \n",
    "    * Add the spacers and screws to fix the two boards together\n",
    "\n",
    "<div>\n",
    "    <img src=\"images/UPS_HAT.jpg\" width=\"500\">\n",
    "</div>\n",
    "\n",
    "\n",
    "### 1.3 Power Up the Pi <a class=\"anchor\" id=\"Ch13\"></a>\n",
    "\n",
    "1. Start up your Raspberry Pi by switching the UPS HAT's switch to __ON__\n",
    "2. While developing the software, though, remember to keep your charger plugged into your Pi.\n",
    "\n",
    "\n",
    "## 2. Setup Software <a class=\"anchor\" id=\"Ch2\"></a>\n",
    "\n",
    "The lab organizers have already gone through [X0_SoftwareSetup](../X0_SoftwareSetup), which sets up the necessary software for you, before you were given the Pi, so you probably don't need to set up any software.\n",
    "\n",
    "The configuration script does things like enabling the `i2c` interface on your pi and installing required libraries such as `bcm2835`. If you're curious about what it did, you can read through `s2.py`'s source code in the [pbl](../X0_SoftwareSetup/pbl/pbl) module.\n",
    "\n",
    "> ℹ️ **Problem With Your Pi?**\n",
    ">\n",
    "> The course organizers have tried their best to ensure all the configuration options and software you'll need is already installed before the course begins, but we can miss things. If you find that the Pi isn't working for you then you can try:\n",
    ">\n",
    "> - Asking for help\n",
    "> - Running `pbl test` in the terminal, which runs some basic checks that ensure things like libraries etc. are installed\n",
    "> - Reinstalling the necessary software by running `sudo pbl install` in the terminal (⚠️ **warning**: takes a long time)\n",
    "> - Manually going through the legacy setup guide [here](Legacy/S2_SetUpRaspberryPi.ipynb)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Control the IMU with Python code <a class=\"anchor\" id=\"Ch3\"></a>\n",
    "\n",
    "The Python __SMbus__ (`python-smbus`) library allows you to read the IMU sensors. We provide an example code called `ICM20948.py`. The example code is pre-installed on your Pi at `/opt/ICM20948.py`. It is also available online at https://github.com/PortableBalanceLab/ICM20948.\n",
    "\n",
    "`ICM20948.py` is an example file from the Sense HAT website. The file estimates the orientation and the output from the accelerometer, gyroscpe, and magnetometer.\n",
    "\n",
    "In the cell below, a basic script that prints the outputs from `ICM20948.py` is provided.\n",
    "\n",
    "1. Open a Python editor on the RP to start a new Python3 script\n",
    "2. **Save the script in a new folder: `home/pbl/Documents/Lab2/IMU_test.py`**\n",
    "3. To demonstrate that the device functions, we will start with an example code that read the values from `ICM20948.py`. Copy the code below to your script and run it. You can stop the script with typing `Ctrl+C` in the Shell window. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import necessary packages\n",
    "# 1. Import the ICM20948.py file and make sure this file can be found\n",
    "import sys\n",
    "sys.path.insert(0, '/opt/')\n",
    "from ICM20948 import *\n",
    "\n",
    "#2. Import 'time' for time.sleep\n",
    "import time\n",
    "\n",
    "print(\"\\nSense HAT Test Program ...\\n\")\n",
    "icm20948=ICM20948()\n",
    "\n",
    "while True:\n",
    "    try:\n",
    "        # Get the values from icm20948\n",
    "        icm20948.icm20948_Gyro_Accel_Read()\n",
    "        icm20948.icm20948MagRead()\n",
    "        icm20948.icm20948CalAvgValue()\n",
    "        time.sleep(0.1)\n",
    "        q0, q1, q2, q3 = icm20948.imuAHRSupdate(MotionVal[0] * 0.0175, MotionVal[1] * 0.0175,MotionVal[2] * 0.0175,\n",
    "                 MotionVal[3],MotionVal[4],MotionVal[5], \n",
    "                 MotionVal[6], MotionVal[7], MotionVal[8])      \n",
    "        pitch = math.asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3\n",
    "        roll  = math.atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3\n",
    "        yaw   = math.atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3\n",
    "\n",
    "        # Print the values\n",
    "        print(\"\\r\\n /-------------------------------------------------------------/ \\r\\n\")\n",
    "        print('\\r\\n Roll = %.2f , Pitch = %.2f , Yaw = %.2f\\r\\n'%(roll,pitch,yaw))\n",
    "        print('\\r\\nAcceleration:  X = %d , Y = %d , Z = %d\\r\\n'%(Accel[0],Accel[1],Accel[2]))  \n",
    "        print('\\r\\nGyroscope:     X = %d , Y = %d , Z = %d\\r\\n'%(Gyro[0],Gyro[1],Gyro[2]))\n",
    "        print('\\r\\nMagnetic:      X = %d , Y = %d , Z = %d'%((Mag[0]),Mag[1],Mag[2]))\n",
    "    \n",
    "    # Stop the script with Ctrl+C\n",
    "    except(KeyboardInterrupt):\n",
    "        print(\"\\n === INTERRUPTED ===\")\n",
    "        break"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. Create an Interface to Visualize the IMU's data <a class=\"anchor\" id=\"Ch4\"></a>\n",
    "\n",
    "Because it is hard to check whether the numbers that are printed by `ICM20948.py` make sense, we will create a GUI that visualizes (plots) the IMU's data in realtime. **Create a new script: `home/pbl/Documents/Lab2/IMU_animation.py`**. To start, you can copy the contents of the script `IMU_test.py` to this new script and add the interface to it.  \n",
    "\n",
    "> 🏆 **Challenge `S2.4`**: Create an interface that visualizes the running data plot while capturing. You can use the `IMU_test.py` script that is provided as a start. \n",
    ">\n",
    "> The interface you create should contain:\n",
    ">\n",
    "> - A `matplotlib` plot that updates with running data\n",
    "> - ⭐⭐ **Optional Challenge #1**: And several lines are visualized within the figure as (sub)plots\n",
    "> - ⭐⭐⭐ **Optional Challenge #2**: And there are start/stop buttons to start/stop the animation\n",
    ">\n",
    "> 💡 **Tips**:\n",
    ">\n",
    "> - This blog helps you with some inital code: https://learn.sparkfun.com/tutorials/graph-sensor-data-with-python-and-matplotlib/speeding-up-the-plot-animation\n",
    "\n",
    "#### Useful Resources\n",
    "\n",
    "- [Sparkfun Guide to Plotting Sensor Data](https://learn.sparkfun.com/tutorials/graph-sensor-data-with-python-and-matplotlib/speeding-up-the-plot-animation)\n",
    "- [Matplotlib API: `FuncAnimation`](https://matplotlib.org/stable/api/_as_gen/matplotlib.animation.FuncAnimation.html#matplotlib.animation.FuncAnimation)\n",
    "- [Matplotlib API: `add_subplot`](https://matplotlib.org/stable/api/figure_api.html#matplotlib.figure.Figure.add_subplot) (⭐⭐)\n",
    "- [Matplotlib API: `Button`](https://matplotlib.org/stable/gallery/widgets/buttons.html) (⭐⭐⭐)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create your own animation code here (IMU_animation.py)\n",
    "## And run it on your Raspberry Pi"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. IMU measurements <a class=\"anchor\" id=\"Ch5\"></a>\n",
    "\n",
    "\n",
    "### 5.1 What is an IMU? <a class=\"anchor\" id=\"Ch51\"></a>\n",
    "\n",
    "An inertial measurement unit (IMU) is a device consisting of three components:\n",
    "\n",
    "- **Accelerometer**: Measures acceleration (incl. gravity)\n",
    "- **Gyroscope**: Measures rotation velocity\n",
    "- **Magnetometer**: Measures magnetic north (the IMU's heading w.r.t. earth)\n",
    "\n",
    "By combining information from these, the orientation of the device can be determined. There are many commercially available IMUs on the market (e.g. [these](https://human-motioncapture.com/measurement-systems/inertial-sensor-measurement-systems/)).\n",
    "\n",
    "\n",
    "### 5.2 Saving IMU data to a CSV file <a class=\"anchor\" id=\"Ch52\"></a>\n",
    "\n",
    "In the previous section, you extracted and plotted live IMU data using `matplotlib`. However, we cannot analyse data if this isn't saved somewhere.\n",
    "\n",
    "To do this, you are going to need to know how to generate unique timestamped filenames ([X2](../X2_GeneratingTimestampedFilenames/X2_GeneratingTimestampedFilenames.ipynb)) and how to write to CSV files ([X1](../X1_WritingCSVFiles/X1_WritingCSVFiles.ipynb)). **You will then produce a new python script, `home/pbl/Documents/Lab2/IMU_logging_data.py`, that writes your output values (and a timestamp) to a CSV file.**\n",
    "\n",
    "> 🏆 **Challenge `S2.5.2a`**: Go through the [X1](../X1_WritingCSVFiles/X1_WritingCSVFiles.ipynb) and [X2](../X2_GeneratingTimestampedFilenames/X2_GeneratingTimestampedFilenames.ipynb) \"eXtra Content\" materials.\n",
    ">\n",
    "> - After going through [X1](../X1_WritingCSVFiles/X1_WritingCSVFiles.ipynb), you should know how to write CSV files\n",
    "> - After going through [X2](../X2_GeneratingTimestampedFilenames/X2_GeneratingTimestampedFilenames.ipynb), you should know how to generate timestamped file names\n",
    "> - Combine both techniques to write your data to a *timestamped* CSV file\n",
    ">\n",
    ">\n",
    "> 🏆 **Challenge `S2.5.2b`**: **Create a new script: `home/pbl/Documents/Lab2/IMU_logging_data.py`**. This script should create a timestamped CSV file containing the raw data. We do this separately from the animation script, because the animation slows our script down. \n",
    ">\n",
    "> - Modify the code you have written up to now to generate a timestamped CSV filename (e.g. `$yourpath$/output_$timestamp$.csv`). Create a list (or other data type) and append a row of IMU data each time new datapoints are generated by the loop. \n",
    "> - Write the entire data to CSV once the program stops. \n",
    "> - Make sure all IMU datapoints are written to the CSV file\n",
    "> - Make sure that when the program stops, you **close** the CSV file\n",
    ">\n",
    ">\n",
    "> 💡 **Tips**:\n",
    "> - Don't forget to import the `datetime` library at the start of your script\n",
    "> - In Lecture 3, you learnt how to open a file for writing with a specific filename\n",
    "> - Think about how you would like to export the data. CSV files typically have a header line that describes each column. **What will be your headers**?\n",
    "> - The rows of your data file will be filled with the IMU data you are measuring"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create your own save to CSV code here (IMU_logging_data.py)\n",
    "## And run it on your Raspberry Pi\n",
    "\n",
    "# 💡 tip: you may need to build a row of your CSV cell-by-cell\n",
    "## Import datetime\n",
    "from datetime import datetime\n",
    "\n",
    "row = []\n",
    "row.append(datetime.now())  # append a timestamp in the first column\n",
    "row.append(value1)          # append value to the row\n",
    "row.append(value2)          # append value to the row\n",
    "\n",
    "# and then write the `row` data array to your data file using `csv.writer`\n",
    "## you can do this per row using writerow\n",
    "## you can do this for multiple rows using writerows (preferably)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}