Laboratory 5: Gesture RC Car

Laboratory Description

I2C sensors are used to further enhance the control developed in Lab 4. A remotely wired compass module is used to directly control the care with hand movements instead of through rotation of potentiometers.

Laboratory Goals

This laboratory is designed to further a student’s understanding of:

• Using digital sensors to provide feedback,

• Implementing and tuning PID control routines,

Preparation

Students should complete Lab 4 and Activity 13 prior to starting the laboratory.

Hardware and Tools

• TI-RSLK Robotic Car

• Two CMPS12 Electronic Compasses, different addresses

Description

The speed and turning control as implemented in Lab 4 provided for a direct calculation of the desired straight line and turn speeds. This lab introduces digital sensors to provide similar control inputs; however, the sensors are used to determine the cars response to external variable stimuli, as opposed to direct user manipulation, though in this case, the stimuli are still provided through user manipulation. Two additional control loops are added to the Integral control of speed to produce a more complicated response.

An electronic compass is mounted on the RSLK to detect the car’s current heading. A second electronic compass is mounted to a hand-held breadboard (akin to Lab 4), where rotating the breadboard causes the car to rotate as well; where the RSLK attempts to always be facing the same compass heading as the hand-held compass. Additionally, the hand-held electronic compass is also used to measure the “pitch” of the sensor as well, where tipping the sensor forward will cause the RSLK to drive forward and tipping backwards will cause it to drive reverse.

While the speed control (tipping the sensor) corresponds nicely with the previously implemented potentiometer speed control; the turning control requires the car to achieve a specific heading. To accomplish this, a PD control scheme will be implemented. Note that the wheel speed Integral control routine from Lab 4 should be left unmodified.

Electronic Compass

Electronic Compass

The electronic compass (Model CMPS12 from robot-electronics.co.uk) measures the orientation of the device relative to magnetic north and saves the measured angle, 0°-360°, in either a single byte, 0-255, or in two bytes in tenths of degrees, 0-3599, in addition to other features. In order to maintain sufficient accuracy of the measurement, the two byte representation should always be used. It is not documented how often the compass updates it’s measurements; so we will assume it is the same rate as the CMPS03 model: every 33.3 ms. Therefore, if multiple values are downloaded from the compass during this update time, the received values will be unchanged and not necessarily representative of the current behavior of the system, namely for the second measurement. The important registers and address for the compass are provided in the table below.

Important

This compass is auto-calibrating upon power-up. When power is applied, the compass will always return a heading of 0 degrees until the device is rotated in 3D space for a few seconds; at which point it should return reasonably correct heading values.

Note that calibration happens upon power-up, not each time the code is run: as long as the 3.3V supply to the compass is not disabled (e.g., from disconnecting the computer or powering off the car if not connected), then the calibration will persist between debugging sessions.

For this lab, power cycling often will be expected and therefore, this laboratory requires that the calibration be checked and ensured complete pprior to starting the control routine. To check the calibration: read the Calibration State registers and isolate the values of bits 0 and 1. The compass functionality is fully calibrated if both bits are high (value = 3).

CMPS12 Electronic Compass Registers

Register #	Read Function	Write Function
0	Software Version	Command Register
1	Heading as 0-255	N/A
2	Heading, Tenths of Degrees, High Byte	N/A
3	Heading, Tenths of Degrees, Low Byte	N/A
4	Pitch angle, Degrees, Signed Byte	N/A
5	Roll angle, Degrees, Signed Byte	N/A
…	…	…
30	Calibration State	N/A

Two compasses are required to be connected to the same I2C bus for this laboratory. To achieve this, several compasses have been reprogrammed to have a different address (labeled on the device). The default 7-bit adddress of the CMPS12 is 0x60.

Heading Control

The heading control for this lab assumes the same method to turn control used in Lab 4, where the desired speed is manipulated with a differential speed between them that causes a rotation:

\[v_l = v_d - \Delta v~~~~v_r = v_d + \Delta v\]

