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 car 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

../_images/compass.png

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:

../_images/heading_control_block.png

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, where the compass is read using the same I2C module and functionality from the previous activity. 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.