Motion | C Labs

Opening brief · Introduction

Know which way is up

How does a machine know what is happening to it in the physical world?

You encounter orientation sensing every day, even when you do not notice it. A phone rotates its display when you turn it. A drone changes motor speeds to remain level. A rocket corrects its course, while a VR headset updates your view as you move your head.

These machines do very different jobs, but each needs an answer to the same question: which way am I facing? That answer is its orientation—its rotation relative to a useful reference such as gravity and the world around it.

Knowing its orientation allows a machine to react rather than simply move. This is a common job for an .

Many embedded systems operate as a repeating loop:

The control loop

Disturbance → Measure → Calculate → Correct → Measure again

A disturbance changes the system. Sensors measure what happened. Software calculates the system’s new state, and a controller decides how to respond. The cycle then begins again with another measurement.

This Lab concentrates on the first essential part of that loop: measurement. Before a drone can keep itself level, it needs a trustworthy description of how it is moving.

Reveal possible answers

Several measurements can help a machine estimate its orientation or movement:

Real systems often combine several sensors because each one answers a different part of the question.

You will not build an entire drone. Instead, you will build the sensing system that lets one begin to answer: what is happening to me in the physical world?

The system you are about to build

PHYSICAL WORLD → MPU6050 → ESP32 → USB → COMPUTER

The measures acceleration and rotation. The reads those measurements and calculates an estimate of orientation. It then sends the result over USB so your computer can reproduce the movement as a live 3D model.

The MPU6050 contains two sensors: an and a . Each sees a different part of the motion. You will learn how to read both measurements and combine them into an orientation estimate.

What you will learn

By working through the investigation, you will learn how to:

communicate with a sensor over I²C;
turn raw register values into physical measurements;
use gravity to calculate tilt;
measure and correct gyroscope bias;
integrate angular velocity over time; and
fuse imperfect measurements into a more useful orientation estimate.

You do not need to understand every term yet. Each one will be introduced when the system needs it. For now, the goal is simple: connect a physical movement in your hand to a measurement you can see, question, and improve.

Preflight · Connect

Talk to the sensor

How do we get information out of a chip?

The sensing chain has two jobs to begin with: the MPU6050 produces measurements, and the ESP32 retrieves them. Once that exchange works, the ESP32 can pass the results over USB to your computer.

Signal chain: physical world → MPU6050 → ESP32 → USB → computer.

Wire the system

Connect VCC → 5V, GND → GND, SDA → GPIO 25, and SCL → GPIO 26. Check the wiring before plugging in USB.

The connection uses : SDA carries data, SCL carries the clock, and each device has an address. Here, the MPU6050 is 0x68.

Before measuring anything, the ESP32 must wake the sensor and select known measurement ranges. A write follows four steps: address the device, name the register, provide its new value, and send the transaction.

Your task

Configure the MPU6050 and prove that it is ready.

Open Serial Monitor at 115200 baud. Success means it shows exactly MPU6050 is ready. Keep loop() empty for now; this stage only proves that setup works.

Hint 1

Initialization happens once, so start in setup().

Hint 2

A reusable writeRegister(reg, value) helper can report whether each I²C transaction succeeded.

Show approach

Start Serial and Wire, then write 0x00 to registers 0x6B, 0x1C and 0x1B. Only print the ready message after all three writes succeed.

Checkpoint 01 · Acceleration

Read physical acceleration

Why are there six bytes for three axes?

The MPU6050 continually updates its measurement registers. Their locations are fixed by the manufacturer, so we use the to find them. Acceleration begins at address 0x3B.

The answer is the . Hardware programming relies on this documentation: the chip defines the interface, and our code must follow it.

One register stores one , but each axis measurement is a signed 16-bit value. The sensor therefore splits each axis into a high byte and a low byte. Three axes require six bytes:

X high · X low · Y high · Y low · Z high · Z low

First point the sensor at 0x3B, keep the transaction open with Wire.endTransmission(false), and request six bytes. Reassemble each pair by moving the high byte into the upper eight bits and joining the low byte beneath it.

Your task

Read X, Y and Z as signed 16-bit values.

Print all three values. Move the sensor and confirm that every axis reacts. Then divide each raw value by 16384.0f to express it in g.

Hint 1

Do X first: each call to Wire.read() consumes one byte.

Hint 2