where \(v_d\) is the desired speed, \(v_l\) is the desired left wheel speed, \(v_r\) is the desired right wheel speed, and \(\Delta v\) is the steering control speed difference used to turn the car. As opposed to Lab 4, the value for \(\Delta v\) is now calculated from the difference between a desired heading and the measured heading using a proportional and derivative control scheme:

Steering control system

\[\Delta v(t) = k_P^h \epsilon_h(t) + k_D^h \frac{\mathrm{d}\epsilon_h(t)}{\mathrm{d}t}\]

\[\Delta v(k) = k_P^h \epsilon_h(k) + k_D^h \left(\epsilon_h(k) - \epsilon_h(k-1)\right)\]

where \(k_P^h\) is the steering proportional gain constant, \(k_D^h\) is the steering derivative gain constant, and \(\epsilon_h\) is the heading error.

Calculation of the heading error is not as straight forward as the difference between the desired heading, \(h_d\) (measured from hand-held compass), and the measured heading, \(h_m\) (measured from on-car compass):

\[\epsilon_h = \left( h_d - h_m \right) ~~~ \leftarrow \mathrm{wrong~by~itself}\]

as heading is a periodic function and is not necessarily linear; namely, the value for heading goes from \(0^\circ\) to \(359^\circ\) and then back to \(0^\circ\). This may result in erroneously large values for \(\epsilon_h\). For example, if \(h_d = 330^\circ\) and \(h_m = 60^\circ\) the direct calculation leads to \(\epsilon_h = 270^\circ\), which would cause the car to turn in the wrong direction towards the desired heading; that is, the long way. Of course, any angle in degrees is equivalent to itself \(\pm 360^\circ\), which may be used to correct the calculated \(\epsilon_h\) to determine the shortest direction to turn to reach the desired heading (three lefts make a right!). Applying to the previous example, the correct error would be \(\epsilon_h = 270^\circ - 360^\circ = -90^\circ\). It is necessary to always minimize the absolute value of \(\epsilon_h\). Since the value of the error may only be changed through \(\pm 360^\circ\), then the absolute value of the error must be bounded by half of that value: \(\left|\epsilon_h\right| \leq 180^\circ\), leading to the pseudocode:

heading error = desired_heading - measured_heading
If heading_error > 180
    Subtract 360 from the heading_error
If heading_error < -180
    Add 360 to the heading_error
... continue control scheme

This is known as “wrapping” the angle to the \([-180^\circ,180^\circ]\) domain. Note that this should only be done with the calculated error, not the individual values for \(h_d\) and \(h_m\)!

Similarly, the derivative of the error will suffer from the same when the error wraps around the \(\pm 180^\circ\) boundary in subsequent samples. However, this case may occurs when the car has a significant error, which is not generally expected when the program is appropriately for this lab. Therefore, correction of the derivative error is neglected.

Implementation Guidance

One compass should be installed on the RSLK the same way as in Activity 13, with the other mounted on the hand-held protoboard, as discussed. They are both connected through the same EUSCI bus. For this to occur, the compasses must have different I2C addresses.

Without a limit on differential speed, \(\Delta_v\), the RSLK will spin much too quickly for the derivative control to be useful; due to the slow iteration rate of the control routine (set as 100 ms per loop in Lab 4). To account for this, two things must be done for this lab:

1. Enforce a maximum differential speed, \(\Delta_{v,max}\), causing the RSLK maximum spin speed to be reduced, and

2. Increase the iteration period of the control routine from 100 ms to 50 ms. Note that the since the CMPS12 updates at approximately 33.3 ms (estimated), it is not useful to reduce the control routine below this value as no new data to act on would be available. Because the routine is running faster, the error accumulation for the integral control will happen twice as fast; therefore, it is necessary to halve the integral gain constant, \(k_I\).

Finally, since there is an active control loop acting to keep the RSLK heading towards a desired value, the integral control routine for the wheel speed is not necessarily required; as both the heading control and the wheel speed control would be acting to achieve a similar goal (drive straight considering fixed desired heading). Therefore, it is acceptable to set \(k_I = 0\) for this laboratory; though not required.

Considering the control schemes presented and the issues and solutions provided above, the following pseudocode is the suggested format for implementing this laboratory. Note that the Wheel Speed Control portion of this pseudocode is carried over from Lab 4 and has not changed.

