Making robots with Ada, Part 2 – Driving the motors

In part one of this series, we introduced the idea of replacing the Lego Mindstorms NXT® Brick with a low-cost ARM evaluation board so that we can reuse the NXT sensors and effectors with a much more powerful hardware and software platform.

Our replacement hardware is one of the very inexpensive “Discovery Kit” products from STMicroelectronics. (Similar boards are available from other vendors as well.) The Discovery Kits have ARM Cortex processors, with the specific processor and memory available depending on the kit. The MCUs on these boards include many on-package devices for interfacing to the external world, including A/D and D/A converters, timers, UARTs, DMA controllers, I2C and SPI communication, and others. Sophisticated external components are also included, depending upon the specific kit. For example, in the previous article’s demonstration we used the STM32F429I Discovery Kit which has a Cortex M4 MCU, a gyroscope, and an LCD screen (among others). In this article’s demonstration we don’t particularly need the LCD so we will use the STM32F4 Discovery Kit. It too has a Cortex M4 MCU, with an accelerometer instead of a gyroscope and no LCD. It is even less expensive than the F429I version – approximately $15.

Our industrial-strength programming language is Ada 2012, an object-oriented real-time / embedded-systems language widely used in commercial and military aircraft and high-speed trains, among many other high-integrity applications. If you have flown on a modern commercial airliner the chances are excellent that Ada code was in the critical software on-board. AdaCore’s GNAT technology implements Ada 2012 (the most recent version of the language standard) for newer ARM targets, including a freely available Community edition for students and hobbyists that supports some Discovery Kits out-of-the-box. (Other Discovery Kits are easily ported by users.) This free toolchain, including a full-strength IDE named GPS (GNAT Programming Studio), is available at http://libre.adacore.com/ for download.

In the previous article (part one) we demonstrated how to use a Discovery Kit and Ada to develop an object-oriented interface to the most basic NXT sensor: the touch sensor. Doing so required exploration of the simple external electronic circuit required and its effect on the software design of an API for discrete inputs. In this article we will add a basic interface for driving the NXT motors. Like the touch sensor, interfacing with the motors requires an external circuit so we discuss the requirements and options. Our new demonstration will use the touch sensor to control NXT motor speed, with on-board colored LEDs displaying the relative safety (slow, cruising, fast, red-lining).

Fundamental Language Concepts

The previous article introduced many of the important fundamentals of Ada. Most importantly, we covered “packages,” which are modules providing compile-time visibility control, and “private types,” which are types declared within packages in a specific way so that the types’ representations are hidden from clients. The combination of a package and a private type, along with associated operations, is used to implement abstract data types (ADTs), one of the most important and pervasive concepts in programming. ADTs are perhaps the key concept in Ada, much as using the notions of protected and private declarations for visibility control are central to class-oriented languages. We also introduced the notion of “generic units,” which are similar in concept to “templates” in C++ and other languages. We introduced object-oriented programming in Ada, and how to choose between, and implement, “interface inheritance” and “implementation inheritance.”  We also introduced the significant concept of contract-based programming in the form of subprogram “preconditions” and “postconditions” that are verified at run-time. All of these concepts were used in the demonstrated program and much will be used in this one as well. That was a lot of material that we will not cover again, however, so please refer to the first article if you have any questions. Note too that free learning material is available on the AdaCore website, at http://university.adacore.com.

Having explained the construction of abstract data types in Ada, we then introduced the Ada Drivers Library (ADL). The ADL provides device drivers, typically in the form of abstract data types, for the timers, UARTs, A/D and D/A converters, GPIO points, and other devices required to replace the NXT brick. The ADL supports a variety of development platforms from various vendors, including STMicroelectronics boards and some Discovery Kits out-of-the-box. The library is available on GitHub for both non-proprietary and commercial use here: https://github.com/AdaCore/Ada_Drivers_Library. Please consider adding new devices and drivers to the library!

Controlling the Motors

NXT Motors

There are a number of Lego motors available, but we focus on those included with the Mindstorms kit. These are 9-volt DC motors with significant gear reduction, producing high torque, and a precise rotation sensor. The motors rotate at a rate relative to the power applied, and can rotate in either direction. The polarity of the power lines controls the direction: positive rotates one way, negative rotates the other way.


Figure 1: NXT motor internals. (Source: LEGO)

Figure 1 illustrates the partial internals of the NXT motor, including the gear train in light blue, and the rotation sensor at the far left, in dark blue, next to the motor itself in orange. Comprehensive information about these motors is available from this excellent web site: http://www.philohome.com/nxtmotor/nxtmotor.htm.