Repeat the same high-byte/low-byte pattern for Y and Z, and verify that exactly six bytes arrived before consuming them.

Show approach

Create readAccel(). Select register 0x3B, request six bytes, then update raw X, Y and Z only after both I²C operations succeed.

Reveal explanation

The accelerometer measures gravity as a constant acceleration. Rotate the sensor by 90 degrees and the 1 g component moves to another axis. Gravity has not moved; the sensor’s coordinate system has rotated around it.

Checkpoint 01 · capability achieved

Acceleration is available in physical units.

✓MPU6050 data reaches the ESP32
✓X, Y and Z are decoded
✓Raw counts are converted to g

Checkpoint 02 · Gravity angles

Turn gravity into angles

Can gravity tell us our full orientation?

Gravity gives us a reference direction: down. As the board tilts, the measured X, Y, and Z components change. Together, they form the sides of a trigonometry problem from which we can calculate and .

For roll, use the Y and Z components:

accelRoll = atan2f(accelY, accelZ) * 180.0f / PI;

Pitch compares X with the combined Y/Z component. Deriving this equation is outside the scope of this Lab, so use it directly and concentrate on interpreting the result:

accelPitch = atan2f(
  -accelX,
  sqrtf(accelY * accelY + accelZ * accelZ)
) * 180.0f / PI;

Your task

Calculate roll and pitch from the gravity vector.

Roll the sensor left and right, then pitch it forward and backward. Observe which value responds and how the sign changes.

Hint 1

Only compute angles after a successful acceleration read.

Hint 2

Keep the equations in a separate computeAccelAngles() function and print both results in degrees.

Show approach

Implement roll first and test a left/right tilt. Add the supplied pitch equation only after roll follows the board correctly.

Reveal explanation

Gravity gives one direction, so it can describe tilt relative to down. Rotation around that same direction does not change the measured gravity vector. This rotation is called . The accelerometer cannot calculate a measurement that is not present in its data.

Checkpoint 02 · capability achieved

Roll and pitch can be measured from gravity.

✓Raw acceleration is available
✓Roll follows lateral tilt
✓Pitch follows forward/back tilt

Checkpoint 03 · Angular velocity

Measure rotation

Why is the gyroscope moving when it is not?

The gyroscope does not report an angle. It reports . This lets it detect rotation around X, Y, and Z—including the yaw motion that did not change the gravity measurement.

Its register pattern mirrors the accelerometer: start at 0x43, request six bytes, combine three high/low pairs. At the configured ±250°/s range, divide raw counts by 131.0f.

Your task

Read angular velocity on all three axes.

Rotate the sensor around each axis. In particular, rotate it flat on the table and confirm that Z now sees the yaw motion.

Hint 1

Reuse the exact six-byte pattern you already proved for acceleration.

Hint 2

Only the starting register and scale factor change: 0x43 and 131.0f.

Show approach

Build readGyro() with the same success/failure contract as readAccel(), then convert and print X, Y and Z in degrees per second.

Reveal explanation

Real sensors have . Estimate it by keeping the board completely still, collecting about 500 samples, and averaging each axis. Subtract the three saved averages from every later reading.

Your task

Teach the gyroscope what zero looks like.

After calibration, stationary values should sit much closer to zero while deliberate rotation remains visible.

Hint 1

Accumulate three independent sums while the sensor is stationary.

Hint 2

If the board moves during calibration, real motion becomes part of the estimated bias.

Show approach

Implement calibrateGyro(), average about 500 valid samples, save one bias per axis, then subtract each bias whenever you use a gyro rate.

Checkpoint 03 · capability achieved

Bias-corrected angular velocity is available on all axes.

✓X, Y and Z rotation rates
✓Physical units in degrees per second
✓Stationary bias removed

Checkpoint 04 · Gyro orientation

Turn rate into angle

What is dt, and why must we measure it?

If the board rotates at 90°/s for one second, it turns through 90°. applies that relationship to every short sampling interval:

degrees/second × seconds = degrees

Code, Serial output, and I²C communication all take time, so the loop period is never perfectly constant. Measure with micros(), then convert microseconds to seconds before multiplying it by the rate.

Your task

Integrate all three gyro rates into angles.

Rotate roughly 90 degrees and see whether the corresponding angle arrives near 90. Run at a fixed minimum period, but always calculate with measured time.

Hint 1

Save the previous timestamp; unsigned subtraction gives elapsed microseconds even when micros() wraps.