main:
    Initializations

    Ensure RSLK LED1 and RGB LED are OFF
    Loop until both compasses report being calibrated:
        Read the calibration register for each compass
        Light LED1 (P1.0) if the on-car compass is calibrated
        Light the RGB LED RED (P2.0) if the hand-held compass is calibrated

    Infinite while loop:
       If ***50 ms*** has passed:
           Read all necessary values from the compasses
           <-- Start Heading Control -->
           Calculate the heading error and "wrap" the error to -180 to 180 (or -1800 to 1800)
           Calculate the differential speed using the PD control (equation given above)
           Enforce maximumum limit on differential speed
           Store the current heading error as previous heading error (for derivative control)
           <-- End Heading Control -->
           <-- Start Speed Control -->
           Calculate the desired speed from the compass tilt value
           <-- End Speed Control -->
           <-- Wheel Speed Control from Lab4 -->

Control Specifications (generally same as Lab 4)

Quantity	Value	Notes
Max |Diff Speed|, \(\Delta v_{max}\)	20 %	Set | \(\Delta v\) | to this if greater than
Min |Speed|, \(v_{d,min}\)	10 %	Set |desired speed| to 0 if less than
Max |Speed|, \(v_{d,max}\)	50 %	Set |desired speed| to this if greater than
Min PWM DC %	10 %	Set compare value to this if less than
Max PWM DC %	90 %	Set compare value to this if greater than
Control Routine Period	50 ms

Part A:

Important

Your finished Lab 4 project should be used as a starting point for this Lab 5. However, the template project used does not have the I2C_readData() and I2C_writeData() functions within it. To add them:

Copy both inc/engr2350_msp432.h and src/engr2350_msp432.c from Activity 13 and paste them into the Lab 5 project in the same corresponding locations.

Warning

There is an issue with the I2C protocol support between the CMPS12 and the MSP432P401R; where it does not seem to be possible to read only a single byte. Therefore, always read at least two bytes of data from the CMPS12. In cases where you only want to read a single register, use that as the start register and request 2 bytes to be read, ignoring the second read byte.

Copy and modify your completed Lab 4 to remove the ADC and potentiometer dependencies and replace with the heading control loop operating on one compass only, to start. The program should read the on-car compass’s heading to measure the current heading, \(h_m\). Assume a constant value for the desired heading, \(h_d\), and also force desired speed to be 0. This control should allow the car to spin in place without moving forward or reverse. Tune the \(k_P^h\) and \(k_D^h\) using the same method as presented in Activity 12:

1. Starting with a small value, gradually increase \(k_P^h\) until the car quickly responds to the desired heading but oscillates about it. Do this with \(k_D^h = 0\).

2. Starting with a small value, gradually increase \(k_D^h\) such that the car still responds quickly to the desired heading but no longer oscillates (it may still overshoot, however). Note that if the car rotation speeds up with increasing \(k_D^h\), the sign of \(k_D^h\) is likely incorrect.

Important

You must be careful selecting initial “small” values for \(k_P\) and \(k_D\): in some cases, a value of “1” might be considered small but in others it may be too large. For this case, the heading control is using the heading error, \(\epsilon_h\), to produce a PWM duty cycle. As \(\epsilon_h\) is by default in tenths of degrees \({}^\frac{\circ}{10}\) (unit from sensor), it will be in the range \([-1800^\frac{\circ}{10},1800^\frac{\circ}{10}]\). A “small” error would still be a large number, e.g., \(-150^\frac{\circ}{10}\). Multiplying this error with \(k_P=1\) results in a desired duty cycle of \(-150 \%\)! By this, it is clear that \(k_P\) should be much smaller to start tuning, such as \(k_P = 0.01\).

Testing of each gain pair may be done by rotating the car away from the desired heading to start and then release. While testing, ensure to print out applicable variables for debugging, such as the measured heading, the current error, and the proportional and derivative control contributions to the calculated differential speed.

It is possible to test the derivative control by itself by setting \(k_P^h = 0\). In this case, the car should respond to being rotated (by hand in air, not on floor/table) by spinning the wheels in the opposite direction. Likewise, the wheels should be stopped when not spinning.