We mentioned that the polarity of the applied power determines the rotation direction. That polarity control requires an external circuit, specifically an ‘H-bridge” circuit that allows us to achieve that effect.


Figure 2: H-bridge circuit showing power source, motor, and switches. (Source: http://en.wikipedia.org, created by Cyril BUTTAY)

Figure 2 shows the functional layout of the H-bridge circuit, in particular the arrangement of the four switches S1 through S4 around the motor M. By selectively closing two switches and leaving the other two open we can control the direction of the current flow, and thereby control the direction of the motor rotation.


Figure 3: H-bridge circuit showing direction options. (Source: http://en.wikipedia.org, created by Cyril BUTTAY)

Figure 3 illustrates two of the three possible switch configurations. The red line shows the current flow. Another option is to close two switches on the same side and end, in which case the rotor will “lock” in place. Opening all the switches removes all power and thus does not cause rotation. The fourth possible combination, in which all switches are closed, is not used.

COTS Motor Interface Hardware

Rather than build our own H-bridge circuit we use a low-cost product dedicated to interfacing with NXT motors and sensors. In addition to the H-bridge circuits, they also provide filters for the rotation sensor’s discrete inputs so that noise does not result in false rotation counts. There are a number of these products available.

One is the “Arduino NXT Shield Version 2” by TKJ Electronics: http://www.tkjelectronics.dk/ in Denmark. The product is described in their blog, here: http://blog.tkjelectronics.dk/2011/10/nxt-shield-ver2/  and is available for sale here: http://shop.tkjelectronics.dk/product_info.php?products_id=29 for a reasonable price. They also provide Arduino software drivers on GitHub. 


Figure 4: NXT Shield V2, top-down view (Source: TKJ Electronics)

The “NXT Shield” can control two NXT motors and a Mindstorms NXT Ultrasonic Sensor. Figure 4 shows the NXT Shield with the two motor connectors on the left and the sensor connector on the right. The headers match the Arduino headers so it can be placed on top of those boards, but of course that is not required.

The kit requires assembly from the components but it is just through-board soldering. As long as you get the diodes oriented correctly everything is straightforward. Figure 5 shows our build, already located in an enclosure and connected to the Discovery Kit, power, and two NXT motors.


Figure 5: Completed NXT Shield inside final enclosure (Source: Pat Rogers)

We supply 9 volts for the motors through a DC power jack on the back of the enclosure, seen on the left. The 5 volts for the on-board electronics comes from the Discovery Kit and is bundled with the white and green wires coming in through the left side in the figure. The enclosure itself is the typical tin box. (In this case it is one of the “Make with Ada” boxes. “Make with Ada” is a competition offering serious prize money for cool projects using embedded targets and Ada. See http://www.makewithada.org/ for more information.)

Rotation Sensor

The rotation sensor in the NXT motors is an optical rotary encoder, specifically a “quadrature rotary encoder.” These devices detect rotations and generate pulses accordingly over two discrete output lines. The pulses on these lines are driven by two distinct tracks, with sectors positioned 90 degrees out of phase with each other. By counting the number of signals over some time interval one can determine the rotation rate. By comparing the leading edges of the signals (e.g., the arrival time) we can determine the direction of rotation. Figure 6 illustrates the signals, in this case with clockwise rotation.


Figure 6: Rotary encoder signals (Source: https://en.wikipedia.org/)

The NXT Shield Kit connectors include the two wires for the inputs coming from the NXT rotation sensor so we just connect them to GPIO points on the Discovery Kit.

The interface kit is completely sufficient for interfacing to the motor and its encoder hardware so now we turn to the software.

The Motor ADT

The NXT motors are represented by an abstract data type named Basic_Motor, declared within a package named NXT.Motors. (We can envision more advanced motor abstractions, e.g., with continuous speed regulation, so this is the basic model.) Based on the hardware within the motor, we need the following components: a PWM signal generator for controlling power applied to the motor, a quadrature encoder interface for decoding the rotation signals, and two discrete outputs for controlling the H-bridge (on the NXT Shield Kit) for controlling the motor rotation direction.

As an abstract data type, Basic_Motor is defined as a private type:

We have elided some of the text of the package to hide some of the operations, just to keep things simple for the introduction of the facility.

On line 6 we bring in the Quadrature_Encoders package providing the Rotary_Encoder abstract data type. (Really this is a rotary encoder decoder , but nobody calls them that.) We use that type to declare the Encoder component on line 39. Similarly, on line 4 we bring in the STM32.PWM package and use that to declare the Power component on line 40. The PWM_Modulator type is essentially a wrapper for a hardware timer from package STM32.Timers and uses a timer channel for the output. That is the Channel component of line 41. Finally, the two discrete outputs controlling the H-bridge circuit are declared on lines 42 and 43, from package STM32.GPIO. You can tell that the GPIO, PWM, and Timers packages are part of the Ada Drivers Library because their package names start with STM32, the root of a set of packages declared in that library. The Quadrature_Encoders package is not part of the library but may be, in the future.

Before examining the implementation of the Basic_Motor abstract data type, i.e., the package body, we will show the interfaces for these other ADTs.

The Rotary Encoder ADT

The interface for the quadrature rotary encoder abstract data type is as follows:

Notice that the hidden representation of the Rotary_Encoder type on line 43 is just as an access type (a pointer) able to designate a Timer object. The additional text on line 43 tells the compiler that we never intend to dynamically allocate a Timer using this type. That’s so because all the Timer objects are already declared, in package STM32.Device. That’s the Ada Drivers Library package that represents any given MCU supported, each of which can have different numbers of on-chip devices (GPIO points, A/D and D/A converters, USARTs, and so on). To dedicate a given timer to a Rotary_Encoder object, clients specify one of the Timer objects in STM32.Device as the actual parameter to a call to Initialize_Encoder, via the parameter on line 28. The two discrete inputs from the encoder hardware are connected to GPIO points that are then supplied as parameters to the procedure (lines 26 and 27).

Note the precondition on procedure Initialize_Encoder at lines 31 and 32. These two checks ensure that we are using a timer that has a 32-bit counter and that it can count both up and down – not all timers do.

The reason that a Rotary_Encoder object is just internally represented as a (pointer to) a timer is that the STMicro timers have built-in support for rotary encoders. Once properly configured, the two incoming signals act as the clock that drives the timer so the timer’s counter always reflects the current encoder count. The timer will even tell us the rotation direction. We don’t need to write any interrupt handlers or count signals, much less time them. All that our software need do is configure the timer and then sample the registers whenever we want. No processor overhead and trivial software – beautiful.

The PWM ADT

The interface of the PWM modulator abstract data type is quite involved because the underlying timers have a very extensive set of PWM capabilities. Too many to explain for our purposes here. Therefore we will show only a very small part of it, eliding the rest.

Like the Rotary_Encoder type, the PWM_Modulator type is also based directly on the built-in capabilities of the hardware timers. We need a little more information for this one so we use a record type to hold the additional components. The pertinent capability for our demonstration is setting the duty cycle. This routine sets the pulse width such that the timer’s PWM output is active for the requested percentage of time.

Motor ADT Implementation

Now that we’ve seen the interfaces for the components in a Basic_Motor object we can examine the implementation of the Basic_Motor operations. This code will be in the package body corresponding to the package declaration for NXT.Motors we saw earlier. We need not see the implementation of all the operations to get the idea, so we will again elide parts of the code. The remaining code should be clear.

Remember that the H-bridge controls the motor rotation direction by closing two switches out of the four in the bridge circuit. (See Figure 2.) We control those switch selections via the motor’s two “polarity” discrete outputs. Setting and clearing the two outputs selects the switches and thereby sets the direction, as seen in lines 17-18 and 20-21. To stop the motor and “lock” the rotor at that point, procedure Stop sets both discrete outputs high (lines 30 and 31) and then applies full power. Current then flows from the “entrance” at the top of the H through the motor coil and then back out via the other side of the top of the H, so the rotor cannot rotate at all. Another procedure named Coast just cuts power to the motor, allowing the rotor to rotate to a stop.

NXT Shield Interface

Lastly, we need a software interface for the NXT_Shield. As yet, we have only implemented the NXT motor driver, not the ultrasonic sensor, so the code only declares two motor objects:

The body of the package (not shown) contains the body of procedure Initialize_Hardware declared on line 8. That procedure body calls the Initialize routine for type Basic_Motor, once each for the two motor objects declared on lines 5 and 6. These calls to the motor object's Initialize routine specify the choices for the timers and GPIO points used by the PWM and rotary encoder components within the motors. The body of Initialize_Hardware hard-codes the timer and GPIO selections passed to the Initialize routine calls.

The last step is to physically connect the NXT Shield Kit to the Discovery Kit’s GPIO points specified in the body of procedure Initialize_Hardware. To do this requires knowing which pins on the NXT Shield are connected to the two motor connectors on that board. The pins are on the two headers on the left of the board shown in Figure 7. (The overall layout shown may differ slightly from configurations that are currently available.)


Figure 7: NXT Shield headers (Source TKJ Electronics)

The mapping of motor functionality to NXT Shield header pin is shown in Table 1. The labels for the individual header pins in Figure 7 are for Arduino use so we just use the pin number in the table.

Table 1: NXT Shield motor functionality mapping

Figure 5 shows the actual wiring to the NXT Shield from the Discovery Kit. The wires for Motor 1 are white; those for Motor 2 are green.

Demonstration

Now let’s put the hardware and software together into a demonstration.

The full system consists of the STM32F4 Discovery Kit, the NXT touch sensor and associated external circuit (from the previous article’s demonstration), the TKJ NXT Shield located within the Make With Ada enclosure, the two NXT motors, and a portable battery. The battery provides connections for both 9 volts and 5 volts so is ideal for driving the motors and supplying the two boards, respectively. Figure 8 shows the full system.


Figure 8: Complete Demonstration System (Source: Pat Rogers)

The Discovery Kit is also within an enclosure, one made of Lego parts for use with Lego robots. Although not visible in the figure, the tin box has a Lego piece connected to it on the back, so that it too can be attached to Lego robots.

The two motors are connected but only one is driven in this demo program.

The code for the demo program is below, with certain parts of it elided for simplicity. We will examine some of those parts as well, after first explaining their context (the main program itself).

The demonstration simply loops forever, incrementing the power applied to Motor1 by 25% each iteration and setting an LED on the board in a color corresponding to the current rotation rate. At the top of the loop the program waits for the user to toggle the touch sensor before continuing.

The names used in the case statement to determine which LED to turn on are just subtypes, declared for the sake of readability:

The actual values in the ranges likely depend on both the battery’s maximum power and the current power available within it. You may need to tune them for your battery.

The Encoder_Delta function samples the encoder counts before and after delaying for the requested sample interval:

The Clock function comes from package Ada.Real_Time, brought in at the top of the main program. The values it returns represent “now” on a monotonic timeline. Line 7 adds the requested sample interval to that Clock result and delays until that time is reached.

The rest of the main program not shown, particularly procedure All_Stop, does not introduce anything new so we will skip it.

Building and Running the Program
You can build and run the executable on the command line via these commands:

Line one builds the entire project. Line two converts the executable to a binary image suitable for downloading to the board. Line three downloads the converted image starting at the address indicated. Note that the generated executable will be located in a subdirectory of the “obj” directory. By default that subdirectory is the “debug” directory but there is another “production” subdirectory possible. The choice reflects the builder switches applied. You control those switches and the corresponding subdirectory by a “scenario variable” named “PLATFORM_BUILD” defined by the project files. To override the default, specify the value of the scenario variable when building:

gprbuild -P demo_2.gpr -XPLATFORM_BUILD=Production –p

Now steps two and three above would reference the files in the “objproduction” subdirectory instead. The “-p” switch tells the builder to create any missing directories.

Doing all this is much easier using the IDE, as is debugging. The Community edition toolchain includes the IDE (named “GPS”) and we have provided the full demonstration project to work with it. There is a tutorial for using GPS on the same website used to download the toolchain itself, but the gist of it is to invoke GPS on the specified project:

gps -P demo_2.gpr

Then, within GPS, use the icon to download to the board. That icon will invoke an action that first builds the project, if necessary, and automatically converts the image format. (If you get a warning in GPS saying that a directory is missing, it is one of these “obj” subdirectories. You can ignore it. A build will create it, depending on the value of the scenario variable.)

What’s Next

In the next article in this series we will explore advanced sensors that require, for example, I2C communication.

Source Code Availability

The full, buildable project for this article is available here: https://github.com/AdaCore/Robotics_with_Ada.  We will update the project content for each article as each becomes available.


Dr. Patrick Rogers has been a computing professional since 1975, primarily working on microprocessor-based real-time applications in Ada, C, C++ and other languages, including high-fidelity flight simulators and Supervisory Control and Data Acquisition (SCADA) systems controlling hazardous materials. Having first learned Ada in 1980, he was director of the Ada9X Laboratory for the U.S. Air Force’s Joint Advanced Strike Technology Program, Principle Investigator in distributed systems and fault tolerance research projects using Ada for the U.S. Air Force and Army, and Associate Director for Research at the NASA Software Engineering Research Center. Dr. Rogers is the head of the US Technical Advisor Group (TAG) to ISO/IEC JTC1/SC22 Working Group 9, the group that is responsible for the definition and evolution of the Ada language. He has B.S. and M.S. degrees in computer systems design and computer science from the University of Houston and a Ph.D. in computer science from the University of York, England. As a member of the Senior Technical Staff at AdaCore, he specializes in supporting real-time/embedded systems developers, creates and provides training courses, and is a developer of the bare-board products for Ada.

Leave a Reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.