Hint 2

Update each angle with correctedRate * dt. Reject an unusually large dt, such as anything above 0.1 seconds.

Show approach

Measure now, calculate dt, save now for the next pass, then add X/Y/Z corrected rates multiplied by dt to the previous roll, pitch and yaw.

Reveal explanation

Calibration improves the zero point, but it is not perfect. A remaining error of only 0.05°/s seems harmless for one sample. Integration, however, accumulates that error continuously: 0.05°/s × forever = a problem.

The gyroscope has no absolute reference. It has no way to know that its growing angle is wrong.

Checkpoint 04 · capability achieved

The gyroscope produces roll, pitch and yaw orientation.

✓Rate is integrated into angle
✓Timing uses measured dt
✓All three axes respond
✓Drift has been observed

Checkpoint 05 · Sensor fusion

Combine imperfect sensors

Can two wrong measurements produce one useful estimate?

You have now observed that the two sensors struggle in almost opposite ways:

The accelerometer knows where down is and is stable long-term, but movement disturbs it and yaw is invisible.
The gyroscope sees smooth rotation on every axis, but it is only relative and drifts forever.

A uses the gyroscope for immediate motion, then lets the accelerometer gradually pull roll and pitch back towards gravity:

filteredAngle = 0.98f * (oldAngle + correctedRate * dt)
              + 0.02f * measuredAccelAngle;

The weights do not mean “98% good sensor and 2% bad sensor.” They describe short-term trust and long-term trust. The gyroscope carries rapid changes; the gravity measurement provides a slow correction.

Your task

Build the complementary filter for roll and pitch.

Try to break the result. Rotate quickly, move the board linearly, put it down, and compare its behavior with acceleration-only and gyro-only estimates.

Hint 1

You already have both ingredients: gyro integration and accelerometer angles.

Hint 2

Apply the equation independently to roll and pitch. Wrap the finished result, not each weighted term.

Show approach

Replace each gyro-only roll/pitch update with the weighted combination. Keep yaw on gyro integration because gravity provides no yaw reference.

Reveal explanation

Gravity still contains no heading reference. A magnetometer could provide one, which is why nine-axis IMUs exist. With the hardware in this Lab, yaw remains gyro-only.

Checkpoint 05 · capability achieved

Roll and pitch are responsive and resist long-term drift.

✓Responsive short-term motion
✓Stable roll and pitch
✓Gyro drift corrected against gravity
✓Yaw limitation understood

Checkpoint 06 · Orientation output

Expose the result

How should one program describe orientation to another?

The orientation estimate is useful inside the ESP32, but the 3D visualiser runs on your computer. The two programs need an agreed way to exchange each measurement.

Human-readable diagnostics are convenient while debugging:

Roll: 12.4 deg Pitch: -3.8 deg Yaw: 41.2 deg

A receiving program needs a predictable instead:

12.421/-3.837/41.204

This Lab’s protocol is deliberately small. The field order is roll, pitch, yaw. / separates the fields, and a newline marks the end of one complete record. Diagnostic lines begin with # so the receiver can identify and ignore them.

Your task

Produce one machine-readable orientation record per line.

Open Serial Monitor and verify that every data line contains exactly three finite slash-separated numbers. Consider how startup text, errors or incomplete lines should be handled.

Hint 1

Print roll and pitch with Serial.print(), adding one slash after each.

Hint 2

Finish yaw with Serial.println() so the receiver knows where the record ends.

Show approach

Print all three values to three decimal places in roll/pitch/yaw order. Count five successful 200 Hz loop passes before outputting to produce about 40 records per second.

Reveal explanation

External input should never be assumed valid—even when you wrote both ends. The visualiser checks the field count, rejects malformed numbers, and ignores diagnostic lines beginning with #.

Checkpoint 06 · capability achieved

A clean orientation stream drives a live 3D digital twin.

✓Roll, pitch and yaw output
✓Machine-readable record format
✓Approximately 40 Hz update rate
✓Physical movement ready for 3D visualisation

Visualise it

Open the provided Processing sketch, select the correct serial port, and run it. Movement of the physical sensor should now be reproduced by the digital model.

You began with raw bytes hidden in sensor registers. The completed chain is:

physical motion → MPU6050 → raw bytes → acceleration + gyro
                → sensor fusion → serial → 3D digital twin