Checkoff

Demonstrate that the heading control implementation works well. Printing of suggested values above may be requested.

Part B:

Add the hand-held compass to the program to provide both desired heading and desired speed. The desired speed should be calculated from the measured “pitch angle” of the compass. See the CMPS12 datasheet for the applicable register(s) to read from.

It is up to the groups to determine a reasonable conversion between pitch angle and desired speed.

Finally: As repeated testing and use will require power cycling the compasses, it is necessary to ensure that the compasses are calibrated prior to going into the driving state. The sensors themselves indicate if they are fully calibrated within the “Calibration State” register; where it is necessary for both bits 0 and 1 of this register to be 1 for each compass before proceeding. Within this loop, LED1 (on-car compass) and the RGB LED (hand-held compass) should be lit when the corresponding compass is noted as being fully calibrated.

Checkoff

Demonstrate the functional Lab 5.

Part C:

The CMPS12 Electronic “Compasses” are much more than just a compass. The heart of the CMPS12 is the Bosch BNO55 sensor chip, which includes a magnetometer (➞ compass), a triaxial accelerometer, and a triaxial gyroscope - a very useful sensor! Instead of using the compass, we’ll now use the accelerometer portion of the chip to detect the car’s tilt with respect to gravity.

The goal: to cause the RSLK to drive up a shallow ramp (now in the laboratory) and stop at the peak by using feedback from the CMPS12 accelerometer measurement.

A quick summary of what an accelerometer does and how gravity interacts with it: An accelerometer literally measures acceleration (rate-of-change of velocity) of the reference frame (i.e., the chip). A Triaxial accelerometer measures the acceleration in three orthogonal axes, which can fully describe any three-dimensional acceleration. The axes are commonly labeled “x”, “y”, and “z”; where each axis will have a specific orientation with respect to the chip. Accelerometers commonly measure acceleration by characterizing the force applied to a very small mass, \(F = m a\). Gravity acts on bodies as a force, though the total force applied to the body is related to the mass: \(F = m g\)! Therefore, accelerometers must inherently measure gravity in additional to other accelerations 1. Because of this, \(1 \mathrm{g}\), (1 gravity, \(9.81~\mathrm{m/s^2}\)) will almost always be evident in an accelerometers measurement, pointing towards the center of Earth. It will not appear in measurements only if either (1) the sensor is in free-fall (the sensor is accelerating with gravity) or (2) the sensor does measure constant accelerations, only variable or AC accelerations.

Assuming the RSLK is only driving forward or reverse such that it is only accelerating in the forward or reverse directions, the RSLK will sense only two distinct “accelerations”: the acceleration caused by driving and gravity (this is also assuming the surface is relatively flat). These are shown in the figure below, where the CMPS12 is mounted such that one axis, labelled \(\alpha\), is aligned with the forward/reverse axis, and another, labeled \(\beta\), is pointing up (or down) from the car. The third axis (not shown) would be parallel with the drive motor axes. Note that these would correspond with \(x\), \(y\), and \(z\), depending on how the compass is mounted. If the car is on flat ground (left figure), the drive acceleration, labeled \(d\), would be completely sensed on the \(\alpha\) axis while gravity, labeled \(g\), would be completely sensed on \(\beta\) axis. However, if the car is tilted from horizontal by an angle \(\theta\), such that it is facing uphill (right figure), then while \(d\) is still only sensed on \(\alpha\), \(g\) would be sensed by both. This results in a total acceleration sensed by each axis, \(a_\alpha\) and \(a_\beta\), described by the equations below the figure.

Sensed acceleration components by CMPS12 on RSLK

\[a_\alpha = d - g\sin(\theta)\]

\[a_\beta = -g\cos(\theta)\]

For the purposes of this lab, we effectively want to use the measurement of acceleration to measure \(\theta\). If the measurement is extremely accurate, then this could be done by simply measuring \(a_\beta\) and calculating \(\theta\), as \(g\) is known. Unfortunately, when \(\theta\) is small (as in this lab), the change induced in \(a_\beta\) will be very small. For example, \(\cos(10^\circ) = 0.9848\), meaning a \(10^\circ\) tilt will only cause a \(1.5\%\) change in \(a_\beta\) (!!), which is quite possibly below the noise floor of the sensor (unable to accurately detect). On the flip side, \(\sin(10^\circ) = 0.1736\) implying that \(a_\alpha\) has a much higher sensitivity for sensing \(\theta\) but as noted will also measure the drive acceleration. 2

Fortunately for us, we know that the car is very quick to accelerate to speed changes (discussed in Lab 4), so if the control routine measures the value of \(a_\alpha\) an appropriate time after the previous speed change, it may be assumed that the value for \(d\) will be small in the measurement and it can be assumed (hopefully) that \(a_\alpha \approx - g \sin(\theta)\), or more simply \(a_\alpha \approx -g \theta\).

Considering the above, groups must review the CMPS12 datasheet to select the correct register to read from to acquire “\(a_\alpha\)” and/or “\(a_\beta\)”, and how to interpret the data properly.

Hint

The datasheet does not tell the alignment of the acceleration axes! You must either experimentally determine them via measuring gravity or identify them from the BNO055 datasheet.

The CMPS12 also does not provide units for the acceleration axes. The question is: do you need them?

Using the measured acceleration(s), groups should then implement a proportional control scheme to cause the car to drive up the ramp (drive either forward or reverse as necessary with measured \(\theta\)) and stop at the top of the ramp. For testing and checkoff, it may be assumed that the car is placed on the ramp, near the bottom, such that it starts slightly inclined and does not have to “find” the ramp. The car will always be placed such that it is aligned directly up or down the ramp such that it does not need to turn to align itself with the ramp.

Groups should start with their finished Lab 5B code and remove the original compass steering and drive controls. The integral control on the wheel speed should be re-enabled if it has be disabled by previously setting \(k_I = 0\). Further, the control loop should be set again to increment at 100 ms. This may be increased as needed to help to remove measurement of the drive acceleration \(d\).

Rules:

1. The control of the car must be purely P control. For example: adding an if or while statement to cause the car to stop at the top of the ramp, preventing any further movement, is not allowed.

2. Groups are only allowed to use the acceleration measurements from the CMPS12. Yes, it might be tempting to use the “pitch” register instead. Do not!

3. The car must generally “stop” at the top of the ramp for it to be a successful checkoff (see below).

Checkoff

Demonstrate that the RSLK can drive both forward and reverse up the ramp and stop at the top. A satisfactory “stop” is considered as:

1. The car comes to a complete stop at the top of the ramp without oscillation back and forth. This would result in a check-plus.

2. The car stops at the top of the ramp but “jitters” back and forth (within a few cm, no larger); either for a few seconds or indefinitely. This would result in a check.

Note that the “top” of the ramp will be marked with two lines of tape. The RSLK wheels must be within the markings to be considered “on the top”.

Documentation and Deliverables

Produce a time plot of the car driving up the ramp plotting both the measured tilt angle and the desired PWM DC resulting from the control equation. Clearly mark and label the axes and add a legend as necessary. To acquire the data, you should print the data to the terminal and copy the output to whichever program used to make the plot. This implies that either the program is tested with a computer connected to it while it travels on the ramp or that the data is stored on the RSLK and later downloaded after reconnected to the computer (reconnect terminal but not a new debug session).

Submit this plot (as a picture) and your final code to the Gradescope assignment “Lab 5C”.

Footnotes

1

This is why strong accelerations on humans and other objects, are commonly described in terms of “g’s” (e.g., a “2g turn”): gravity is an intuitively understood amount of force.

2

Note that \(10^\circ\) in radians is \(0.1745~\mathrm{rad}\) such that \(\sin(10^\circ) = \sin(0.1745~\mathrm{rad}) = 0.1736\). This leads to a very useful approximation, known as the Small Angle Approximation:

\[\sin(\theta) \approx \theta~~~~~\mathrm{for~small~|\theta|}\]

\[\cos(\theta) \approx 1~~~~~\mathrm{for~small~|\theta|}\]