John Cutler john.cutler@aggiemail.usu.edu http://cutler.is-a-geek.org (435)232-6592 Robin Bailey robin@robinbailey.com (435)787-2373 April 14, 2008 Dr. YangQuan Chen Department of Electrical Engineering Utah State University Dear Dr. Chen: The following document contains our final project report for Design III. This includes details of our whole design process. The original goal of this project was to create a modular robot and interface it with a computer using Microsoft Robotics Studio. The original hardware design for our modular robot was more time consuming than we expected. As a result, we simplified the hardware for the modular robot, but we ended up integrating Robin Bailey’s Robo-Magellan project as well. There are two main components of the development of this project. The first part, the hardware development, is currently complete. The second part, the software interface to Microsoft Robotics Studio, will be completed April 28, 2008. Sincerely, Robin Bailey John Cutler

Senior design final report for

Microsoft Robotics Studio and

Modular Mobile Robotics Platform

ECE 4840 Senior Design III

April 29, 2008

Robin Bailey

John Cutler

Instructor’s Approval ____________________________________ __________

Dr. YangQuan Chen Date Department of Electrical & Computer Engineering Utah State University

Executive Summary

The intent is to build a modular robotics platform that can be used for

experimentation and educational purposes. Modules that perform specific

functions will be linked together using a serial bus. This serial bus will deliver

control commands as well as get status and information from the modules. These

individual modules can then be controlled from a PC using software developed in

Microsoft Robotics Studio 1.5.

A second robot named Robo-Magellan will also be interfaced to a PC

using Microsoft Robotics Studio. The hardware design of this second robot falls

under the scope of a separate class and will not be covered here.

i | P a g e

Table of Contents 1 Introduction .............................................................................................................................. 1

1.1 Background ...................................................................................................................... 1

2 Review of Conceptual and Preliminary Design ......................................................................... 3

2.1 Design Objectives ............................................................................................................ 3

2.2 Design Specifications ....................................................................................................... 3

2.3 Engineering approach ...................................................................................................... 4

2.4 Preliminary System Overview .......................................................................................... 4

2.4.1 High Level Control ................................................................................................... 5

2.4.2 External Communication ......................................................................................... 6

2.4.3 Communication Module .......................................................................................... 6

2.4.4 Common Control Bus (CCB) ..................................................................................... 7

2.4.5 Motor Module ......................................................................................................... 8

2.4.6 Proximity Sensor Module ........................................................................................ 8

2.4.7 Bumper Module ....................................................................................................... 8

3 Final Design Details................................................................................................................... 8

3.1 Changes to the Objective of the project ......................................................................... 9

3.2 Changes to the Common Control Bus ............................................................................. 9

3.3 Microcontrollers .............................................................................................................. 9

4 Hardware Implementation ..................................................................................................... 10

4.1 Robo‐Magellan Platform ............................................................................................... 10

4.1.1 Hardware Overview ............................................................................................... 10

4.1.2 Firmware Overview ............................................................................................... 24

4.1.3 Instruction Overview ............................................................................................. 26

4.2 Tandy‐Bot Platform ....................................................................................................... 27

4.2.1 Hardware Overview ............................................................................................... 27

4.2.2 Firmware Overview ............................................................................................... 33

4.2.3 Instruction Overview ............................................................................................. 34

5 Software implementation ...................................................................................................... 36

5.1 Overview ........................................................................................................................ 36

5.2 Magellan Robot Software .............................................................................................. 37

ii | P a g e

5.2.1 Custom “Brick” Service .......................................................................................... 37

5.2.2 Generic Motor Service ........................................................................................... 43

5.2.3 Other Generic Services .......................................................................................... 47

5.2.4 Magellan Behavior Control .................................................................................... 47

5.3 Tandy‐Bot Software ....................................................................................................... 48

5.3.1 State ....................................................................................................................... 48

5.3.2 Generic Differential Drive Service ......................................................................... 49

6 Project Testing ........................................................................................................................ 50

6.1 Hardware Test Results ................................................................................................... 50

6.2 Software Test Results .................................................................................................... 50

6.2.1 Communication Configuration .............................................................................. 50

6.2.2 Simulating hardware communication ................................................................... 51

6.2.3 Testing State Changes ........................................................................................... 52

6.2.4 Testing Notifications .............................................................................................. 53

7 Project Costs ........................................................................................................................... 55

7.1 Budget ........................................................................................................................... 55

7.2 Parts and Costs List ........................................................................................................ 55

7.2.1 Modular Robot ...................................................................................................... 55

A Project Management .............................................................................................................. 57

A.1 Project Management Documents ................................................................................. 57

B Microsoft Robotics Studio Overview ...................................................................................... 58

B.1 Background .................................................................................................................... 58

B.2 MSRS Quick Overview ................................................................................................... 60

C Code ........................................................................................................................................ 61

C.1 Magellan MSRS Interface .............................................................................................. 61

C.1.1 MagellanTypes.cs .................................................................................................. 61

C.1.2 Magellan.cs ............................................................................................................ 71

C.1.3 MagellanMotor.cs ................................................................................................. 78

C.1.4 MagellanEncoder.cs .............................................................................................. 80

C.1.5 MagellanSonar.cs .................................................................................................. 84

C.2 RMInterface (Program used for testing services using a null modem cable) ............... 86

C.3 Tandy‐Bot MSRS Interface ............................................................................................. 88

iii | P a g e

C.3.1 TandybotTypes.cs .................................................................................................. 88

C.3.2 Tandybot.cs ........................................................................................................... 97

C.3.3 TandyDifferentialDrive.cs .................................................................................... 104

C.4 Magellan Firmware ...................................................................................................... 109

C.5 Tandy‐Bot Firmware .................................................................................................... 134

List of Figures Figure 2‐1: Preliminary design overview ......................................................................................... 5

Figure 2‐2: Microsoft Visual Programming Language ..................................................................... 6

Figure 2‐3: PIC18F4550 .................................................................................................................... 7

Figure 3‐1: Final design overview .................................................................................................... 9

Figure 4‐1: Robo‐Magellan Robot ‐ Top View ............................................................................... 10

Figure 4‐2: Robo‐Magellan Hardware Block Diagram ................................................................... 11

Figure 4‐3: BDMicro Mavric‐IIB Microcontroller ........................................................................... 12

Figure 4‐4: BDMicro Mavric‐IIB on the Robot ............................................................................... 13

Figure 4‐5: Devantech SRF04 Ultrasonic Ranger ........................................................................... 13

Figure 4‐6: SRF04 Pin‐outs ............................................................................................................. 13

Figure 4‐7: SRF04 Mounted on Servo ............................................................................................ 14

Figure 4‐8: SRF04 Beam Pattern .................................................................................................... 14

Figure 4‐9: SRF04 Timing Diagram ................................................................................................ 15

Figure 4‐10: SRF04 Calibration and Characterization .................................................................... 16

Figure 4‐11: SRF04 Schematic ....................................................................................................... 16

Figure 4‐12: Sharp IR Ranger on Robo‐Magellan .......................................................................... 17

Figure 4‐13: Sharp Family of IR Rangers ........................................................................................ 18

Figure 4‐14: Operation in Infrared Ranging .................................................................................. 18

Figure 4‐15: Sharp IR Ranger Product Line .................................................................................... 19

Figure 4‐16: Sharp IR Calibration and Characterization ................................................................ 20

Figure 4‐17: Typical Response Curve ............................................................................................. 20

Figure 4‐18: Strategic Sharp IR Positioning ................................................................................... 20

Figure 4‐19: Sharp IR Cross‐Firing Geometry ................................................................................ 21

Figure 4‐20: HMC6352 Digital Compass ........................................................................................ 21

Figure 4‐21: HMC6352 Dimensions ............................................................................................... 21

Figure 4‐22: Gear‐Box mounted Hall Effect Sensor ....................................................................... 22

iv | P a g e

Figure 4‐23: GPS on Robot ............................................................................................................ 23

Figure 4‐24: Coyote Datacom Radio .............................................................................................. 24

Figure 4‐25: Command Line Programming in Cygwin ................................................................... 25

Figure 4‐26: Tandy‐Bot Robot – Isometric View ........................................................................... 27

Figure 4‐27: Tandy‐Bot Hardware Block Diagram ......................................................................... 28

Figure 4‐28: Communication Module Board ................................................................................. 28

Figure 4‐29: Communication Module Schematic .......................................................................... 29

Figure 4‐30: Module Interface on Comm. Module ....................................................................... 30

Figure 4‐31: Generic Module ......................................................................................................... 31

Figure 4‐32: Motor Drive Assembly ............................................................................................... 31

Figure 4‐34: Motor Module and Assembly .................................................................................... 32

Figure 4‐33: Photo‐Reflector Schematic ....................................................................................... 32

Figure 4‐35: Sonar Module in Place .............................................................................................. 33

Figure 5‐1: Microsoft Robotics Interface Overview ...................................................................... 36

Figure 5‐2: UpdateRobot flowchart............................................................................................... 41

Figure 5‐3: Notification flowchart ................................................................................................. 41

Figure 5‐4: Magellan Brick Service Initialization ........................................................................... 42

Figure 5‐5: Receive Thread ............................................................................................................ 43

Figure 5‐6: DSS Manifest Editor ‐ Magellan Motor Service ........................................................... 45

Figure 5‐7: Magellan Motor Service Initial State ........................................................................... 45

Figure 5‐8: Choosing a manifest .................................................................................................... 46

Figure 5‐9: Magellan Robot being controlled by an Xbox 360 Controller ..................................... 47

Figure 6‐1: com0com virtual null modem configuration .............................................................. 51

Figure 6‐2: Hardware simulation software .................................................................................... 51

Figure 6‐3: Testing state update – Before ..................................................................................... 52

Figure 6‐4: Testing state update – After ........................................................................................ 53

Figure 6‐5: VPL program used to test encoder notifications ........................................................ 53

Figure 6‐6: Encoder test result ...................................................................................................... 54

Figure A.1‐1: MSRS/Modular Robot Gantt Chart .......................................................................... 57

Figure A.1‐2: Robo‐Magellan Gantt Chart ..................................................................................... 58

Figure B.2‐1: DSS Service Components ......................................................................................... 60

1 | P a g e

1 Introduction The intent is to build a modular robotics platform that can be used for experimentation and educational purposes. Modules that perform specific functions will be linked together using a serial bus. This serial bus will deliver control commands as well as get status and information from the modules. These individual modules can then be controlled from a PC using software developed in Microsoft Robotics Studio 1.5. A second robot named Robo‐Magellan will also be interfaced to a PC using Microsoft Robotics Studio. The hardware design of this second robot falls under the scope of a separate class and will not be covered here.

1.1 Background On Dec 16th 2006, Microsoft founder Bill Gates published an article in Scientific American titled, “A Robot in Every Home.” The article itself has acted as a “call to arms” for much of the robotics community. Gates said:

[T]he challenges facing the robotics industry are similar to those we tackled in computing three decades ago. Robotics companies have no standard operating software that could allow popular application programs to run in a variety of devices. The standardization of robotic processors and other hardware is limited, and very little of the programming code used in one machine can be applied to another. Whenever somebody wants to build a new robot, they usually have to start from square one. Back in the early days of the personal computer, we realized that we needed an ingredient that would allow all of the pioneering work to achieve critical mass, to coalesce into a real industry capable of producing truly useful products on a commercial scale . . . [I]t seemed clear to me that before the robotics industry could make the same kind of quantum leap that the PC industry made 30 years ago, it, too, needed to find that missing ingredient . . . The goal was to see if it was possible to provide the same kind of common, low‐level foundation for integrating hardware and software into robot designs (Gates, Bill. “A Robot in Every Home.” Scientific American January 2007).

Microsoft’s answer to provide robotics with a foundation is two‐part. First a

unifying software package called Microsoft Robotics Studio has been developed by a MS team lead by Tandy Trower, a 25‐year Microsoft veteran. Second, was Gates’ call for researchers and developers to pursue unification in robotics in general.

Tandy Trower’s team at Microsoft has developed the software suite that is

Gate’s and Microsoft’s answer to the challenge. Microsoft Robotics Studio is a Windows‐

2 | P a g e

based environment for hobbyist, academic and commercial developers to create robotics applications for a variety of hardware platforms. The Microsoft Robotics Studio includes a lightweight REST‐style, service‐oriented runtime, with a set of visual authoring and simulation tools. It allows easy programming and debugging of a robot using Microsoft’s Visual programming language where users without programming experience can drag and drop components and services and connect them together creating a program. Robotic applications can also be developed using Visual Studio and Visual Studio Express (C# or VB.NET) for those who prefer these programming languages.

Robotics Studio also makes asynchronous programming simple. The Concurrency and Coordination Runtime (CCR), which is part of the foundation for Robotics Studio, greatly simplifies handling asynchronous input from multiple sources. Decentralized Software Services (DSS), another key feature of Robotics Studio, makes it easy to connect and work with decentralized processes. By using Robotics Studio to control the behavior of the robot, we hope to make the software modular in a way that will match the hardware. Developing the software in a modular fashion also allows for reuse of code, which will simplify programming and save time when adapting the robot to a new task. Gates’ call for a unification of industry and academia has resulted in, among other efforts, a response from the U.S. Government. A Bi‐Partisan effort was announced called the Congressional Caucus on Robotics by Pennsylvania Congressman Mike Doyle and Tennessee Congressman Zach Wamp. A June 20, 2007 White House Press Release announced:

The preliminary, proposed goals of the bipartisan Robotics Caucus will include, but are not limited to:

Increasing general awareness of robotics industry challenges and issues

among Members of Congress and policy analysts in the federal government; Educating Members of Congress and congressional staff on current and future research, development, and utilization initiatives involving robotics; Serving as a forum where robotics‐related policy issues can be discussed and debated; and

Ensuring that our nation remains globally competitive as the robotics industry rapidly expands and begins to exert a profound effect on the way our citizens live their lives (White House Press Release. Washington DC. June 20, 2007).

3 | P a g e

2 Review of Conceptual and Preliminary Design With a major part of the robotics industry, including research, pursuing the ideals laid out by Gates, there is a need for both software and hardware unification. Microsoft’s answer addresses the software problem with Microsoft Robotics Studio. What remains is the establishment of standardized robotics hardware.

2.1 Design Objectives This project proposes a solution that comes in the form of an open source and

standardized robotics architecture that attempts to be reminiscent of early Personal Computer (PC) architecture. This expandable hardware architecture is followed up by integration into Microsoft Robotics Studio. The major goal of this project is to establish a possible standard for robotics architecture that would allow for easy expandability and implementation.

This project entails developing and building a production capable prototype of

such a system. The robot platform must allow for flexible and expandable use. The hardware must also be easy to expand using a common bus architecture. Each hardware module must also be recognized and interfaced with a PC.

2.2 Design Specifications The following is a list of objectives that need to be accomplished in order to make

this robot a reality:

1. Define the common bus (the Spine) that will be used to connect the modules. This common bus will be the backbone that will provide the communication between all the different modules. This bus will have to be robust but flexible.

2. Build essential modules. These modules will include the motor control module, the communication/coordination module and basic sensor modules such as sonar and/or bumper sensors.

3. Define framework for communication between the communication module and the PC. A flexible method for communication between the computer that will act as the brain of the robot and the communication/coordination module needs to be developed. This framework needs to include methods for discovering what modules are connected and coordinating communication between the services that will control them from Robotics Studio.

4 | P a g e

4. Services for individual modules need to be developed in Robotics Studio. These services will enable the modules to be controlled from Robotics Studio. These services will allow users to interact with the robot in MS Robotics Studio 1.5.

2.3 Engineering approach Development of a common bus to facilitate communication between different

modules is not a new concept. In fact there are several different common serial bus specifications already in use. It will save us a lot of time and effort to adopt one of these already developed buses and use that for our bus. The difficulty will be to choose a bus that will work well for us and make sure that it will meet our needs as far as reliability, flexibility and data throughput.

Each module will need a controller that is able to communicate with this serial bus, interpret commands sent to it, collect and send status information as well as interact with the other electronic components in the module to make them perform the desired function. This can be easily accomplished with a microcontroller. Choosing an existing common bus also has the advantage of being able to choose a microcontroller that has the serial bus implemented in hardware. Combining the programmability of a microcontroller with a hardware implemented serial bus will make developing additional modules easy and give each module the intelligence it needs to do its job. The protocol for communication between the communication/control module and the PC will use a request‐response style protocol. The PC will send a control request and the robot will respond with the results of the command. Physically interfacing the PC with a device can be accomplished using either RS‐232 or USB. We are also hoping to have a wireless link to the Mobile robot so it doesn’t have to be connected with cables.

2.4 Preliminary System Overview Figure 2‐1 provides a general overview of our preliminary design. The following

sections will now break down this overview and examine specific parts.

5 | P a g e

External CommunicationHigh Level Control

User Interface(MS Robotics Studio or other

program)

Computer Control(MS Robotics

Studio Service)RS-232

Communication Module

Wireless Connection

Motor Control

Proximity Sensor

Bumper Sensors

Expansion Module

Common Control Bus (I2C)

UART

Figure 2‐1: Preliminary design overview

2.4.1 High Level Control The high level control will control the behavior of the robot. Ideally this will be

hosted on a PC that has the power needed to perform computationally intensive tasks. Other things we are looking for include

Free or low cost Previously existing – we don’t want to create our own Universal standard Reusable code Modeling and simulation Object oriented approach Flexible programming methods

While there are no software packages out there that include perfectly everything we want, we found that Microsoft Robotics Studio (MSRS) is the closest. It is currently free to use. It has the potential of becoming a universal standard. It offers Object Oriented Programming and a reusable service based model. Included are the AGEIA physics engine and simulation environment. It also has many different programming options including C++, Visual Basic, C#, and the new Visual Programming Language (VPL) shown in Figure 2‐2.

6 | P a g e

Alternatives include using Robot Commander or writing our own interface from scratch. Robot Commander is nice, but it doesn’t seem as complete and as easily adaptable as MSRS. As for creating our own interface it is fairly easy to do but has limited expandability and is very time consuming. Not to mention that it is often difficult for other people to use.

All

these factors combined with the exciting possibilities of Microsoft Robotics Studio lead us to choose MSRS as the foundation for building the high level control for our robot.

2.4.2 External Communication For the communication link between the PC running the high level control and

the robot, we chose to use RS‐232. This is by far one of the more common protocols and it is also the easiest to implement. However, it also offers flexibility for expansion in the future. RS‐232 can be implemented using Bluetooth as well as USB. Also, most microcontrollers currently have support for RS‐232 communication that makes interfacing the microcontroller with the PC relatively simple.

We considered several other interfacing standards but selected RS‐232 because

it was the easiest to implement. It is very easy to add RS‐232 compatible wireless transceivers to this connection allowing for wireless communication between the PC and the robot. It has the bandwidth we require and makes it possible for future upgrades.

2.4.3 Communication Module The communication module provides translation between commands sent from

the high level control to the other modules connected to the common bus. It will also act as moderator for all communication on the common bus. It also will need to

Figure 2‐2: Microsoft Visual Programming Language

7 | P a g e

discover modules connected to the common bus and match the hardware configuration with its matching software configuration.

For this module we wanted to use a more powerful microprocessor that could also run basic behavioral code that would cause the robot to halt in case it lost communication with the PC. We looked at several different microcontrollers and microcontroller families including PIC, ATMEL and Motorola, but we decided on the PIC 18F4550 shown in Figure 2‐3. This particular microprocessor features

• 4K of program space • Operates at clock speeds up to 40MHz • Built in Hardware Support for I2C, RS‐232, CAN

and SPI • USB 2.0 hardware support for future upgrades • Low price

2.4.4 Common Control Bus (CCB) The common control bus will provide the medium and protocol for all communication between the modules connected in the modular robot. Requirements for the Common Control Bus include

• Multiple devices can be connected to the bus at the same time • Simple interface • Readily available on microcontrollers • Two‐way communication • Some method for addressing individual modules.

We first looked at developing our own protocol in software but the overhead and complexity were simply not worth it when there are already several standards available. We looked at several different standards including CAN, I2C, SPI, RS‐232, and RS‐485; of those standards our top two choices where I2C and RS‐485. Both of these standards are well defined, very common in microcontrollers and offered all the features we were looking for. Ultimately we chose to use I2C because it was slightly more common than RS‐485 and there are several existing devices that could be added to the robot that use the I2C standard. The I2C standard includes the following features:

• Two different modes for addressing that support up to 127 or 1023 unique addresses depending on mode

Figure 2‐3: PIC18F4550

8 | P a g e

• Three different speeds 100Kbit/s, 400Kbit/s, and 3.4Mbit/s • Only needs three wires to implement • Very common • Supports broadcasting a message to all addresses

2.4.5 Motor Module The motor module will provide basic movement for the robot. Each motor module would have a unique address. Multiple motor modules could combine to achieve the desired configuration of the robot. For example, two motor modules on a robot could be used to create a directional drive configuration. The module would include a microcontroller that controls the motor and communicates with the other modules using the common control bus as well as providing control signals for the motor. It would also include a motor speed controller, the motor and any gearbox required. For the motor microcontroller we chose the PIC18F2431 over all the others because it included I2C support as well as three 12 bit Pulse Width Modulation outputs, and it was cheap.

2.4.6 Proximity Sensor Module The proximity sensor would detect the distance between the robot and other physical objects. The module would include a microcontroller that would interpret the sensor output and communicate with other modules using the CCB, the sensor of our choosing, probably a sonar sensor, and any other circuitry needed to interface with the sensor.

2.4.7 Bumper Module The bumper module would detect when the robot has made physical contact with any object and send a message to the controller or motors causing the robot to come to a halt. This module would include contact sensors, a microcontroller that would interpret the sensors and communicate with other modules on the CCB.

3 Final Design Details Some of our original design decisions changed as the project progressed. There were also some significant changes to the objective of this project that affected the final design. This section will briefly discuss the changes made to the preliminary design.

9 | P a g e

External CommunicationHigh Level Control

User Interface(MS Robotics Studio or other

program)

Computer Control(MS Robotics

Studio Service)RS-232

Communication Module

Wireless Connection

Motor Control

Proximity Sensor

Bumper Sensors

Expansion Module

Common Control Bus (RS-232)

UART

Figure 3‐1: Final design overview

3.1 Changes to the Objective of the project The original project was to build a single robot and interface it with Microsoft Robotics Studio. At the same time we were working on this project, Robin was developing a second robot for a different project. With some encouragement from Dr. Chen we decided to add this second robot to our project. To make more time to work with this additional robot we decided to simplify the design of the modular robot, later named Tandy‐Bot.

3.2 Changes to the Common Control Bus We found that the complexity of programming for I2C soon became a hindrance. We were spending too much time trying to get I2C working properly and decided to change it to RS‐232, which was easy to implement and familiar to us. This, however, limited the number of devices that could be connected to the CCB to eight.

3.3 Microcontrollers We originally chose the PIC line of microcontrollers made by Microchip. We invested a fair amount of time and money into learning to program these microcontrollers. However, with the new time constraints we decided that it would be better for us to switch to a comparable microcontroller we were more familiar with. So to simplify things we switched to ATMEL microcontrollers that we were familiar with.

10 | P a g e

4 Hardware Implementation This section includes the full details on how the final hardware design was implemented to create the final working project. It will first address the non‐modular design in the Robo‐Magellan platform. In the second section, it will cover the hardware of the modular design of the Tandy‐Bot platform. Section 5 will cover the software design of these two robots.

4.1 Robo-Magellan Platform This section provides details of the hardware implementation of the Robo‐Magellan robot. The Robo‐Magellan Platform is an example of a Non‐Modular, or traditional, robot design. The electronics, firmware and instruction set will be discussed. A full code listing is located in the Appendix C.4. A picture of the Robo‐Magellan robot is shown in Figure 4‐1.

Figure 4‐1: Robo‐Magellan Robot ‐ Top View

4.1.1 Hardware Overview This sub‐section will cover details on the electronics hardware. Upon

examination of the block diagram of the Robo‐Magellan robot, as seen in Figure 4‐2, it

11 | P a g e

becomes obvious that the architecture is based heavily around a single powerful microcontroller. The major hardware elements will be described here.

Figure 4‐2: Robo‐Magellan Hardware Block Diagram 4.1.1.1 Microcontroller

The microcontroller of any traditional, non‐modular robot lies at the heart of the

design. The purpose of the microcontroller in this architecture is control of all the electronics on the robot. The microcontroller firmware is responsible for servicing all components. The firmware of traditional robot varied widely from design to design. The role of the microcontroller will be contrasted later with the Modular robot design.

For the Robo‐Magellan robot, the microcontroller is responsible for controlling

all sensors and actuators. Actuators of the robot include the motor speed controllers, or ESC, the steering servos and the four sensor servos. All of these are driven using a PWM signal, typical to hobby servos, featuring a 1000us‐2000us pulse with a period of 20ms. The sensors of the robot include two Devantech SRF04 Sonar Rangers, two Sharp GP2D12 Digital Infrared Rangers, one Digital Compass, one GPS, an odometer, and a CMUcam2 to be interfaced at a later time.

In choosing a fitting microcontroller, the required I/O, code space and

programming language were all serious factors. The chosen microcontroller is a Mavric‐

12 | P a g e

IIB by BDMicro shown in Figure 4‐3. It is a breakout board of the Atmel atmega128 along featuring on‐board power regulation and RS‐232 voltage level shifter.

Figure 4‐3: BDMicro Mavric‐IIB Microcontroller

The Mavric‐IIB features seven 8‐bit ports of I/O, including one full port of A/D Converters and two UARTs in either TTL or RS‐232 level voltages. The Atmega128 is also capable of driving six servos using timer interrupts. Programming and debugging for the Mavric‐IIB is done via a 10‐pin ISP header and JTAG header. The board features an external oscillator available in 16 MHz or 14.7456 MHz. The later speed was chosen in this application due to zero error at our planned baud rates. Figure 4‐4 shows the Mavric‐IIB board mounted on Magellan.

13 | P a g e

Figure 4‐4: BDMicro Mavric‐IIB on the Robot 4.1.1.2 Sonar Rangers

The first sensor selected for this robot was the ultrasonic ranging modules. The role of the sonar unit is primarily contactless object detection. Obstacle detection for a mobile robot is best done without contact. The ability to detect the presence of obstacles remotely gives the robot the ability to practice obstacle avoidance.

The chosen sonar unit was the Devantech SRF04 ultrasonic ranger module. At

the time, this was the most competitively priced unit. We decided against traditional Polaroid ranging units due to cost and power requirements. Since the inception of this project, several more sonic ranging units have become available. The Tandy‐Bot platform, discussed later, uses a newer sonar unit that features enhanced interfacing options. Pictured in Figure 4‐5 and Figure 4‐6 are the sonar units used.

Figure 4‐5: Devantech SRF04 Ultrasonic Ranger

Figure 4‐6: SRF04 Pin‐outs

14 | P a g e

The Robo‐Magellan platform uses two Sonar rangers mounted on servos located at the front of the top level of the robot. Since the beam width of the sonar units is wide (see Figure 4‐8), approximately 22.5 degrees, they give the robot a good ability to detect most obstacles. The servos give the robot the ability to re‐position the sonar units to areas of interest or to sweep the sonar units to get a better field of view. An example of these is using the left servo to wall follow with it turned 90 degrees out and then using the right sonar for obstacle avoidance by sweeping it left and right.

Figure 4‐7: SRF04 Mounted on Servo

Figure 4‐8: SRF04 Beam Pattern

15 | P a g e

The Devantech Sonar Ranging units are read using a trigger and an echo pin. To initiate a reading, the robot asserts the trigger pin then the time of sound reflection is seen on the echo pin. This is shown in the timing diagram in Figure 4‐9.

Figure 4‐9: SRF04 Timing Diagram

These sensors were then calibrated and characterized. During calibration, it was

found that a reading of 535us, for example, corresponded to a distance of 5 feet. It was also shown that readings from this sensor have a linear relationship to the distance. When no echo is detected, a value of about 18ms is returned. Using the entire data set, and excluding outliers, the sensor value was characterized by the equation:

5308.199.104 += xd where d is the distance in feet and x is the pulse width in µsec. This calibration curve and best fit equation, derived in Matlab, is shown in Figure 4‐10.

The SRF04 is designed to run off a 5v supply and consumes 30mA (50mA Max) or

current. It also uses a 40 kHz chirp. The maximum range, according to the specifications, is 10 feet; however, reading up to 15 feet occurred with moderate success. A schematic of the SRF04 is also shown in Figure 4‐11.

16 | P a g e

Figure 4‐10: SRF04 Calibration and Characterization

Figure 4‐11: SRF04 Schematic

17 | P a g e

4.1.1.3 Infrared Rangers The next sensor selected for this robot was the infrared ranging modules. The

infrared rangers serve multiple purposes. The first purpose is to monitor the terrain in front of each wheel. This is done by scanning a range of ground in front of the wheel. The secondary purpose is to provide the robot the ability to position itself in a centered fashion to a traffic cone as part of the Robo‐Magellan contest.

The infrared ranging sensors are located on the front axle of the Robo‐Magellan

platform and are mounted on servos. The orientation of these sensors is at a horizontal angle of 45 degrees with the two beams crossing. In this geometry, the right sensor is looking in front of the left wheel and vice‐versa. The servos, instead of panning the servos along the horizontal angle, rotate the sensors at a vertical angle from negative 45 degrees to positive 45 degrees from horizontal. This is shown in Figure 4‐12.

Figure 4‐12: Sharp IR Ranger on Robo‐Magellan

The selected infrared distance sensor was the Sharp GP2D02. This model has

been since discontinued and replaced by the GP2Y0D02. It is seen at the bottom of Figure 4‐13.

18 | P a g e

Figure 4‐13: Sharp Family of IR Rangers

These sensors operate on a different principle from the sonic ranging modules.

Instead of detecting the delay of a transmitted signal, they measure the return angle, with a CCD sensor, of a transmitted infrared light. This principle is shown in Figure 4‐14.

Figure 4‐14: Operation in Infrared Ranging

The GP2D02 was chosen due to four factors. Since it uses a digital interface it is

only active when a reading is taking place. Also, due to its digital design, it can communicate with a non‐analog capable microcontroller. It was also one of a few non‐analog sensors that reported a distance, not just a true or false to a threshold distance. The Sharp GP2D02 also had a reading range from 4 inches to 32 inches. The current product line of Sharp Rangers is shown in Figure 4‐15.

19 | P a g e

Figure 4‐15: Sharp IR Ranger Product Line

The Sharp GP2D02 sensors were then calibrated and characterized. During

calibration, it was found that readings from this sensor have a non‐linear relationship to the distance. A linear fit may be used, but a quadratic or cubic fit is better. Generally, a linear fit is used within certain ranges of red values. Using the valid range of the data set, and excluding outliers, the sensor value was characterized by the quadratic equation: 63.20959.410402.31 2 +−= xxd where d is the distance in feet and x is the returned byte. This calibration curve and best fit equation, derived in Matlab, is shown in Figure 4‐16.

20 | P a g e

Figure 4‐16: Sharp IR Calibration and Characterization

Figure 4‐17: Typical Response Curve

Another important fact in using Sharp IR Sensors is the behavior of the sensor to

report very close objects as very distant. This behavior is exhibited in Figure 4‐17. The best solution around this situation is the physical positioning of the sensor itself. If the sensor is located such that the invalid range is always clear. An example of strategies is shown in Figure 4‐18 and Figure 4‐19.

Figure 4‐18: Strategic Sharp IR Positioning

21 | P a g e

Figure 4‐19: Sharp IR Cross‐Firing Geometry

The next sensors for the Robo‐Magellan robot needed to provide navigational

data. This data included such variables as current heading, distance traveled and global position. These sensors are currently under development and fall outside the scope of this project. Therefore, they will only be briefly covered in this report. 4.1.1.4 Digital Compass

Knowing the heading of the robot becomes an important factor in position calculation and path planning. The heading data from the digital compass, along with the odometer data, is used to calculate the position of the robot.

The digital compass used is a HMC6352 available from SparkFun Electronics. It

takes a supply voltage of 2.7v to 5.2v. The compass interfaces via as I2C bus. The sensor is shown in Figure 4‐20 and Figure 4‐21.

Figure 4‐20: HMC6352 Digital Compass

Figure 4‐21: HMC6352 Dimensions

22 | P a g e

4.1.1.5 Odometer Knowing the distance traveled by the robot is also an important factor in position

calculation and path planning. The odometer and compass data is used for position calculation.

The implementation of an odometer on the Robo‐Magellan Platform was done

using a Hall Effect sensor mounted to the exterior of the gearbox in conjunction with two rare earth magnets, in opposite magnetic orientation, that are mounted to the surface of the primary spur gear in the gear box. As the gear rotates, the field near the sensor switches direction and triggers the sensor latch. This provides a rugged and reliable count of motor revolutions. The mounting of the gearboxes sensor can be seen in Figure 4‐22.

Figure 4‐22: Gear‐Box mounted Hall Effect Sensor

The output from the odometer is run into a small PIC microcontroller. This

controller solely monitors the Hall Effect sensor and outputs a 7bit count to the Mavric‐IIB board. The microcontroller may also be reset using the eighth bit. 4.1.1.6 GPS

The key player in the Robo‐Magellan goal is a GPS receiver. The GPS data is used for global localization and goal orientation. It is, however, not used for navigational computations. Navigational goals are computed using the difference in the actual and goal latitude and longitude. One strategy used is that the robot only needs a GPS fix to begin the course. The rest of the position calculation is done using compass and odometer data. Over time, if the relative position calculation becomes inaccurate, the GPS data may be used to readjust the relative to the global positions to bring them both into harmony.

The GPS used was a Rand McNally GPS receiver that was included with

StreetFinder Deluxe 2000. It features a true RS‐232 interface and takes a 5v supply from

23 | P a g e

a PS/2 connector. These connectors were removed and wired directly to the robot. A picture of the receiver is shown in Figure 4‐23.

Figure 4‐23: GPS on Robot

The GPS string, NMEA format, is decoded by a separate PIC microcontroller and

transmitted through a data switch that shares the transmit side of the wireless data radio. During GPS data transmission, control of the radio has been given to the GPS decoder circuit. 4.1.1.7 Wireless Data Link

The final part of the Robo‐Magellan platform is a wireless RS‐232 interface. This interface allows the controlling host PC to operate the robot remotely. A final realization of this robot will feature a small form factor laptop located onboard the platform.

The wireless radio system used is a Coyote Datacom CDR‐915L. It is shown in

Figure 4‐24 mounted on the robot. It features 1mW of transmit power along with an onboard antenna. Its operational voltage range is 8v to 14v. Data rates range from 2400 bps to 56kbps with an 8N1 format.

24 | P a g e

Figure 4‐24: Coyote Datacom Radio

4.1.2 Firmware Overview This sub‐section will briefly cover the firmware code architecture of the Robo‐

Magellan platform. 4.1.2.1 Firmware Language The firmware’s target device is an Atmel Atmega128 microcontroller. One of the

advantages to this controller is the variety of programming languages available. The language chosen was the GNU‐C library included with WinAVR programming IDE. Much of the appeal of GNU‐C was its portability and universal use of the language. Also, using a higher level language enhanced development time and effort. 4.1.2.2 Firmware IDE

The code development was accomplished using several methods. These methods were first, command line compilation using makefiles in the linux environment of cygwin running on a windows PC. Second, the use of a free editor, such as Notepad++, running in a windows environment was used. Third, programming of the controller was accomplished using a STK500 board. A complete copy of the code, including the makefile, is included in the Appendix.

A batch file was used to setup the environment and call cygwin. The .bat file

contains the following code:

@rem set the path to the GNU tools set PATH=%PATH%;C:\WinAVR-20071221\bin

25 | P a g e

@rem now invoke cygwin call C:\Cygwin\cygwin.bat

Programming the microcontroller consists of plugging in the programming cable into the 10‐pin ISP header on the microcontroller. The UART0 RX pin also needs disconnected from the controller to allow programming. The programmer is called from the cygwin environment using a command such as:

“$ avrdude –c stk500v2 –p atmega128 –U flash:Magellan.hex –P com5”

After such a command, the programmer attempts to connect to the microcontroller. If successful, it will check the signature then begin dumping the hex file. A screen shot of the make and programming process is shown in Figure 4‐25.

Figure 4‐25: Command Line Programming in Cygwin

26 | P a g e

4.1.3 Instruction Overview This sub‐section will briefly cover the communication protocol to interface with

the robot. 4.1.3.1 Command Structure The structure of commands to the Robo‐Magellan robot has a few unique

features. All commands are sent in frames of “[“ and “]”. Second, when the robot replies, the format is similar. The robot may be configured to respond to commands to ensure correct reception. Any instructions that occur without brackets or of improper syntax are ignored. 4.1.3.2 Command Summary

A listing of the current commands used for the Robo‐Magellan robot is listed below in Table 4‐1. Command Function Returns [gps] Report GPS Data [lat 46.1234]

[long 36.1234] [sat 06]

[sr#] Read Sonar Ranger Number # [sr# 64] [ir#] Read IR Ranger Number # [ir# 120] [odr] Read Odometer Value [odr 154] [orc] Clear Odometer Value [odc] [cmp] Compass Heading [cmp 127.34] [m# dist] Command Motor # to travel distance “dist” [m# dist] [s# val] Command Servo # to Position val [s# val] [dev arg] Sends “Device” argument arg [dev arg] [devr] Receives “Device” data [devr data...]

Table 4‐1: Robo‐Magellan Command Set

27 | P a g e

4.2 Tandy-Bot Platform This section provides details of the hardware implementation of the Tandy‐Bot. The Tandy‐Bot Platform is an example of a Modular robot design. The electronics, firmware and instruction set will be discussed. A full code listing is located in the Appendix. A picture of the Tandy‐Bot is seen below in Figure 4‐27.

Figure 4‐26: Tandy‐Bot Robot – Isometric View

4.2.1 Hardware Overview This sub‐section will cover details on the electronics hardware. Upon

examination of the block diagram, shown in Figure 4‐27, of the Tandy‐Bot there are multiple differences between the structures to that of the Robo‐Magellan platform shown in Figure 4‐2. The modular architecture is based around several microcontrollers working together in a plug‐n‐play fashion. The major hardware elements will be described here.

28 | P a g e

Figure 4‐27: Tandy‐Bot Hardware Block Diagram 4.2.1.1 Communication Module

The heart of the modular design is the communications module. The role of the communications module, or CM, is primarily directing of inbound and outbound traffic. It also functions as a common hub for multiple modular to connect to the host. On this design up to 8 modules may be connected. A picture of the Communication Module is shown in Figure 4‐28 . The full schematic of the Communication Module is shown in Figure 4‐29.

Figure 4‐28: Communication Module Board

29 | P a g e

Figure 4‐29: Communication Module Schematic

The design of the communications module required the use of a microcontroller

with a significant number of digital I/O pins as well as a UART. The controller chosen is an Atmel atmega16L. The 16L features low power consumption, up to 8 MHz instruction clock, 16K of program space and 4 ports of I/O.

The full realization of the communications module also uses a Max3232 RS‐232

to TTL transceiver wired to the TX and RX pins as well as the CTS and RTS pins. This allows both two way communication with the PC, but also handshaking if necessary.

The next major part of the Communications Module is the Write‐Bus control

logic. This consists of eight gates which control the routing of PC‐to‐Module traffic to the various modules.

30 | P a g e

Next, the CM has logic that controls sharing of the Read‐Bus. This is necessary in the merging data and the forbidding of data collisions. This is done using eight three state buffers all wired to a common output bus.

The handshaking between the Communications Module and the other modules

is done by the Communications controller automatically or manually by the host application. The code for the Communications Module can be found in the Appendix C.5. 4.2.1.2 Generic Module

The generic module is a microcontroller that interfaces with the communication module. It is required to have a TTL serial TX and RX lines as well as a RTS and CTS lines. The TX and RX are used to send data to and from the host PC through the CM. The RTS is an output pin that requests control of the Read‐Bus, which is the output bus from the robot to the host. The CTS is an input pin of the module that notifies it that the request of bus control has been given. The interface also provided a reference ground and a possible power output. The module connection can be seen in Figure 4‐30.

Figure 4‐30: Module Interface on Comm. Module

A generic module for this stage in the design was build from a simple

microcontroller. The board has three LED’s, two analog inputs, a Communication Module interface, and at some I/O to spare. Most modules on this robot used a single input and a single output. This controller was build from a modified microcontroller called the PunkRC Computer. It is shown in Figure 4‐31.

31 | P a g e

Figure 4‐31: Generic Module

4.2.1.3 Motor Module The role of the Motor Module is to control a generic motor. The motor assembly

on the Tandy‐Bot consists of a gear‐head motor, electronic motor speed controller (ESC), and an IR Photo‐reflective odometer. The entire assembly can be seen in Figure 4‐32. The interface to this hardware requires a PWM output signal to control the ESC, similar to a servo, and an Analog Input to monitor the Photo‐Reflector.

Figure 4‐32: Motor Drive Assembly

32 | P a g e

An attribute of a typical motor that makes

control difficult or intensive is the necessity to both signal a controller that requires a time sensitive, hence non‐interruptible, signal as well as monitor the state of another rapidly changing input. This sort of tasking can prove impossible for a microcontroller to handling other critical services concurrently.

The solution is a dedicated microcontroller like

a generic module microcontroller shown in Figure 4‐34. This microcontroller can be tasked with rudimentary motor control, while reporting periodically to higher level control. Here is where a modular architecture begins to make more sense, but it only gets better.

In the modular architecture, the main controller, aka the host, can issue

commands to a dedicated motor module and then move onto other important events. These commands may be standardized in such a fashion that most all motor modules can respond to the same command and control those motors specifically. Here is where it gets even better, when all motor modules are commanded similarly, and then the high level control code becomes generic. If a ‘smart’ high level controller can command any number of motor modules, then Motor Modules may be added to the system dynamically.

Figure 4‐34: Motor Module and Assembly

4.2.1.4 Sonar Module The role of the Sonar Module is to control an ultrasonic ranging sensor. The

Tandy‐Bot features three forward‐facing sonar units. The role of the Sonar Module is to control the sonar at the request of the host, then report as requested. Running a sonar

Figure 4‐33: Photo‐Reflector Schematic

33 | P a g e

module is a less intensive task, unlike a motor controller. However, there are some timing sensitivities that justify in keeping with our modularity. The sonar sensors were also each controlled by a separate module as shown in Figure 4‐35.

Figure 4‐35: Sonar Module in Place

The sonar is triggered nearly identical to that of the SRF04 sonar. However, the interfacing capabilities of this sonar unit are excellent. The traditional trigger and echo pin scenario was used similar to that described in Figure 4‐9: SRF04 Timing Diagram.

An advantage, similar to those of the motor module, is the critical timing where

the microcontroller is measuring the width of the return pulse. During this time, not other tasks may be serviced or the falling edge may be missed. This uses a digital input and output to trigger and read the sonar.

4.2.2 Firmware Overview This sub‐section will briefly cover the firmware code architecture of the Tandy‐Bot

platform. 4.2.2.1 Firmware Language The firmware’s target device is an Atmel Atmega128 microcontroller. One of the

advantages to this controller is the variety of programming languages available. The language chosen was the GNU‐C library included with WinAVR programming IDE. Much of the appeal of GNU‐C was its portability and universal use of the language. Also, using a higher level language enhanced development time and effort. 4.2.2.2 Firmware IDE

The code development was accomplished using the same environment as the Robo‐Magellan platform as described in 4.1.2.2 Firmware IDE. The environment and

34 | P a g e

programmer were identical. Since the target is also an Atmel atmega series microcontroller, the compiler is identical as well. A complete copy of the Tandy‐Bot code, including the makefile, is included in the Appendix C.5.

Programming is also done via the 10‐pin ISP header. The atmega16L does not use the RX pin, so disconnection of the RX pin is not necessary. The programmer is called from the cygwin environment using a command such as:

“$ avrdude –c stk500v2 –p atmega16 –U flash:modular.hex –P com5”

After such a command, the programmer attempts to connect to the microcontroller. If successful, it will check the signature then begin dumping the hex file. A screen shot of the make and programming process is shown in Figure 4‐25.

4.2.3 Instruction Overview This sub‐section will briefly cover the communication protocol to interface with

the robot. 4.2.3.1 Command Structure The structure of commands to the Tandy‐Bot has a few unique features in

addition to the traditional Robo‐Magellan type commands. First, traditional commands are sent to individual modules. The format is similar is that all commands are sent in frames of square brackets of “[“ and “]”, with one command per frame. The commands, however, vary from one module to another. Second, the modular design also takes commands specifically for the communications module.

The communication module commands differ slightly in their structure. The main

difference is the command(s) are placed in frames of angle brackets of “<” and”>”. Also multiple commands may be places within the same frames. This is called a shared frame. Any instructions that occur without angle brackets are ignored by communications module and any commands that occur without square brackets are muted by the communications module and thus ignored by the attached modules.

The reason behind the shared frame, when using the communication module

commands, is that all traffic is transparently passed to all enabled modules. However, when the CM hears a “<”, it will automatically mute transmission to any enable modules until the “>” character is sent. Using this method, the enabled module will still hear the initial “<” character before the communication module mutes them. For this reason, a shared frame will lessen rouge angle bracket frequency. 4.2.3.2 Command Summary

A listing of the current commands used for the Tandy‐Bot is described below. The first command set is those commands specifically for the communications module. These are listed in Table 4‐2: Communication Module Command Set. The command set

35 | P a g e

for individual modules varies between modules. A listing of the command set for the motor module is listed in Table 4‐3: Motor Module Command Set. The command set for the sonar module is listed in Table 4‐4: Sonar Module Command Set.

Command Function R0 Gives Communication Module (Module 0) Control of the Read Bus R# Gives Module # (1‐8) Control of the Read Bus (from Module to PC) RS Requests the status of the Request for Read Bus of the Modules. AR Enables Communication Module Auto‐Routing on Read Bus /AR Disables Communication Module Auto‐Routing on Read Bus WA Enables Write Bus to All Modules (from PC to Module) W# Enables Write Bus to Module #(1‐8) (from PC to Module) /WA Disables Write Bus to All Modules (from PC to Module) /W# Disables Write Bus to Module # (1‐8) (from PC to Module)

Table 4‐2: Communication Module Command Set

Command Function Returns [id #] Sets the Module ID Number Nothing [f ###] Next Motor Movement Forward for ### Rotations Nothing [r ###] Next Motor Movement Reverse for ### Rotations Nothing [g] Start Next Motor Movement [g#] [p ###] Set Power to the motor (000 to 100) Nothing [s] Motor Stop Nothing [o] Read Odometer [o# value]

Table 4‐3: Motor Module Command Set

Command Function Returns [id #] Sets the Module ID Number Nothing [s] Reads the Sonar [s# value]

Table 4‐4: Sonar Module Command Set

36 | P a g e

5 Software implementation This section contains details about the software written to interface the two robots to Microsoft Robotics Studio (MSRS). We will first discuss what is needed to interface a robot with MSRS and then look at the details of implementing these for both robots.

5.1 Overview To interface custom hardware with Microsoft Robotics Studio you first need to create a brick service. This brick service is an abstraction of the hardware that MSRS can understand and interact with. As you can see in Figure 5‐1, the custom brick service is the base layer that all other services must go through to access the hardware. For more information on what a service is please see Appendix B. After the brick service has been created it is often easiest to create custom services that mimic the behavior of generic services but send custom commands to your hardware as needed. For example, to access a motor it is often easiest to write a service that inherits the properties of the generic motor service. That is, having the same inputs and outputs but it is connected to your brick service and can translate input given to the generic service into a command that is understood by your hardware. There are many different generic services that can be used and expanded if needed. The services that we chose to implement include Generic Motor, Generic Sonar, and Generic Encoder. All code is listed in appendix C.1

Figure 5‐1: Microsoft Robotics Interface Overview

37 | P a g e

5.2 Magellan Robot Software The Magellan Robot is a robot that Robin Bailey created for a different class. Details on this particular robot can be found in Appendix C.1

5.2.1 Custom “Brick” Service When integrating custom hardware into Microsoft Robotics Studio the first step is to create a “Brick” service for your hardware. A “Brick” service provides a MSRS abstraction of your robot. Also included in this service is all the code needed to interface with the hardware as well as control all communication using the COM port. This “Brick” service is a decentralized software service (DSS) service and has four components of interest (see Appendix B for more details). These include the State representation, Main Port, Service Handlers and Notifications. All other properties of the DSS service are trivial and are listed in Table 5‐1. Properties Value Service Identifier /magellan Contract Identifier http://schemas.tempuri.org/2008/04/magellan.html Partners SubscriptionManager

Table 5‐1: Trivial properties for Magellan Brick Service 5.2.1.1 State The state at any given point in time represents a snapshot of the status of the hardware. This is represented in either a SOAP message or, if the service includes the necessary handlers, a XML document. Below is a copy of the XML state of Robo‐Magellan during testing.

<?xml version="1.0" encoding="utf-8" ?> <MagellanState xmlns:s="http://www.w3.org/2003/05/soap-envelope"

xmlns:wsa="http://schemas.xmlsoap.org/ws/2004/08/addressing" xmlns:d="http://schemas.microsoft.com/xw/2004/10/dssp.html" xmlns="http://schemas.tempuri.org/2008/04/magellan.html">

<ComPort>10</ComPort> <Debug>false</Debug> <EnableUpdateLoop>false</EnableUpdateLoop> <CommandDelay>50</CommandDelay> <UpdateLoop> <string>odr</string> <string>s1</string> <string>s2</string> <string>ir1</string> <string>ir2</string> <string>odr</string> <string>s1</string> <string>s2</string> <string>ir1</string>

38 | P a g e

<string>ir2</string> <string>odr</string> <string>s1</string> <string>s2</string> <string>ir1</string> <string>ir2</string> <string>odr</string> <string>s1</string> <string>s2</string> <string>ir1</string> <string>ir2</string> <string>gps</string> </UpdateLoop> <Connected>true</Connected> <Compass>0</Compass> <DriveMotor> <MotorSpeed>0</MotorSpeed> <WheelRadius>2.5</WheelRadius> <Od>0</Od> <Ticks>36.3</Ticks> </DriveMotor> <SteeringMotor> <MotorSpeed>0</MotorSpeed> <WheelRadius>1</WheelRadius> <Od>0</Od> <Ticks>0</Ticks> </SteeringMotor> <SR1> <Reading>0</Reading> <SensorPosition>330</SensorPosition> <InchPer>15.5</InchPer> <SensorAngle>0</SensorAngle> </SR1> <SR2> <Reading>0</Reading> <SensorPosition>30</SensorPosition> <InchPer>15.5</InchPer> <SensorAngle>0</SensorAngle> </SR2> <IR1> <Reading>0</Reading> <SensorPosition>15</SensorPosition> <InchPer>10</InchPer> <SensorAngle>0</SensorAngle> </IR1> <IR2> <Reading>0</Reading> <SensorPosition>345</SensorPosition> <InchPer>10</InchPer> <SensorAngle>0</SensorAngle> </IR2> <GPS> <Latitude>0</Latitude> <Longitude>0</Longitude> <Satelites>0</Satelites>

</GPS>

39 | P a g e

</MagellanState>

The components are broken down into three classes of components: motor, sensor and GPS. There are two drive motors, so the same data structure is used for both. The four different sensors are represented using the same data structure as well and the GPS is unique and has its own data structure. The motor data structure contains the current motor speed, the radius of the tire in inches, the number of odometer ticks detected and the odometer ticks per complete wheel rotation. Using this data you can easily calculate the distance traveled, rate of travel and other useful information. The data structure for the sensor contains the current reading from the sensor, the position on the robot it is physically mounted, and a conversion factor for converting the reading to feet or inches. There is also information included in the state that controls certain aspects of how the brick service interacts with the hardware. For example, the COM Port used to connect to the robot is stored here as well as commands that are sent in a loop to poll the sensors on the robot. The state for the brick listed above is saved in an xml file that can be edited and read as the service is loaded. This allows some behavior of the brick service to be changed without needing to recompile. 5.2.1.2 Main Port In MSRS all communication between services is handled by sending messages. The main port is where a service receives these messages. The main port primarily defines what messages the brick service can receive and the data structure of the message. For those familiar with programming terms these ports can be thought of as functions while the data structure of the body of the message represents the parameters of the function. Table 5‐2 contains a list of the ports defined in our brick service, a description of what they do and the data structure contained in the body of the message. Note that the standard default ports DsspDefaultLookup, DsspDefaultDrop, and Get use the standard MSRS implementation and are not listed. Port Description Message Body Structure

Data Type Name UpdateRobot Receives all incoming state String Command

40 | P a g e

updates from hardware. Some state variables are updated here others are delegated to one of the other ports so that notifications can be generated.

String Value

UpdateEncoder This handles messages from the robot that containing updates to the encoder on the drive wheel.

Int CurrentReading

UpdateGPS Handles messages that contain updates to the GPS data

Double Double Int

Latitude Longitude Satellites

UpdateSensor Handles updates for all the sensors.

String Int Int

HardwareID Angle CurrentReading

UpdateCompass Handles updates from the digital compass

Double NewCompass

EnableUpdateLoop Handles messages to enable/disable the hardware update loop.

Bool EnableUpdate

SendCommand Sends a command to the robot. (The services sending the command are responsible for formatting the command properly)

String Command

HttpGet This formats the State information to XML

None None

Subscribe This allows a service to subscribe to the brick service so it can receive Notifications

See MSRS documentation

Table 5‐2: Magellan Brick Service Main Port UpdateRobot is used to process all information sent from the hardware. This is the most used port, it decides if the message from the robot is valid and what to do with it if it is. In some cases it will update the state information itself in other cases it will delegate to a different port. This process is outlined in the flowchart in Figure 5‐2. Cases where the message is delegated to different port are discussed in section 5.2.1.3

41 | P a g e

Figure 5‐2: UpdateRobot flowchart 5.2.1.3 Notifications

Notifications allow the brick service to send messages to other services alerting them when events happen. When a message is sent to the main port of a service it is then passed to the message handler. The message handler then decides if it should send a message notification. The data structure of the notification message typically matches the same structure of the original message. For example UpdateEncoder, shown in Figure 5‐3, receives a message when the hardware sends a message with encoder information. If the value sent from the hardware matches the current value stored in the state, the message is discarded. However, if the message contains a value that is different, the state is updated and the original message is forwarded to all subscribing services. The services from Table 5‐2 we implemented as a strategy for dealing with notifications are: UpdateEncoder, UpdateGPS, UpdateCompass and UpdateSensor. It should be mentioned that UpdateSensor notifications trigger notifications when

UpdateGPS, UpdateCompass, UpdateSensor, UpdateEncoder

Message received

Message different from

state?Update StateYes

Discard message

No

Forward message to subscribers

Figure 5‐3: Notification flowchart

42 | P a g e

any of the four sensors change and it is the responsibility of the subscribing services to discard any notifications it does not want. Any services that need to receive notifications need to subscribe to the brick service by sending standard subscription requests. Subscriptions on to the brick service are handled using the subscription service. 5.2.1.4 Hardware Interface Communication between the robot and the PC uses standard RS‐232 serial communication at 9600 baud no Parity bit, 8 data bits and one stop bit. The code that implements the hardware interface falls under three main categories: initialization, reading data from the robot and writing commands to the robot. Initialization is done as the brick service is initializing shown in Figure 5‐4. Code responsible for initializing the hardware reads the COM port from the state of the service and then tests to see if the port can be opened. If the COM port cannot be opened it logs an error message and goes into a dead state where it does nothing but display the error message.

Figure 5‐4: Magellan Brick Service Initialization

If the COM port can be opened it attaches the read function that runs in a separate thread from the rest of the service. This allows both threads to operate independent of each other in a non‐blocking manner.

43 | P a g e

The read thread shown in Figure 5‐5 is a non‐blocking event triggered when data is received on the serial port. It first parses what it receives checking for errors and if there are none it breaks it down into command and value. These two values are then put into an UpdateRobot message and sent to the main port to be processed.

Figure 5‐5: Receive Thread

All commands that are sent to the robot are handled through the SendCommand message handler. This handler simply puts brackets around the command sent in the message and then writes it to the serial port. If the command sent in the message is incorrectly formatted or the robot does not understand it will be sent back from by the robot where the UpdateRobot handler will log it and store it in the error string in the state.

5.2.2 Generic Motor Service The generic motor service accepts values from ‐1 to 1, where 1 is forward full speed and ‐1 is reverse full speed and 0 is stop. It then scales this input properly and sends the appropriate command to the robot using the SendCommand port of the brick service. MSRS includes utilities to automatically generate at least a code template for creating new DSS services. The command used to generate code for the motor service is listed below. DssNewService.exe -service:MagellanMotor -dir:magellan\motor -namespace:Microsoft.Robotics.Services.Magellan.Motor -alt:"http://schemas.microsoft.com/robotics/2006/05/motor.html" -implement:bin\RoboticsCommon.dll This command creates all the files needed to create a new service using Visual Studio 2005 or 2008. Telling it to inherit from an existing service gives it many of the same properties that the original service has and allows you to customize it to fit your needs.

44 | P a g e

5.2.2.1 Magellan Motor State The state of the motor service is inherited from the generic motor service and we used it to distinguish between the six available motors or servos on the robot. The state of the motor services is shown in xml format below.

<?xml version="1.0" encoding="utf-8" ?> - <MotorState xmlns:s="http://www.w3.org/2003/05/soap-envelope"

xmlns:wsa="http://schemas.xmlsoap.org/ws/2004/08/addressing" xmlns:d="http://schemas.microsoft.com/xw/2004/10/dssp.html" xmlns="http://schemas.microsoft.com/robotics/2006/05/motor.html">

<Name>Drive Motor</Name> <HardwareIdentifier>2</HardwareIdentifier> <CurrentPower>0</CurrentPower> <PowerScalingFactor>1000</PowerScalingFactor> <ReversePolarity>false</ReversePolarity> - <Pose> - <Position

xmlns="http://schemas.microsoft.com/robotics/2006/07/physicalmodel.html">

<X>0</X> <Y>0</Y> <Z>0</Z> </Position>

- <Orientation xmlns="http://schemas.microsoft.com/robotics/2006/07/physicalmodel.html">

<X>0</X> <Y>0</Y> <Z>0</Z> <W>0</W> </Orientation> </Pose> </MotorState>

The state variables we use are Name, HardwareIdentifier, PowerScalingFactor and ReversePolarity. All the other values are ignored for now but could be used later for simulation of the robot.

Name - Used just to help identify the motor or servo that this service is attached too. HardwareIdentifier – Identifies the device messages should be sent to. See Table C-1 in appendix C for more details. PowerScalingFactor – The value used to scale the input range [-1, 1] to the appropriate value for the robot. (Table C-1) ReversePolarity – If true the polarity is reversed.

45 | P a g e

Implementing the motor service this way makes it so that you can use one motor service for all the servos and motors on the robot while writing only a small amount of code. Once the service is written, using it is a simple matter of configuring the initial state either by setting the initial state in VPL or building a Manifest using Microsoft DSS Manifest Editor shown in Figure 5‐6. Simply start the Manifest editor, drag the MagellanMotorService to the middle section. Once the MagellanMotorService is listed select it, then click the “Create Initial State” button.

Figure 5‐6: DSS Manifest Editor ‐ Magellan Motor Service

After clicking the “Create Initial State” button, values for the initial state can be entered. Figure 5‐7 shows the actual state used to connect to the drive motor. After the initial state information has been entered, save the manifest in the Robotics Studio folder where it will be used later. To use the manifest, open Microsoft Visual Programming language, drag and connect the generic motor service as needed. Select the Generic Motor Service by clicking on it. You will then have the option to use a manifest as shown in Figure 5‐8. Select choose import and then select the manifest you just saved.

Figure 5‐7: Magellan Motor Service Initial State

46 | P a g e

Figure 5‐8: Choosing a manifest

47 | P a g e

5.2.3 Other Generic Services The other generic services that apply to the Magellan robot include Generic Sonar and Generic Encoder. While we did create these services using the same techniques used to build the generic motor service, we found that they weren’t that useful. It was often just easier and faster to connect directly to the events on the Magellan Brick Service. Code for these other generic services is included in appendix C.1.

5.2.4 Magellan Behavior Control Once the services are built, simple behavior programming can be done using Microsoft Visual Programming Language. Figure 5‐9 shows a screen shot of the program we used to control all six actuators on the Magellan robot. It is difficult to develop complex behavioral control in Visual Programming Language. To accomplish this it is much easier to write a service that subscribes to the sensor events of the brick service and then sends control messages to the robot making it perform more complex behaviors.

Figure 5‐9: Magellan Robot being controlled by an Xbox 360 Controller

48 | P a g e

5.3 Tandy-Bot Software The Tandy‐Bot is a modular robot that currently moves using two motors arranged in a directional drive configuration. It has three sonar sensors on the front half of the robot that provide input. While it has many similarities with the Magellan robot, it also has a few differences. Much of the code for the Tandy‐Bot brick service is actually copied from the Magellan brick service. The following sections will cover what is different from the Magellan software. Trivial properties for the Tandy‐Bot brick service are listed in Table 5‐3. All code for the Tandy‐Bot is listed in Appendix C.3 Properties Value Service Identifier /tandybot Contract Identifier http://schemas.tempuri.org/2008/04/tandybot.html Partners SubscriptionManager

Table 5‐3: Trivial properties for Tandy‐Bot Brick Service

5.3.1 State The state of the Tandy‐Bot is really where the main difference between these two robots is most apparent. The state of the Magellan Robot is static and does not change. Every time the Magellan brick service starts it has the same data structure representing the same components configured in the same way. On the other hand, the Tandy‐Bot is modular and the configuration of the robot can change every time. To deal with this possibility of change, the state for the Tandy‐Bot must be built dynamically every time the service is started. The Tandy‐Bot can have up to eight modules connected to it. Each module can represent either a motor or a sonar sensor. Sometime in the future we plan to develop and add additional modules. Also, these modules can be connected in different arrangements by the user. To accommodate this dynamic configuration the state is broken down into two sections. One contains the general configuration settings that will not change; the other section is an array structure representing the eight modules. The data structures attached to this array are dynamically created and can represent different modules. When the service starts it uses the configuration information to connect to the robot and scan it to see what hardware is connected and where it is connected. Then it uses this information to build the dynamic structure.

<?xml version="1.0" encoding="utf-8" ?> <TandyBotState xmlns:s="http://www.w3.org/2003/05/soap-envelope"

xmlns:wsa="http://schemas.xmlsoap.org/ws/2004/08/addressing" xmlns:d="http://schemas.microsoft.com/xw/2004/10/dssp.html" xmlns="http://schemas.tempuri.org/2008/04/tandybot.html">

<Connected>false</Connected> <ComPort>10</ComPort>

49 | P a g e

<BaudRate>2400</BaudRate> <EnableUpdateLoop>false</EnableUpdateLoop> <LoopDelay>100</LoopDelay> <Debug>false</Debug> <LeftMotor> <HardwareID>7</HardwareID> <CurrentPower>0</CurrentPower> <NewPower>50</NewPower> <WheelRadius>2.5</WheelRadius> <Od>0</Od> <Ticks>24</Ticks> </LeftMotor> <RightMotor> <HardwareID>8</HardwareID> <CurrentPower>0</CurrentPower> <NewPower>50</NewPower> <WheelRadius>2.5</WheelRadius> <Od>0</Od> <Ticks>24</Ticks> </RightMotor> <LeftSonar> <HardwareID>6</HardwareID> <Reading>0</Reading> <SensorAngle>-30</SensorAngle> <InchPer>1</InchPer> <MaximumRange>1888</MaximumRange> </LeftSonar> <CenterSonar> <HardwareID>5</HardwareID> <Reading>0</Reading> <SensorAngle>0</SensorAngle> <InchPer>0</InchPer> <MaximumRange>1888</MaximumRange> </CenterSonar> <RightSonar> <HardwareID>4</HardwareID> <Reading>0</Reading> <SensorAngle>0</SensorAngle> <InchPer>30</InchPer> <MaximumRange>1888</MaximumRange> </RightSonar> </TandyBotState>

5.3.2 Generic Differential Drive Service The other main difference between the Magellan robot and the Tandy‐Bot is the drive configuration. The Tandy‐Bot uses a differential drive configuration. To take advantage of this we created a Generic Differential Drive Service. The Generic Differential Drive Service is a bit more intelligent than the Generic Motor Service. It envelopes two motors and two encoders into one service as well as adds special functionality that use all four components together to accomplish. Table 5‐4 has a breakdown of the main ports.

50 | P a g e

Port Description Message Body Structure Data type Name

AllStop Stops both motors None None DriveDistance Moves the robot a

specified distance (in ft) forward or backwards.

Double Double

Distance Power

EnableDrive If enabled other commands are followed if disabled then motor does not move.

Bool Enable

RotateDegrees Causes the robot rotate in place a specified distance

Double Double

Degrees Power

SetDrivePower Sets the power for both motors

Double Double

LeftMotor RightMotor

SetDriveSpeed Sets the drive speed for both motors

Double Double

LeftMotor RightMotor

Table 5‐4: Generic Differential Drive Service Main ports

6 Project Testing Project testing will be broken down into two main groups, hardware testing and software testing. While they are related, both the hardware and the software were developed independently, tested and then put together to verify that both parts worked.

6.1 Hardware Test Results

6.2 Software Test Results Through the development of the brick services it was necessary to monitor data sent and received through the COM port. It was also necessary to be able to simulate the hardware by sending specific data through the COM port to the program being tested.

6.2.1 Communication Configuration To meet the communication needs we used an open source software package to create two virtual COM ports link with a Null Modem cable. To find more information regarding this open source package visit http://com0com.sourceforge.net/. Figure 6‐1 shows the configuration screen

51 | P a g e

Figure 6‐1: com0com virtual null modem configuration

This software offered options for emulating baud rate as well as buffer overrun problems. However, we found that these features seem to have a few problems. We would often receive errors when these options were turned on but when we repeated the test using real ports and a null modem cable the errors were not present.

6.2.2 Simulating hardware communication To simulate the hardware we wrote a program that connected to the other virtual COM port that would display data sent from the brick service being tested. It also allowed you to type in your own response or click a button to load a often sent string of commands. Figure 6‐2 is a screen shot of the program in action. This program was a vital tool for developing and testing all services built for both robots.

Figure 6‐2: Hardware simulation software

52 | P a g e

6.2.3 Testing State Changes The state of a brick service is supposed to update when it receives updates from the robot. A snapshot of the state can easily be viewed by using a web browser and navigating to the URL of the service. To verify that state variables update properly the state of the service was viewed after it was initialized and still set to the default as in Figure 6‐3.

Figure 6‐3: Testing state update – Before

A string representing the hardware response was sent back to the service being tested. Then the state information would be refreshed as in Figure 6‐4. It then was a simple matter of verifying that the state changed as expected.

53 | P a g e

Figure 6‐4: Testing state update – After

6.2.4 Testing Notifications Verifying that notifications were being sent properly involved developing simple VPL programs. For example, to verify that the Generic Encoder Service was sending notifications as expected the program shown in Figure 6‐5 was used.

Figure 6‐5: VPL program used to test encoder notifications

54 | P a g e

There are two expected behaviors tested in this program. First when “OK” on the “Reset Encoder?” confirmation is clicked it should send the command [odc] to the robot to clear the encoder. Second, when the service receives a response from the robot containing an encoder update a dialog box with that new value should pop up. Figure 6‐6 shows the outcome after these events occurred. Notice that receiving the [odr 4] command from the robot only triggered the event once as expected. The “Reset Encoder?” dialog is not shown but the result of clicking “Ok” is shown in the “Received Data” window.

Figure 6‐6: Encoder test result

55 | P a g e

7 Project Costs

7.1 Budget This project has no outside funding; therefore, all funding for parts and equipment

will be coming from personal donations. Costs will be incurred at multiple times during the project, which costs include, electronics, PCB manufacturing, PIC programming/debugging equipment, platform hardware, and platform machining. Total costs are estimated to be around $1,500.

7.2 Parts and Costs List

7.2.1 Modular Robot 7.2.1.1 Mechanical Motor Item Name: 28mm Planetary Gearmotor, RS‐385 Motor, 64:1 Item #: M‐200‐30‐MP‐28064‐385 Cost: 30.39 Supplier: www.trossenrobotics.com Wheel Item Name: Colson Wheel 5” x 7/8” Item #: M‐400‐30‐WC‐2807 Cost: 4.45 Supplier: www.trossenrobotics.com Wheel Hub Item Name: Wheel Hub, Colson 7/8" Wheel, 6mm, 1/8" Broach Item #: M‐400‐30‐WH‐WC002‐B Cost: 6.00 Supplier: www.trossenrobotics.com Sheet Metal Supplier: Ipaco Bolts, Nuts and Brackets Supplier: Lowes, Home Depot and Ipaco 7.2.1.2 Electrical Sonar Item Name: LV‐MaxSonar‐EZ1 Item #: S‐10‐EZ1

56 | P a g e

Cost: 29.95 Supplier: www.trossenrobotics.com Photo‐Reflector Item Name: QRD1114 IR Photo reflector Item #: S‐10‐QRD1114 Cost: 2.49 Supplier: www.trossenrobotics.com Motor Speed Controller Item Name: Traxxas XL‐1 Electric Speed Control w/Reverse Item #: LXJM28 Cost: 39.99 Supplier: www.towerhobbies.com Item Name: Atmel Atmega16L Item #: ATMEGA16L‐8PU‐ND Cost: 6.56 Supplier: www.digikey.com Item Name: 74LS125 Quad 3‐State Buffer Item #: 296‐1638‐5‐ND Cost: 0.72 Supplier: www.digikey.com Item Name: 74LS32 Quad 2‐Input OR Gate Item #: 296‐1658‐5‐ND Cost: 0.52 Supplier: www.digikey.com Item Name: Maxim RS‐232 Transceiver Item #: MAX3232ECPE+‐ND Cost: 5.04 Supplier: www.digikey.com Misc Parts: PCB Board, Wire Wrapping Sockets, RS‐232 Connector, Capacitors, etc Supplier: ECE Store

57 | P a g e

Appendix

A Project Management

A.1 Project Management Documents Gantt chart Following are the Gantt charts for this project, see Figure A.1‐1, and the chart for the Systems Engineering team working on the Robo‐Magellan Platform, see Figure A.1‐2. Task Name Start

Robo Magellan Mon 1/21/08Repair Sonar Mon 1/28/08Compas Mon 1/21/08Position Calculation Mon 1/21/08Calibrate Sensors Mon 2/4/08GPS Interface Mon 1/21/08Serial Connection Fri 2/1/08Brick Service Mon 3/17/08Generic Services Fri 4/4/08

Modular Robot Tue 1/1/08Controll Module Tue 1/1/08Motor Modules Tue 1/1/08Sonar Modules Tue 2/12/08Serial Connection Wed 2/20/08Brick Service Fri 4/4/08Generic Services Wed 4/9/08

MSRS Mon 4/7/08Behavorial Programing Mon 4/7/08

RM TeamDave

DaveBilly

RobinRobin

JohnJohn

JohnRobin

RobinRobin

JohnJohn

T

9 16 23 30 6 13 20 27 3 10 17 24 2 9 16 23 30 6 13 20 27'07 Jan '08 Feb '08 Mar '08 Apr '08 M

Figure A.1‐1: MSRS/Modular Robot Gantt Chart

58 | P a g e

Figure A.1‐2: Robo‐Magellan Gantt Chart

B Microsoft Robotics Studio Overview Microsoft has developed the software suite that is Gate’s and Microsoft’s answer to the challenge. Microsoft Robotics Studio is a Windows‐based environment for hobbyist, academic and commercial developers to create robotics applications for a variety of hardware platforms. The Microsoft Robotics Studio includes a lightweight REST‐style, service‐oriented runtime, with a set of visual authoring and simulation tools. It allows easy programming and debugging of a robot using Microsoft’s Visual programming language where users without programming experience can drag and drop components and services and connect them together creating a program. Robotic applications can also be developed using Visual Studio and Visual Studio Express (C# or VB.NET) for those who prefer these programming languages. Robotics Studio also makes Asynchronous programming simple. The Concurrency and Coordination Runtime (CCR) which is part of the foundation for Robotics Studio greatly simplifies handling asynchronous input from multiple sources. Decentralized Software Services (DSS), another key feature of Robotics Studio, makes it easy to connect and work with decentralized processes. For more information please see http://msdn2.microsoft.com/en‐us/library/bb483104.aspx

B.1 Background On Dec 16th 2006, Microsoft founder Bill Gates published an article in Scientific American titled, “A Robot in Every Home.” The article itself has acted as a “call to arms” for much of the robotics community. Gates said:

59 | P a g e

"[T]he challenges facing the robotics industry are similar to those we tackled in computing three decades ago. Robotics companies have no standard operating software that could allow popular application programs to run in a variety of devices. The standardization of robotic processors and other hardware is limited, and very little of the programming code used in one machine can be applied to another. Whenever somebody wants to build a new robot, they usually have to start from square one.

Back in the early days of the personal computer, we realized that we needed an ingredient that would allow all of the pioneering work to achieve critical mass, to coalesce into a real industry capable of producing truly useful products on a commercial scale . . . [I]t seemed clear to me that before the robotics industry could make the same kind of quantum leap that the PC industry made 30 years ago, it, too, needed to find that missing ingredient . . . The goal was to see if it was possible to provide the same kind of common, low-level foundation for integrating hardware and software into robot designs." (Gates, Bill. “A Robot in Every Home.” Scientific American January 2007)

Microsoft’s answer to provide robotics with a foundation is two‐part. First a unifying software package called Microsoft Robotics Studio has been developed by a MS team lead by Tandy Trower, a 25‐year Microsoft veteran. Second, was Gates’ call for researchers and developers to pursue unification in robotics in general. Tandy Trower’s team at Microsoft has developed the software suite that is Gate’s and Microsoft’s answer to the challenge. Microsoft Robotics Studio is a Windows‐based environment for hobbyist, academic and commercial developers to create robotics applications for a variety of hardware platforms. Gates’ call for a unification of industry and academia has resulted in, among other efforts, a response from the U.S. Federal Government. A Bi‐Partisan effort was announced, called the Congressional Caucus on Robotics, by Pennsylvania Congressman Mike Doyle and Tennessee Congressman Zach Wamp. A June 20, 2007 White House Press Release announced:

"The preliminary, proposed goals of the bipartisan Robotics Caucus will include, but are not limited to:

• Increasing general awareness of robotics industry challenges and issues among Members of Congress and policy analysts in the federal government;

• Educating Members of Congress and congressional staff on current and future research, development, and utilization initiatives involving robotics;

• Serving as a forum where robotics‐related policy issues can be discussed and debated; and

• Ensuring that our nation remains globally competitive as the robotics industry rapidly expands and begins to exert a profound effect on the way our citizens live their lives." (White House Press Release. Washington DC. June 20, 2007)

60 | P a g e

B.2 MSRS Quick Overview Concurrency and Coordination Runtime (CCR) was introduced to provide asynchronous operations as well as handle concurrency. CCR in a way abstracts multi‐threading freeing the programmer from needing to deal with that additional complexity. CCR introduces several primitives that are used throughout MSRS. There are 3 main categories to the CCR primitives

1. Port and PortSet provide FIFO style queues; these are used as the basis of all message exchange.

2. Arbiters are the coordination primitives these attach to ports and execute user code, often delegating to some method when certain conditions are met.

3. The Dispatcher, DispatcherQueue and Task primitives provide scheduling and load balancing logic abstracting multithreading.

Decentralized Software Services (DSS) is a lightweight, loosely coupled application model. DSS is based on the Representational State Transfer (REST) design principles. DSS is basically an extension of SOAP based web services, with several additions. First the DSS runtime is built on top of CCR, messages passed between services are sent to CCR ports where they can be handled in an asynchronous manner. Second there support for subscriptions that allow a service to send and receive event notifications. Every DSS services consist of a common set of components as shown in Figure B.2‐1.

Figure B.2‐1: DSS Service Components

• Service Identifier is a unique value that indicates the location of the service. This

essentially is the last part of the URL pointing to this particular service.

61 | P a g e

• Contract Identifier is a unique identifier that used to identify the service. It if is in the form of a URL. Before releasing a service to the public it is a good idea to change this to a URL relating to your company.

• State is a data structure that represents the hardware of the robot. When the state information is requested it gives a snapshot of the robot at any given time.

• Partners are any other services that this current service depends on to operate. There are mechanisms built into the MSRS runtime that allow these services to be started by another service.

• MainPort is the default port that normally accepts all incoming messages. As messages come into the main port it determines what type of message it is and passes it to the appropriate service handler.

• Notifications are ports used to send notices to other services when an event occurs.

C Code

C.1 Magellan MSRS Interface

C.1.1 MagellanTypes.cs //------------------------------------------------------------------------------ // <auto-generated> // This code was generated by a tool. // Runtime Version:2.0.50727.1433 // // Changes to this file may cause incorrect behavior and will be lost if // the code is regenerated. // </auto-generated> //------------------------------------------------------------------------------ using Microsoft.Ccr.Core; using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using System.ComponentModel; using Microsoft.Dss.Core.DsspHttp; using System; using System.Collections.Generic; using W3C.Soap; using magellan = Microsoft.Robotics.Services.Magellan; using submgr = Microsoft.Dss.Services.SubscriptionManager; //Reference the subscription manager namespace Microsoft.Robotics.Services.Magellan { #region Contract /// <summary> /// Magellan Contract class

62 | P a g e

/// </summary> public sealed class Contract { /// <summary> /// The Dss Service contract /// </summary> public const String Identifier = "http://schemas.tempuri.org/2008/04/magellan.html"; } #endregion //Contract #region Magellan State /// <summary> /// The Magellan State /// </summary> [DataContract()] public class MagellanState { private bool _connected; private int _comPort; private bool _debug; private string _error; private DriveConfig _driveMotor; private DriveConfig _steering; private SonarConfig _sr1; private SonarConfig _sr2; private SonarConfig _ir1; private SonarConfig _ir2; private GPSConfig _gps; private string _name; private double _degree; private string[] _commandLoop; private bool _enableCommandLoop; private int _commandDelay; [DataMember] [Description("The name of the robot")] public string RobotName { get { return _name; } set { _name = value; } } [DataMember] [Description("Indicates what COM port the robot is connected to")] public int ComPort { get { return _comPort; } set { _comPort = value; } } [DataMember] [Description("Indicates how verbose the debuging mode should be")] public bool Debug { get { return _debug; } set { _debug = value; } } [DataMember] [Description("Sets if the sensor update loop should be run")] public bool EnableUpdateLoop { get { return _enableCommandLoop; } set { _enableCommandLoop = value; } }

63 | P a g e

[DataMember] [Description("The delay between sending update loop command in ms")] public int CommandDelay { get { return _commandDelay; } set { _commandDelay = value; } } [DataMember] [Description("Array of commands used to preform sensor update loop")] public String[] UpdateLoop { get { return _commandLoop; } set { _commandLoop = value; } } [DataMember] [Description("Indicates whether the Robot is connected")] public bool Connected { get { return _connected; } set { _connected = value; } } [DataMember] [Description("Degree heading with east being 0, Compass is alligned pointing forward")] public double Compass { get { return _degree; } set { _degree = value; } } [DataMember] [Description("Holds any Error messages from the robot")] public string ErrorMessage { get { return _error;} set { _error = value;} } [DataMember] [Description("Indicates the state of the drive motor")] public DriveConfig DriveMotor { get { return this._driveMotor; } set { this._driveMotor = value; } } [DataMember] [Description("Indicates the state of the steering motor")] public DriveConfig SteeringMotor { get { return this._steering; } set { this._steering = value; } } [DataMember] [Description("SR1 sonar sensor")] public SonarConfig SR1 { get { return this._sr1; } set { this._sr1 = value; } } [DataMember] [Description("SR2 sonar sensor")] public SonarConfig SR2 {

64 | P a g e

get { return this._sr2; } set { this._sr2 = value; } } [DataMember] [Description("Infrared sensor 1")] public SonarConfig IR1 { get { return _ir1; } set { _ir1 = value; } } [DataMember] [Description("Infrared sensor 2")] public SonarConfig IR2 { get { return _ir2; } set { _ir2 = value; } } [DataMember] [Description("GPS Information")] public GPSConfig GPS { get { return _gps; } set { _gps = value; } } } #region GPS State class [DataContract] [Description("Magelland GPS implementation")] public class GPSConfig { private double _lat; private double _lon; private int _sat; [DataMember] [Description("Latitude of the GPS")] public double Latitude { get { return _lat; } set { _lat = value; } } [DataMember] [Description("Longitude of the GPS")] public double Longitude { get { return _lon; } set { _lon = value; } } [DataMember] [Description("Number of Satilites detected")] public int Satelites { get { return _sat; } set { _sat = value; } } public GPSConfig() { Latitude = 0; Longitude = 0; Satelites = 0; } } #endregion //GPS State class

65 | P a g e

#region Sonar Sensor Class [Description("Magellan Sonar Sensor implementation")] [DataContract] public class SonarConfig { private int _sonar; private double _angle; private double _inchPer; private int _acutator; [DataMember] [Description("Sensor reading for sonar")] public int Reading { get { return _sonar; } set { _sonar = value; } } [DataMember] [Description("Angle of the sensor relative to the front of the robot")] public double SensorPosition { get { return _angle; } set { _angle = value; } } [DataMember] [Description("Conversion factor for converting sensor reading to inches")] public double InchPer { get { return _inchPer;} set { _inchPer = value; } } [DataMember] [Description("Contains the position information of the Acutator the sesor is mounted on")] public int SensorAngle { get { return _acutator; } set { _acutator = value; } } public SonarConfig() { } public SonarConfig(double angle, double inchper) { SensorPosition = angle; InchPer = inchper; } } #endregion //Sonar Sensor Class #region Motor Class [Description("Magellan motor configuration")] [DataContract] public class DriveConfig { private int _motorSpeed; private double _radius; private int _od; private double _ticks; [DataMember] [Description("Describes the motor speed")] public int MotorSpeed { get { return _motorSpeed; }

66 | P a g e

set { _motorSpeed = value; } } [DataMember] [Description("Radius of wheel attached to motor in inches, used for calculating distance traveled")] public double WheelRadius { get { return _radius; } set { _radius = value; } } [DataMember] [Description("Tracks the Odometry of the motor")] public int Od { get { return _od; } set { _od = value; } } [DataMember] [Description("Number of Ticks of the odometer per revolution of the tire")] public double Ticks { get { return _ticks; } set { _ticks = value; } } public DriveConfig() { MotorSpeed = 0; WheelRadius = 2.5; Od = 0; Ticks = 24; } public DriveConfig(int motorSpeed, double radius, double ticks) { MotorSpeed = motorSpeed; WheelRadius = radius; Od = 0; Ticks = ticks; } } #endregion //Motor Class #endregion //Magellan State #region Main Operations Port /// <summary> /// Magellan Main Operations Port /// </summary> [ServicePort()] public class MagellanOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get, UpdateRobot, UpdateEncoder, UpdateGPS, UpdateSensor, UpdateCompass, EnableUpdateLoop, SendCommand, HttpGet, Subscribe> { } #endregion //Main operations port

67 | P a g e

#region Operations /// <summary> /// Magellan Get Operation /// </summary> public class Get : Get<GetRequestType, PortSet<MagellanState, Fault>> { /// <summary> /// Magellan Get Operation /// </summary> public Get() { } /// <summary> /// Magellan Get Operation /// </summary> public Get(Microsoft.Dss.ServiceModel.Dssp.GetRequestType body) : base(body) { } /// <summary> /// Magellan Get Operation /// </summary> public Get(Microsoft.Dss.ServiceModel.Dssp.GetRequestType body, Microsoft.Ccr.Core.PortSet<MagellanState,W3C.Soap.Fault> responsePort) : base(body, responsePort) { } // public } public class Subscribe : Subscribe<SubscribeRequestType, PortSet<SubscribeResponseType,Fault>> { } public class UpdateEncoder : Update<UpdateEncoderRequest, PortSet<DefaultUpdateResponseType, Fault>> { public UpdateEncoder() : base(new UpdateEncoderRequest()) { } public UpdateEncoder(int tick) : base(new UpdateEncoderRequest(tick)) { } } [DataContract] public class UpdateEncoderRequest { private int _currentReading; [DataMember] public int CurrentReading { get { return _currentReading; } set { _currentReading = value; } }

68 | P a g e

public UpdateEncoderRequest() { } public UpdateEncoderRequest(int ticks) { CurrentReading = ticks; } } public class SendCommand: Update<SendCommandRequest, PortSet<DefaultUpdateResponseType, Fault>> { public SendCommand(): base(new SendCommandRequest()) { } public SendCommand(string command): base(new SendCommandRequest(command)) { } } [DataContract] public class SendCommandRequest { private string _command; [DataMember] public string Command { get { return _command;} set { _command = value;} } public SendCommandRequest() { } public SendCommandRequest(string command) { Command = command; } } public class UpdateCompass : Update<UpdateCompassRequest, PortSet<DefaultUpdateResponseType, Fault>> { public UpdateCompass() : base(new UpdateCompassRequest()) { } public UpdateCompass(double newcompass) : base(new UpdateCompassRequest(newcompass)) { } } [DataContract] public class UpdateCompassRequest { private double _degree; [DataMember] public double NewCompass { get { return _degree; } set { _degree = value; } } public UpdateCompassRequest()

69 | P a g e

{ } public UpdateCompassRequest(double newcompass) { NewCompass = newcompass; } } public class UpdateSensor : Update<UpdateSensorRequest, PortSet<DefaultUpdateResponseType, Fault>> { public UpdateSensor() : base(new UpdateSensorRequest()) { } public UpdateSensor(string hardwareID, int angle, int currentReading) : base(new UpdateSensorRequest(hardwareID, angle, currentReading)) { } } [DataContract] public class UpdateSensorRequest { private string _hwID; private int _angle; private int _currentReading; [DataMember] public string HardwareID { get { return _hwID; } set { _hwID = value; } } [DataMember] public int Angle { get { return _angle; } set { _angle = value; } } [DataMember] public int CurrentReading { get { return _currentReading; } set { _currentReading = value; } } public UpdateSensorRequest() { } public UpdateSensorRequest(string hardwareID, int angle, int currentReading) { HardwareID = hardwareID; Angle = angle; CurrentReading = currentReading; } } public class UpdateRobot : Update<UpdateRobotRequest, PortSet<DefaultUpdateResponseType, Fault>> { public UpdateRobot() : base(new UpdateRobotRequest()) { }

70 | P a g e

public UpdateRobot(string cmd, string val) : base(new UpdateRobotRequest(cmd, val)) { } } [DataContract] public class UpdateRobotRequest { private string _command; private string _value; [DataMember] public string Command { get { return _command; } set { _command = value; } } [DataMember] public string Value { get { return _value; } set { _value = value; } } public UpdateRobotRequest() { } public UpdateRobotRequest(string cmd, string val) { Command = cmd; Value = val; } } public class UpdateGPS : Update<UpdateGPSRequest, PortSet<DefaultUpdateResponseType, Fault>> { public UpdateGPS() : base(new UpdateGPSRequest()) { } public UpdateGPS(double lat, double lon, int sat) : base(new UpdateGPSRequest(lat, lon, sat)) { } } [DataContract] public class UpdateGPSRequest { private double _latitiude; private double _longitude; private int _satNum; [DataMember] public double Latitude { get { return _latitiude; } set { _latitiude = value; } } [DataMember] public double Longitude { get { return _longitude; } set { _longitude = value; } }

71 | P a g e

[DataMember] public int Satellites { get { return _satNum; } set { _satNum = value; } } public UpdateGPSRequest() { } public UpdateGPSRequest(double lat, double lon, int sat) { Latitude = lat; Longitude = lon; Satellites = sat; } } public class EnableUpdateLoop : Update<EnableUpdateLoopRequest, PortSet<DefaultUpdateResponseType, Fault>> { public EnableUpdateLoop() : base(new EnableUpdateLoopRequest()) { } public EnableUpdateLoop(bool enable) : base(new EnableUpdateLoopRequest(enable)) { } } [DataContract] public class EnableUpdateLoopRequest { private bool _enable; [DataMember] public bool EnableUpdate { get { return _enable; } set { _enable = value; } } public EnableUpdateLoopRequest() { } public EnableUpdateLoopRequest(bool enable) { EnableUpdate = enable; } } #endregion //Operations }

C.1.2 Magellan.cs //------------------------------------------------------------------------------ // <auto-generated> // This code was generated by a tool. // Runtime Version:2.0.50727.1433 // // Changes to this file may cause incorrect behavior and will be lost if

72 | P a g e

// the code is regenerated. // </auto-generated> //------------------------------------------------------------------------------ using Microsoft.Ccr.Core; using Microsoft.Dss.Core; using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using Microsoft.Dss.Core.DsspHttp; using Microsoft.Dss.ServiceModel.DsspServiceBase; using System.IO.Ports; using System; using System.Collections.Generic; using System.ComponentModel; using System.Xml; using W3C.Soap; using magellan = Microsoft.Robotics.Services.Magellan; using submgr = Microsoft.Dss.Services.SubscriptionManager; using System.Timers; namespace Microsoft.Robotics.Services.Magellan { /// <summary> /// Implementation class for Magellan /// </summary> [DisplayName("Magellan")] [Description("The Magellan Service")] [Contract(Contract.Identifier)] public class MagellanService : DsspServiceBase { #region Partners //Creates a partner with the subscription service [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)] submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort(); #endregion //Setup Partners #region Initialization Classes and Ports /// <summary> /// _state Innitialize the state /// </summary> [InitialStatePartner(Optional=true, ServiceUri="magellan/magellan.config.xml")] //Allows you to load a state from a saved file private MagellanState _state = new MagellanState(); /// <summary> /// _main Port /// </summary> [ServicePort("/magellan", AllowMultipleInstances=false)] private MagellanOperations _mainPort = new MagellanOperations(); /// <summary> /// Default Service Constructor /// </summary> public MagellanService(DsspServiceCreationPort creationPort) : base(creationPort) { } private UpdateGPS gps; private System.Timers.Timer UpdateTimer; private int _currentUpdateItme = 0;

73 | P a g e

#endregion //Initialization Classes and Ports #region Start() /// <summary> /// Service Start /// </summary> protected override void Start() { //Check to see if the state already exists. //if not then we will initialize it if (_state == null) { _state = new MagellanState(); _state.ComPort = 0; _state.Connected = false; _state.Debug = false; _state.DriveMotor = new DriveConfig(0, 2.5, 36.3); _state.SteeringMotor = new DriveConfig(0, 1, 0); _state.SR1 = new SonarConfig(330, 15.5); _state.SR2 = new SonarConfig(30, 15.5); _state.IR1 = new SonarConfig(15, 10); _state.IR2 = new SonarConfig(345, 10); _state.GPS = new GPSConfig(); _state.UpdateLoop = new string[] { "odr", "sr1", "sr2", "ir1", "ir2", "odr", "sr1", "sr2", "ir1", "ir2", "odr", "sr1", "sr2", "ir1", "ir2", "odr", "sr1", "sr2", "ir1", "ir2", "gps"}; _state.CommandDelay = 100; } _state.Connected = false; base.SaveState(_state); base.Start(); ConnectToMagellan(); gps = new UpdateGPS(); UpdateTimer = new System.Timers.Timer(_state.CommandDelay); UpdateTimer.Elapsed += new ElapsedEventHandler(Timer_event); EnableTimer(_state.EnableUpdateLoop); } #endregion //Start() #region Other functions private void Timer_event(object sender, ElapsedEventArgs e) { if (_state.Connected) { if (_currentUpdateItme >= _state.UpdateLoop.GetLength(0)) _currentUpdateItme = 0; _mainPort.Post(new SendCommand(_state.UpdateLoop[_currentUpdateItme++])); } } private void EnableTimer(bool enable) { if (_state.Connected) { UpdateTimer.Enabled = enable; } } #endregion //Other functions #region Service Handler Routines /// <summary> /// Get Handler

74 | P a g e

/// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> GetHandler(Get get) { get.ResponsePort.Post(_state); yield break; } [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> HTTPGetHandler(HttpGet httpget) { httpget.ResponsePort.Post(new HttpResponseType(_state)); yield break; } [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> SendCommandHandler(SendCommand sendcommand) { if (_state.Connected) { _robotport.WriteLine("[" + sendcommand.Body.Command.ToLower() + "]"); sendcommand.ResponsePort.Post(DefaultUpdateResponseType.Instance); } else { LogError("Could not send " + sendcommand.Body.Command + " (Comport is closed)"); sendcommand.ResponsePort.Post(new Fault()); } yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateEncoderHandler(UpdateEncoder encoder) { if (_state.DriveMotor.Od != encoder.Body.CurrentReading) { _state.DriveMotor.Od = encoder.Body.CurrentReading; base.SendNotification(_subMgrPort, encoder); } yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateGPSHandler(UpdateGPS newGPS) { if ((_state.GPS.Latitude != newGPS.Body.Latitude) || (_state.GPS.Longitude != newGPS.Body.Longitude) || (_state.GPS.Satelites != newGPS.Body.Satellites)) { _state.GPS.Latitude = newGPS.Body.Latitude; _state.GPS.Longitude = newGPS.Body.Longitude; _state.GPS.Satelites = newGPS.Body.Satellites; base.SendNotification(_subMgrPort, newGPS); } yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateSensorHandler(UpdateSensor sensor) { bool error = false; switch (sensor.Body.HardwareID.ToLower()) { case "ir1": _state.IR1.Reading = sensor.Body.CurrentReading; _state.IR1.SensorAngle = sensor.Body.Angle; break;

75 | P a g e

case "ir2": _state.IR2.Reading = sensor.Body.CurrentReading; _state.IR2.SensorAngle = sensor.Body.Angle; break; case "sr1": _state.SR1.Reading = sensor.Body.CurrentReading; _state.SR1.SensorAngle = sensor.Body.Angle; break; case "sr2": _state.SR2.Reading = sensor.Body.CurrentReading; _state.SR2.SensorAngle = sensor.Body.Angle; break; default: LogError("UpdateSensorError sensor: " + sensor.Body.HardwareID + " not found"); error = true; break; } if (error) sensor.ResponsePort.Post(new Fault()); else { sensor.ResponsePort.Post(DefaultUpdateResponseType.Instance); base.SendNotification(_subMgrPort, sensor); } yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateRobotHandler(UpdateRobot urr) { bool sucess = true; string cmd = urr.Body.Command; string value = urr.Body.Value; try { switch(cmd) { case "sr1": if (_state.SR1.Reading != Convert.ToInt32(value)) _mainPort.Post(new UpdateSensor("sr1",_state.SR1.SensorAngle, Convert.ToInt32(value))); break; case "sr2": if (_state.SR2.Reading != Convert.ToInt32(value)) _mainPort.Post(new UpdateSensor("sr2", _state.SR2.SensorAngle, Convert.ToInt32(value))); break; case "ir1": if (_state.IR1.Reading != Convert.ToInt32(value)) _mainPort.Post(new UpdateSensor("ir1", _state.IR1.SensorAngle, Convert.ToInt32(value))); break; case "ir2": if (_state.IR2.Reading != Convert.ToInt32(value)) _mainPort.Post(new UpdateSensor("ir2", _state.IR2.SensorAngle, Convert.ToInt32(value))); break; case "s3": _state.SR1.SensorAngle = Convert.ToInt32(value); //right Sonar break; case "s4": _state.SR2.SensorAngle = Convert.ToInt32(value); //Left Sonar break; case "s5": _state.IR1.SensorAngle = Convert.ToInt32(value); break; case "s6": _state.IR2.SensorAngle = Convert.ToInt32(value); break; case "s1": _state.SteeringMotor.MotorSpeed = Convert.ToInt32(value); break;

76 | P a g e

case "s2": _state.DriveMotor.MotorSpeed = Convert.ToInt32(value); break; case "odr": _mainPort.Post(new UpdateEncoder(Convert.ToInt32(value))); //_state.DriveMotor.Od = Convert.ToInt32(value); break; case "lat": gps.Body.Latitude = Convert.ToDouble(value); // _state.GPS.Latitude = Convert.ToDouble(value); break; case "lon": gps.Body.Longitude = Convert.ToDouble(value); //_state.GPS.Longitude = Convert.ToDouble(value); break; case "sat": gps.Body.Satellites = Convert.ToInt32(value); //_state.GPS.Satelites = Convert.ToInt32(value); _mainPort.Post(gps); break; case "name": _state.RobotName = value; break; case "cmp": _mainPort.Post(new UpdateCompass(Convert.ToDouble(value))); //_state.Compass = Convert.ToDouble(value); break; default: _state.ErrorMessage = "UpdateRobotHandler Error ["+ cmd + "," +value +"]"; LogError("UpdateRobotHandler Error ["+ cmd + "," +value +"]"); sucess = false; break; } } catch (Exception ex) { LogError("UpdateRobotHandler Unknow error:" + ex.Message); _state.ErrorMessage ="UpdateRobotHandler Unknow error:" + ex.Message; sucess = false; } if (sucess) urr.ResponsePort.Post(DefaultUpdateResponseType.Instance); else urr.ResponsePort.Post(new Fault()); yield break; } [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> SubscribeHandler(Subscribe sub) { SubscribeRequestType request = sub.Body; LogInfo("Subscribe request from: " + request.Subscriber); SubscribeHelper(_subMgrPort, request, sub.ResponsePort); yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateCompassHandler(UpdateCompass comp) { if (_state.Compass != comp.Body.NewCompass) { _state.Compass = comp.Body.NewCompass; base.SendNotification(_subMgrPort, comp); } yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> EnableUpdateLoopHandler(EnableUpdateLoop loop) { _state.EnableUpdateLoop = loop.Body.EnableUpdate;

77 | P a g e

EnableTimer(loop.Body.EnableUpdate); yield break; } #endregion //Service handler routines #region Hardware Interface Code private SerialPort _robotport; private string _comBuffer; private void ConnectToMagellan() { string errorMessage; _state.Connected = Connect(_state.ComPort, out errorMessage); if (!_state.Connected && !string.IsNullOrEmpty(errorMessage)) { LogError(LogGroups.Activation, errorMessage); return; } } public bool Connect(int comport, out string errorMessage) { //Check to see if comport is configured if (comport <= 0) { errorMessage = "The Serial port is not configured please edit magellen.config.xml"; return false; } //Check to see if we are already connected if (_state.Connected) { _robotport.Close(); } //Try opening a connection try { _robotport = new SerialPort("COM" + comport.ToString(), 9600, Parity.None, 8, StopBits.One); _robotport.DataReceived += new SerialDataReceivedEventHandler(dataIn); _robotport.Open(); _robotport.DiscardInBuffer(); //clears any junk data that may be errorMessage = string.Empty; LogInfo(LogGroups.Activation, "Magellan connected on COM" + comport.ToString()); return true; } catch (Exception e) { errorMessage = "Error connecting to Magellan on COM" + comport.ToString() + ": " + e.Message; return false; } } /// <summary> /// Reads the data recived on the com port, breaks it down into /// command and value then calls the UpdateValue command /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void dataIn(object sender, SerialDataReceivedEventArgs e) { string temp; string command;

78 | P a g e

string value; int start, end, space; temp = _comBuffer + _robotport.ReadExisting(); if (_state.Debug) LogInfo("Recived from Serial: " +temp); while (temp.Length > 0) { start = temp.IndexOf('['); end = temp.IndexOf(']'); if (start < end) { command = temp.Substring(start + 1, end - start - 1); space = command.IndexOf(' '); if (space > 0) { value = command.Substring(space + 1); command = command.Remove(space); _mainPort.Post(new UpdateRobot(command.ToLower(),value)); } temp = temp.Substring(end + 1); } else { _comBuffer = temp; temp = ""; } } } #endregion //Hardware Interface Code } }

C.1.3 MagellanMotor.cs //------------------------------------------------------------------------------ // <auto-generated> // This code was generated by a tool. // Runtime Version:2.0.50727.1433 // // Changes to this file may cause incorrect behavior and will be lost if // the code is regenerated. // </auto-generated> //------------------------------------------------------------------------------ using Microsoft.Ccr.Core; using Microsoft.Dss.Core; using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using Microsoft.Dss.ServiceModel.DsspServiceBase; using System; using System.Collections.Generic; using W3C.Soap; using dssphttp = Microsoft.Dss.Core.DsspHttp; using pxmotor = Microsoft.Robotics.Services.Motor.Proxy; using xml = System.Xml; using magellan = Microsoft.Robotics.Services.Magellan; namespace Microsoft.Robotics.Services.Magellan.Motor { /// <summary> /// Magellan Motor Service

79 | P a g e

/// </summary> [Contract(Contract.Identifier)] [AlternateContract("http://schemas.microsoft.com/robotics/2006/05/motor.html")] public class MagellanMotorService : DsspServiceBase { /// <summary> /// Partners with the Magellan service /// </summary> [Partner("Megellan", Contract = Magellan.Proxy.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate , Optional = false)] private magellan.Proxy.MagellanOperations _magellan = new magellan.Proxy.MagellanOperations(); /// <summary> /// _state /// </summary> [InitialStatePartner(Optional = false, ServiceUri = "magellan/magellanMotor.config.xml")] //Allows you to load a state from a saved file private pxmotor.MotorState _state = new Microsoft.Robotics.Services.Motor.Proxy.MotorState(); /// <summary> /// _main Port /// </summary> [ServicePort("/magellanmotor", AllowMultipleInstances=true)] private pxmotor.MotorOperations _mainPort = new Microsoft.Robotics.Services.Motor.Proxy.MotorOperations(); /// <summary> /// Default Service Constructor /// </summary> public MagellanMotorService(DsspServiceCreationPort creationPort) : base(creationPort) { } /// <summary> /// Service Start /// </summary> protected override void Start() { if (_state == null) { LogError("Megellan Motor Service not configured. Please assign initial state or a Manifest"); return; } else { LogInfo("Motor: " + _state.Name + " Starting, Hardware ID:" + _state.HardwareIdentifier.ToString()); base.Start(); } } /// <summary> /// Get Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> GetHandler(pxmotor.Get get) { get.ResponsePort.Post(_state); yield break; }

80 | P a g e

/// <summary> /// HttpGet Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> HttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet get) { get.ResponsePort.Post(new dssphttp.HttpResponseType(_state)); yield break; } /// <summary> /// Replace Handler /// </summary> /// <param name="replace"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> ReplaceHandler(pxmotor.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; } /// <summary> /// SetMotorPower Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> SetMotorPowerHandler(pxmotor.SetMotorPower update) { double revPow = update.Body.TargetPower; if (_state.ReversePolarity) { revPow *= -1.0; } int power = (int)Math.Round(revPow * _state.PowerScalingFactor); //Make sure the power is capped at + or - Power Scaling Factor power = Math.Max(power, (int)(-1 * _state.PowerScalingFactor)); power = Math.Min(power, (int)_state.PowerScalingFactor); magellan.Proxy.SendCommandRequest request = new magellan.Proxy.SendCommandRequest(); // LogInfo("Steering Motor Name: " + _state.Name); // LogInfo("Setting Steering motor to : " + power.ToString()); request.Command = "S" + _state.HardwareIdentifier.ToString() + " " + power.ToString(); _magellan.SendCommand(request); update.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; } } }

C.1.4 MagellanEncoder.cs //------------------------------------------------------------------------------ // <auto-generated>

81 | P a g e

// This code was generated by a tool. // Runtime Version:2.0.50727.1433 // // Changes to this file may cause incorrect behavior and will be lost if // the code is regenerated. // </auto-generated> //------------------------------------------------------------------------------ using Microsoft.Ccr.Core; using Microsoft.Dss.Core; using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using Microsoft.Dss.ServiceModel.DsspServiceBase; using System; using System.Collections.Generic; using W3C.Soap; using dssphttp = Microsoft.Dss.Core.DsspHttp; using pxencoder = Microsoft.Robotics.Services.Encoder.Proxy; using xml = System.Xml; using magellan = Microsoft.Robotics.Services.Magellan; using submgr = Microsoft.Dss.Services.SubscriptionManager; namespace Microsoft.Robotics.Services.Magellan.Encoder { /// <summary> /// Magellan Encoder Service /// </summary> [Contract(Contract.Identifier)] [AlternateContract("http://schemas.microsoft.com/robotics/2006/05/encoder.html")] public class MagellanEncoderService : DsspServiceBase { /// <summary> /// Partners with the Magellan service /// </summary> [Partner("Megellan", Contract = Magellan.Proxy.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate , Optional = false)] private magellan.Proxy.MagellanOperations _magellan = new magellan.Proxy.MagellanOperations(); private magellan.Proxy.MagellanOperations _encoderNofity = new magellan.Proxy.MagellanOperations(); //Creates a partner with the subscription service [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)] submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort(); /// <summary> /// _state /// </summary> private pxencoder.EncoderState _state = new Microsoft.Robotics.Services.Encoder.Proxy.EncoderState(); /// <summary> /// _main Port /// </summary> [ServicePort("/magellanencoder", AllowMultipleInstances=false)] private pxencoder.EncoderOperations _mainPort = new Microsoft.Robotics.Services.Encoder.Proxy.EncoderOperations(); /// <summary> /// Default Service Constructor /// </summary> public MagellanEncoderService(DsspServiceCreationPort creationPort) : base(creationPort) {

82 | P a g e

} /// <summary> /// Service Start /// </summary> protected override void Start() { _state = new Microsoft.Robotics.Services.Encoder.Proxy.EncoderState(); base.Start(); Activate<ITask>( Arbiter.Receive<magellan.Proxy.UpdateEncoder>(true, _encoderNofity, NotifyHandler)); SpawnIterator(OnStartup); } /// <summary> /// This retrieved the current state of the Magellan robot, updates the ticksPerRevolution /// bassed on the driveMotor state. It then subscribes to the UpdateEncoder method /// </summary> /// <returns></returns> private IEnumerator<ITask> OnStartup() { magellan.Proxy.MagellanState state = null; yield return Arbiter.Choice( _magellan.Get(GetRequestType.Instance), delegate(Magellan.Proxy.MagellanState response) { state = response; }, delegate(Fault fault) { LogError(null, "Unable to get drive state", fault); } ); if (state != null) { _state.TicksPerRevolution = (int)Math.Round(state.DriveMotor.Ticks); _state.CurrentReading = state.DriveMotor.Od; } yield return Arbiter.Choice( _magellan.Subscribe(_encoderNofity), delegate(SubscribeResponseType response) { }, delegate(Fault fault) { LogError(null, "Unable to subscribe to UpdateEncoder", fault); } ); } /// <summary> /// Get Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> GetHandler(pxencoder.Get get) { get.ResponsePort.Post(_state); yield break; }

83 | P a g e

/// <summary> /// HttpGet Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> HttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet get) { get.ResponsePort.Post(new dssphttp.HttpResponseType(_state)); yield break; } /// <summary> /// Reset Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> ResetHandler(pxencoder.Reset update) { magellan.Proxy.SendCommandRequest command = new magellan.Proxy.SendCommandRequest(); command.Command = "odc"; _magellan.SendCommand(command); _state.CurrentReading = 0; _state.CurrentAngle = 0; _state.TicksSinceReset = 0; yield break; } private void NotifyHandler(magellan.Proxy.UpdateEncoder update) { //LogInfo("Recived notify" + update.Body.CurrentReading.ToString()); pxencoder.UpdateTickCount newcounter= new pxencoder.UpdateTickCount(); newcounter.Body.Count = update.Body.CurrentReading; newcounter.Body.TimeStamp = System.DateTime.Now; _mainPort.Post(newcounter); } /// <summary> /// UpdateTickCount Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateTickCountHandler(pxencoder.UpdateTickCount update) { _state.CurrentReading = update.Body.Count; _state.TimeStamp = update.Body.TimeStamp; base.SendNotification(_subMgrPort, update); yield break; } /// <summary> /// Replace Handler /// </summary> /// <param name="replace"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> ReplaceHandler(pxencoder.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break;

84 | P a g e

} /// <summary> /// ReliableSubscribe Handler /// </summary> /// <param name="subscribe"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> ReliableSubscribeHandler(pxencoder.ReliableSubscribe subscribe) { yield break; } /// <summary> /// Subscribe Handler /// </summary> /// <param name="subscribe"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> SubscribeHandler(pxencoder.Subscribe subscribe) { SubscribeRequestType request = subscribe.Body; SubscribeHelper(_subMgrPort, request, subscribe.ResponsePort); yield break; } } }

C.1.5 MagellanSonar.cs //------------------------------------------------------------------------------ // <auto-generated> // This code was generated by a tool. // Runtime Version:2.0.50727.1433 // // Changes to this file may cause incorrect behavior and will be lost if // the code is regenerated. // </auto-generated> //------------------------------------------------------------------------------ using Microsoft.Ccr.Core; using Microsoft.Dss.Core; using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using Microsoft.Dss.ServiceModel.DsspServiceBase; using System; using System.Collections.Generic; using W3C.Soap; using dssphttp = Microsoft.Dss.Core.DsspHttp; using pxsonar = Microsoft.Robotics.Services.Sonar.Proxy; using xml = System.Xml; using magellan = Microsoft.Robotics.Services.Magellan; using submgr = Microsoft.Dss.Services.SubscriptionManager; namespace Microsoft.Robotics.Services.Magellan.Sonar { /// <summary> /// Magellan Sonar Service /// </summary> [Contract(Contract.Identifier)] [AlternateContract("http://schemas.microsoft.com/robotics/2006/06/sonar.html")] public class MagellanSonarService : DsspServiceBase { /// <summary> /// Partners with the Magellan service /// </summary>

85 | P a g e

[Partner("Megellan", Contract = Magellan.Proxy.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate , Optional = false)] private magellan.Proxy.MagellanOperations _magellan = new magellan.Proxy.MagellanOperations(); private magellan.Proxy.MagellanOperations _sonarNotify = new magellan.Proxy.MagellanOperations(); //Creates a partner with the subscription service [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)] submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort(); /// <summary> /// _state /// </summary> private pxsonar.SonarState _state = new Microsoft.Robotics.Services.Sonar.Proxy.SonarState(); /// <summary> /// _main Port /// </summary> [ServicePort("/magellansonar", AllowMultipleInstances=false)] private pxsonar.SonarOperations _mainPort = new Microsoft.Robotics.Services.Sonar.Proxy.SonarOperations(); /// <summary> /// Default Service Constructor /// </summary> public MagellanSonarService(DsspServiceCreationPort creationPort) : base(creationPort) { } /// <summary> /// Service Start /// </summary> protected override void Start() { base.Start(); // Add service specific initialization here. } /// <summary> /// Get Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> GetHandler(pxsonar.Get get) { get.ResponsePort.Post(_state); yield break; } /// <summary> /// HttpGet Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> HttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet get) { get.ResponsePort.Post(new dssphttp.HttpResponseType(_state)); yield break; }

86 | P a g e

/// <summary> /// Replace Handler /// </summary> /// <param name="replace"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> ReplaceHandler(pxsonar.Replace replace) { _state = replace.Body; replace.ResponsePort.Post(DefaultReplaceResponseType.Instance); yield break; } /// <summary> /// ReliableSubscribe Handler /// </summary> /// <param name="subscribe"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> ReliableSubscribeHandler(pxsonar.ReliableSubscribe subscribe) { yield break; } /// <summary> /// Subscribe Handler /// </summary> /// <param name="subscribe"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> SubscribeHandler(pxsonar.Subscribe subscribe) { SubscribeRequestType request = subscribe.Body; SubscribeHelper(_subMgrPort, request, subscribe.ResponsePort); yield break; } } }

C.2 RMInterface (Program used for testing services using a null modem cable)

using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Text; using System.Windows.Forms; namespace RMInterface { public partial class Form1 : Form { public Form1() { InitializeComponent(); } private void serialPort1_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e) {

87 | P a g e

recived.Invoke(new EventHandler(delegate { recived.AppendText(serialPort1.ReadExisting()); })); // MessageBox.Show(serialPort1.ReadLine()); } /* private delegate void readSerial(string s) { recived.AppendText(s); } */ private void comPort_KeyUp(object sender, KeyEventArgs e) { if (e.KeyCode.Equals(Keys.Return)) { //recived.AppendText("Return" + "\n"); Sentdata.AppendText(comPort.Text); serialPort1.Write((comPort.Text)); comPort.Clear(); // serialPort1.ReadTimeout = 500; // recived.AppendText(serialPort1.ReadLine()); } } private void Form1_Shown(object sender, EventArgs e) { serialPort1.Open(); if (serialPort1.IsOpen) { recived.AppendText("Serial Open\t"); recived.AppendText(serialPort1.PortName.ToString() + "\n"); } else recived.AppendText("Serial Closed"); } private void button1_Click(object sender, EventArgs e) { recived.AppendText(serialPort1.ReadExisting()); comPort.Text = "[s1 101][s2 102][sr1 16][sr2 17][ir1 18][ir2 19][lat 44.435][lon 23.2346][sat 15][name Robo Magellan][odr 4][cmp 359][s3 34][s4 44][s5 54][s6 65]"; } private void comPort_TextChanged(object sender, EventArgs e) { } private void recived_TextChanged(object sender, EventArgs e) { recived.ScrollToCaret(); } private void recived_DoubleClick(object sender, EventArgs e) { recived.Clear(); } private void Form1_Load(object sender, EventArgs e) { } private void Sentdata_TextChanged(object sender, EventArgs e) {

88 | P a g e

} } }

C.3 Tandy-Bot MSRS Interface

C.3.1 TandybotTypes.cs //------------------------------------------------------------------------------ // <auto-generated> // This code was generated by a tool. // Runtime Version:2.0.50727.1433 // // Changes to this file may cause incorrect behavior and will be lost if // the code is regenerated. // </auto-generated> //------------------------------------------------------------------------------ using Microsoft.Ccr.Core; using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using System; using System.ComponentModel; using System.Collections.Generic; using W3C.Soap; using Microsoft.Dss.Core.DsspHttp; using tandybot = Microsoft.Robotics.Services.TandyBot; using submgr = Microsoft.Dss.Services.SubscriptionManager; //Reference the subscription manager namespace Microsoft.Robotics.Services.TandyBot { #region Contract /// <summary> /// TandyBot Contract class /// </summary> public sealed class Contract { /// <summary> /// The Dss Service contract /// </summary> public const String Identifier = "http://schemas.tempuri.org/2008/04/tandybot.html"; } #endregion //Contract #region TandyBot State /// <summary> /// The TandyBot State /// </summary> [DataContract()] public class TandyBotState { private bool _connected; private int _comPort; private int _baud; private bool _debug; private bool _enableLoop; private int _loopDelay; private string _error; private DriveConfig _leftMotor;

89 | P a g e

private DriveConfig _rightMotor; private SonarConfig _leftSonar; private SonarConfig _centerSonar; private SonarConfig _rightSonar; [DataMember] [Description("Identifies if the robot is connected")] public bool Connected { get { return _connected; } set { _connected = value; } } [DataMember] [Description("Identifies the Comport the robot is connected to")] public int ComPort { get { return _comPort; } set { _comPort = value; } } [DataMember] [Description("Sets the baud rate for the COM Port")] public int BaudRate { get { return _baud; } set { _baud = value; } } [DataMember] [Description("Contains any error message from robot")] public string ErrorMessage { get { return _error; } set { _error = value; } } [DataMember] [Description("Enables the update loop timer")] public bool EnableUpdateLoop { get { return _enableLoop; } set { _enableLoop = value; } } [DataMember] [Description("sets the loop delay for the timer in ms")] public int LoopDelay { get { return _loopDelay; } set { _loopDelay = value; } } [DataMember] [Description("If set to true loging is more verbose, set to true for debuging purposes only")] public bool Debug { get { return _debug; } set { _debug = value; } } [DataMember] [Description("Left Drive Motor")] public DriveConfig LeftMotor { get { return _leftMotor; } set { _leftMotor = value; } } [DataMember]

90 | P a g e

[Description("Right Drive Motor")] public DriveConfig RightMotor { get { return _rightMotor; } set { _rightMotor = value; } } [DataMember] [Description("Left Sonar Reading")] public SonarConfig LeftSonar { get { return _leftSonar; } set { _leftSonar = value; } } [DataMember] [Description("Center Sonar reading")] public SonarConfig CenterSonar { get { return _centerSonar; } set { _centerSonar = value; } } [DataMember] [Description("Right Sonar Reading")] public SonarConfig RightSonar { get { return _rightSonar; } set { _rightSonar = value; } } } #region Sonar Sensor Class [Description("Magellan Sonar Sensor implementation")] [DataContract] public class SonarConfig { private int _hwID; private int _sonar; private double _angle; private double _inchPer; private int _maxRange; [DataMember] [Description("Hardware ID for the Sensor")] public int HardwareID { get { return _hwID; } set { _hwID = value; } } [DataMember] [Description("Sensor reading for sonar")] public int Reading { get { return _sonar; } set { _sonar = value; } } [DataMember] [Description("Angle of the sensor relative to the front of the robot")] public double SensorAngle { get { return _angle; } set { _angle = value; } } [DataMember]

91 | P a g e

[Description("Conversion factor for converting sensor reading to inches")] public double InchPer { get { return _inchPer; } set { _inchPer = value; } } [DataMember] [Description("The upper limit on the sonar sensor")] public int MaximumRange { get { return _maxRange; } set { _maxRange = value; } } public SonarConfig() { } public SonarConfig(int id, double angle, double inchper, int maxRange) { HardwareID = id; SensorAngle = angle; InchPer = inchper; MaximumRange = maxRange; } } #endregion //Sonar Sensor Class #region Motor Class [Description("Magellan motor configuration")] [DataContract] public class DriveConfig { private int _currentPower; private int _newPower; private int _HWID; private double _radius; private int _od; private double _ticks; [DataMember] [Description("Identifies the chanel the Motor is connected to")] public int HardwareID { get { return _HWID; } set { _HWID = value; } } [DataMember] [Description("This is the current motor power.")] public int CurrentPower { get { return _currentPower; } set { _currentPower = value; } } [DataMember] [Description("The power setting the motor will be set to next loop")] public int NewPower { get { return _newPower; } set { _newPower = Math.Min(99, value); _newPower = Math.Max(0, value); } } [DataMember]

92 | P a g e

[Description("Radius of wheel attached to motor in inches, used for calculating distance traveled")] public double WheelRadius { get { return _radius; } set { _radius = value; } } [DataMember] [Description("Tracks the Odometry of the motor")] public int Od { get { return _od; } set { _od = value; } } [DataMember] [Description("Number of Ticks of the odometer per revolution of the tire")] public double Ticks { get { return _ticks; } set { _ticks = value; } } public DriveConfig() { NewPower = 50; WheelRadius = 2.5; CurrentPower = 0; Od = 0; Ticks = 24; } public DriveConfig(int hwid, double radius, double ticks) { HardwareID = hwid; WheelRadius = radius; CurrentPower = 0; NewPower = 50; Od = 0; Ticks = ticks; } } #endregion //Motor Class #endregion //Tandybot State #region Main Operations Port /// <summary> /// TandyBot Main Operations Port /// </summary> [ServicePort()] public class TandyBotOperations : PortSet< DsspDefaultLookup, DsspDefaultDrop, HttpGet, UpdateRobot, UpdateSensor, UpdateCurrentPower, EnableUpdateLoop, SendCommand, SetDrive, Get, Subscribe> { }

93 | P a g e

#endregion //Main Operations port #region Operations /// <summary> /// TandyBot Get Operation /// </summary> public class Get : Get<GetRequestType, PortSet<TandyBotState, Fault>> { /// <summary> /// TandyBot Get Operation /// </summary> public Get() { } /// <summary> /// TandyBot Get Operation /// </summary> public Get(Microsoft.Dss.ServiceModel.Dssp.GetRequestType body) : base(body) { } /// <summary> /// TandyBot Get Operation /// </summary> public Get(Microsoft.Dss.ServiceModel.Dssp.GetRequestType body, Microsoft.Ccr.Core.PortSet<TandyBotState,W3C.Soap.Fault> responsePort) : base(body, responsePort) { } } public class Subscribe : Subscribe<SubscribeRequestType, PortSet<SubscribeResponseType, Fault>> { } public class UpdateRobot : Update<UpdateRobotRequest, PortSet<DefaultUpdateResponseType, Fault>> { public UpdateRobot() : base(new UpdateRobotRequest()) { } public UpdateRobot(string cmd, string val) : base(new UpdateRobotRequest(cmd, val)) { } } [DataContract] public class UpdateRobotRequest { private string _command; private string _value; [DataMember] public string Command { get { return _command; } set { _command = value; } }

94 | P a g e

[DataMember] public string Value { get { return _value; } set { _value = value; } } public UpdateRobotRequest() { } public UpdateRobotRequest(string cmd, string val) { Command = cmd; Value = val; } } public class SendCommand : Update<SendCommandRequest, PortSet<DefaultUpdateResponseType, Fault>> { public SendCommand() : base(new SendCommandRequest()) { } public SendCommand(string command) : base(new SendCommandRequest(command)) { } } [DataContract] public class SendCommandRequest { private string _command; [DataMember] public string Command { get { return _command; } set { _command = value; } } public SendCommandRequest() { } public SendCommandRequest(string command) { Command = command; } } public class UpdateSensor : Update<UpdateSensorRequest, PortSet<DefaultUpdateResponseType, Fault>> { public UpdateSensor() : base(new UpdateSensorRequest()) { } public UpdateSensor(int hardwareID, int currentReading) : base(new UpdateSensorRequest(hardwareID, currentReading)) { } } [DataContract]

95 | P a g e

public class UpdateSensorRequest { private int _hwID; private int _currentReading; [DataMember] public int HardwareID { get { return _hwID; } set { _hwID = value; } } [DataMember] public int CurrentReading { get { return _currentReading; } set { _currentReading = value; } } public UpdateSensorRequest() { } public UpdateSensorRequest(int hardwareID, int currentReading) { HardwareID = hardwareID; CurrentReading = currentReading; } } public class EnableUpdateLoop : Update<EnableUpdateLoopRequest, PortSet<DefaultUpdateResponseType, Fault>> { public EnableUpdateLoop() : base(new EnableUpdateLoopRequest()) { } public EnableUpdateLoop(bool enable) : base(new EnableUpdateLoopRequest(enable)) { } } [DataContract] public class EnableUpdateLoopRequest { private bool _enable; [DataMember] public bool EnableUpdate { get { return _enable; } set { _enable = value; } } public EnableUpdateLoopRequest() { } public EnableUpdateLoopRequest(bool enable) { EnableUpdate = enable; } } public class UpdateCurrentPower : Update<UpdateCurrentPowerRequest, PortSet<DefaultUpdateResponseType, Fault>>

96 | P a g e

{ public UpdateCurrentPower() : base(new UpdateCurrentPowerRequest()) { } public UpdateCurrentPower(int left, int right) : base(new UpdateCurrentPowerRequest(left, right)) { } } [DataContract] public class UpdateCurrentPowerRequest { private int _leftMotor; private int _rightMotor; [DataMember] public int LeftMotor { get { return _leftMotor; } set { _leftMotor = value; } } [DataMember] public int RightMotor { get { return _rightMotor; } set { _rightMotor = value; } } public UpdateCurrentPowerRequest() { } public UpdateCurrentPowerRequest(int left, int right) { LeftMotor = left; RightMotor = right; } } public class SetDrive : Update<SetDriveRequest, PortSet<DefaultUpdateResponseType, Fault>> { public SetDrive() : base(new SetDriveRequest()) { } public SetDrive(double left, double right) : base(new SetDriveRequest(left, right)) { } } [DataContract] public class SetDriveRequest { private double _leftMotor; private double _rightMotor; [DataMember] public double LeftMotor { get { return _leftMotor; } set { _leftMotor = value; } } [DataMember] public double RightMotor

97 | P a g e

{ get { return _rightMotor; } set { _rightMotor = value; } } public SetDriveRequest() { } public SetDriveRequest(double left, double right) { LeftMotor = left; RightMotor = right; } } #endregion //Operations }

C.3.2 Tandybot.cs //------------------------------------------------------------------------------ // <auto-generated> // This code was generated by a tool. // Runtime Version:2.0.50727.1433 // // Changes to this file may cause incorrect behavior and will be lost if // the code is regenerated. // </auto-generated> //------------------------------------------------------------------------------ using Microsoft.Ccr.Core; using Microsoft.Dss.Core; using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using Microsoft.Dss.ServiceModel.DsspServiceBase; using System; using System.Threading; using System.Collections.Generic; using System.ComponentModel; using System.Xml; using W3C.Soap; using tandybot = Microsoft.Robotics.Services.TandyBot; using System.IO.Ports; using submgr = Microsoft.Dss.Services.SubscriptionManager; using Microsoft.Dss.Core.DsspHttp; using System.Timers; namespace Microsoft.Robotics.Services.TandyBot { /// <summary> /// Implementation class for TandyBot /// </summary> [DisplayName("TandyBot")] [Description("Custom Brick Service for TandyBot")] [Contract(Contract.Identifier)] public class TandyBotService : DsspServiceBase { #region Partners //Creates a partner with the subscription service [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)] submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

98 | P a g e

#endregion //Setup Partners #region Initialization Classes and Ports /// <summary> /// _state /// </summary> [InitialStatePartner(Optional=true, ServiceUri="tandy/tandybot.config.xml")] //Allows you to load a state from a saved file private TandyBotState _state = new TandyBotState(); // private Port<UpdateRobot> _updatePort = new Port<UpdateRobot>(); /// <summary> /// _main Port /// </summary> [ServicePort("/tandybot", AllowMultipleInstances=false)] private TandyBotOperations _mainPort = new TandyBotOperations(); /// <summary> /// Default Service Constructor /// </summary> public TandyBotService(DsspServiceCreationPort creationPort) : base(creationPort) { } private System.Timers.Timer UpdateTimer; private int _currentLoopItem = 0; #endregion //Initialization Classes and Ports #region Start() /// <summary> /// Service Start /// </summary> protected override void Start() { if (_state == null) { _state = new TandyBotState(); _state.ComPort = 0; _state.BaudRate = 2400; _state.EnableUpdateLoop = false; _state.LoopDelay = 100; _state.LeftSonar = new SonarConfig(6, -30, 1, 1888); _state.CenterSonar = new SonarConfig(5, 0, 0, 1888); _state.RightSonar = new SonarConfig(4, 0, 30, 1888); _state.LeftMotor = new DriveConfig(7, 2.5, 24); _state.RightMotor = new DriveConfig(8, 2.5, 24); } _state.LeftSonar.Reading = 0; _state.CenterSonar.Reading = 0; _state.RightSonar.Reading = 0; _state.Connected = false; base.SaveState(_state); base.Start(); UpdateTimer = new System.Timers.Timer(_state.LoopDelay); UpdateTimer.Elapsed += new ElapsedEventHandler(Timer_event); ConnectToTandy();

99 | P a g e

} #endregion //Start() #region Other functions private void Timer_event(object sender, ElapsedEventArgs e) { if (_robotport.BytesToWrite == 0) { switch (_currentLoopItem++) { case 0: _mainPort.Post(new SendCommand("<w4><w5><w6>[s]</wa>")); break; case 1: _mainPort.Post(new SendCommand("<r4>")); break; case 3: _mainPort.Post(new SendCommand("<r5>")); break; case 4: _mainPort.Post(new SendCommand("<r6>")); break; case 2: case 5: UpdateMotors(); break; default: _currentLoopItem = 0; break; } } } private void UpdateMotors() { string rpow = _state.RightMotor.NewPower.ToString(); string lpow = _state.LeftMotor.NewPower.ToString() ; if (lpow.Length < 2) lpow = "0" + lpow; if (rpow.Length < 2) rpow = "0" + rpow; if ((_state.RightMotor.NewPower != _state.RightMotor.CurrentPower) && (_state.LeftMotor.NewPower != _state.LeftMotor.CurrentPower)) { string lid = _state.LeftMotor.HardwareID.ToString(); string rid = _state.RightMotor.HardwareID.ToString(); _mainPort.Post(new SendCommand("<w" + rid + " w" + lid +"></w" + lid + ">[p " + rpow + "]</w" + rid + "><w" + lid + ">[p " + lpow + "]<w" + rid + ">[g]</wa>")); _mainPort.Post(new UpdateCurrentPower(_state.LeftMotor.NewPower, _state.RightMotor.NewPower)); } else if (_state.LeftMotor.NewPower != _state.LeftMotor.CurrentPower) { string id = _state.LeftMotor.HardwareID.ToString(); _mainPort.Post(new SendCommand("<w" + id + ">[p " + lpow + "][g]</wa>")); _mainPort.Post(new UpdateCurrentPower(_state.LeftMotor.NewPower, _state.RightMotor.CurrentPower)); } else if (_state.RightMotor.NewPower != _state.RightMotor.CurrentPower) { string id = _state.RightMotor.HardwareID.ToString(); _mainPort.Post(new SendCommand("<w" + id + ">[p " + rpow + "][g]</wa>")); _mainPort.Post(new UpdateCurrentPower(_state.LeftMotor.CurrentPower, _state.RightMotor.NewPower)); } }

100 | P a g e

private void EnableTimer(bool enable) { if (_state.Connected) { UpdateTimer.Enabled = enable; } } #endregion //Other functions #region Service Handler Routines /// <summary> /// Get Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> GetHandler(Get get) { get.ResponsePort.Post(_state); yield break; } [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> SubscribeHandler(Subscribe sub) { SubscribeRequestType request = sub.Body; LogInfo("Subscribe request from: " + request.Subscriber); SubscribeHelper(_subMgrPort, request, sub.ResponsePort); yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateSensorHandler(UpdateSensor sensor) { bool error = false; bool sendmessage = false; switch (sensor.Body.HardwareID) { case 4: if (_state.RightSonar.Reading != sensor.Body.CurrentReading) { _state.RightSonar.Reading = sensor.Body.CurrentReading; sendmessage = true; } break; case 5: if (_state.CenterSonar.Reading != sensor.Body.CurrentReading) { _state.CenterSonar.Reading = sensor.Body.CurrentReading; sendmessage = true; } break; case 6: if (_state.LeftSonar.Reading != sensor.Body.CurrentReading) { _state.LeftSonar.Reading = sensor.Body.CurrentReading; sendmessage = true; } break; default: LogError("UpdateSensorError sensor: " + sensor.Body.HardwareID.ToString() + " not found"); error = true; break; } if (error)

101 | P a g e

sensor.ResponsePort.Post(new Fault()); else { sensor.ResponsePort.Post(DefaultUpdateResponseType.Instance); if (sendmessage) base.SendNotification(_subMgrPort, sensor); } yield break; } [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> HTTPGetHandler(HttpGet httpget) { httpget.ResponsePort.Post(new HttpResponseType(_state)); yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateRobotHandler(UpdateRobot urr) { bool sucess = true; string cmd = urr.Body.Command; string value = urr.Body.Value; try { switch (cmd) { case "s4": _mainPort.Post(new UpdateSensor(4, Convert.ToInt32(value))); break; case "s5": _mainPort.Post(new UpdateSensor(5, Convert.ToInt32(value))); break; case "s6": _mainPort.Post(new UpdateSensor(6, Convert.ToInt32(value))); break; default: _state.ErrorMessage = "UpdateRobotHandler Error [" + cmd + "," + value + "]"; LogError("UpdateRobotHandler Error [" + cmd + "," + value + "]"); sucess = false; break; } } catch (Exception ex) { LogError("UpdateRobotHandler Unknow error:" + ex.Message); _state.ErrorMessage = "UpdateRobotHandler Unknow error:" + ex.Message + " Cmd:" + cmd + " value:" + value; sucess = false; } if (sucess) urr.ResponsePort.Post(DefaultUpdateResponseType.Instance); else urr.ResponsePort.Post(new Fault()); yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> SendCommandHandler(SendCommand sendcommand) { string temp = sendcommand.Body.Command.ToLower(); if (_state.Connected) { for (int i=0; i<temp.Length; i++) { _robotport.Write(temp[i].ToString()); Thread.Sleep(1);

102 | P a g e

} sendcommand.ResponsePort.Post(DefaultUpdateResponseType.Instance); } else { LogError("Could not send " + sendcommand.Body.Command + " (Comport is closed)"); sendcommand.ResponsePort.Post(new Fault()); } yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateCurrentPowerHandler(UpdateCurrentPower update) { _state.LeftMotor.CurrentPower = update.Body.LeftMotor; _state.RightMotor.CurrentPower = update.Body.RightMotor; yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> EnableUpdateLoopHandler(EnableUpdateLoop loop) { _state.EnableUpdateLoop = loop.Body.EnableUpdate; EnableTimer(loop.Body.EnableUpdate); yield break; } [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> SetDriveHandler(SetDrive drive) { int right = (int)Math.Round(drive.Body.RightMotor * 50) + 50; int left = (int)Math.Round(drive.Body.LeftMotor * 50) + 50; right = Math.Max(right, 0); right = Math.Min(right, 99); left = Math.Max(left, 0); left = Math.Min(left, 99); _state.LeftMotor.NewPower = left; _state.RightMotor.NewPower = right; yield break; } #endregion //Service Handler Routines #region Hardware Interface Code private SerialPort _robotport; private string _comBuffer; private void ConnectToTandy() { string errorMessage; _state.Connected = Connect(_state.ComPort, out errorMessage); if (!_state.Connected && !string.IsNullOrEmpty(errorMessage)) { LogError(LogGroups.Activation, errorMessage); return; } Thread.Sleep(3000); LogInfo("Sending Tandy-bot init string"); SendCommand cmd = new SendCommand();

103 | P a g e

string j; for (int i = 1; i <= 8; i++) { j = i.ToString(); cmd.Body.Command = "<r" + j+ ">"; _mainPort.Post(cmd); Thread.Sleep(500); cmd.Body.Command = "<w" + j + ">[i " + j + "]</w" + j + ">"; _mainPort.Post(cmd); Thread.Sleep(50); } // Thread.Sleep(5000); // EnableTimer(_state.EnableUpdateLoop); } public bool Connect(int comport, out string errorMessage) { //Check to see if comport is configured if (comport <= 0) { errorMessage = "The Serial port is not configured please edit tandybot.config.xml"; return false; } //Check to see if we are already connected if (_state.Connected) { _robotport.Close(); } //Try opening a connection try { _robotport = new SerialPort("COM" + comport.ToString(), _state.BaudRate, Parity.None, 8, StopBits.One); _robotport.ReceivedBytesThreshold = 8; _robotport.DataReceived += new SerialDataReceivedEventHandler(dataIn); _robotport.Open(); _robotport.DiscardInBuffer(); _robotport.DiscardOutBuffer(); errorMessage = string.Empty; LogInfo(LogGroups.Activation, "TandyBot connected on COM" + comport.ToString() + " Baud:" + _state.BaudRate.ToString()); return true; } catch (Exception e) { errorMessage = "Error connecting to TandyBot on COM" + comport.ToString() + ": " + e.Message; return false; } } /// <summary> /// Reads the data recived on the com port, breaks it down into /// command and value then calls the UpdateValue command /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void dataIn(object sender, SerialDataReceivedEventArgs e) { string temp; string command; string value; int start, middle, end, space;

104 | P a g e

temp = _comBuffer + _robotport.ReadExisting(); _comBuffer = ""; if (_state.Debug) LogInfo("Recived from Serial: " + temp); while (temp.Length > 0) { end = temp.IndexOf(']'); start = findPrevious(temp, '[',end); if (start < end) { command = temp.Substring(start + 1, end - start - 1); space = command.IndexOf(' '); if (space > 0) { value = command.Substring(space + 1); command = command.Remove(space); _mainPort.Post(new UpdateRobot(command.ToLower(), value)); temp = temp.Substring(end + 1); } else { temp = temp.Substring(end + 1, temp.Length - 1 - end); _comBuffer = ""; } } else { _comBuffer = temp.Substring(start); temp = ""; } } } private int findPrevious(string s, char c, int end) { int stop = Math.Min(end, s.Length - 1); for (int i = stop; i >= 1; --i) { if (s[i].Equals(c)) return i; } return 0; } #endregion //Hardware Interface Code } }

C.3.3 TandyDifferentialDrive.cs //------------------------------------------------------------------------------ // <auto-generated> // This code was generated by a tool. // Runtime Version:2.0.50727.1433 // // Changes to this file may cause incorrect behavior and will be lost if // the code is regenerated. // </auto-generated> //------------------------------------------------------------------------------ using Microsoft.Ccr.Core; using Microsoft.Dss.Core;

105 | P a g e

using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using Microsoft.Dss.ServiceModel.DsspServiceBase; using Microsoft.Dss.Services.SubscriptionManager; using System; using System.Collections.Generic; using System.ComponentModel; using W3C.Soap; using drive = Microsoft.Robotics.Services.Drive.Proxy; using dssphttp = Microsoft.Dss.Core.DsspHttp; using xml = System.Xml; using tandybot = Microsoft.Robotics.Services.TandyBot; namespace Microsoft.Robotics.Services.TandyBot.DiffDrive { /// <summary> /// Provides access to a differential drive (that coordinates two motors that function together). /// </summary> [DisplayName("Generic Differential Drive service for Tandy-Bot")] [Description("Provides access to a differential drive (that coordinates two motors that functio" + "n together).")] [Contract(Contract.Identifier)] [AlternateContract("http://schemas.microsoft.com/robotics/2006/05/drive.html")] public class TandyDifferentialDriveService : DsspServiceBase { /// <summary> /// Adds Tandybot brick service as partner /// </summary> [Partner("TandyBot", Contract = TandyBot.Proxy.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)] private tandybot.Proxy.TandyBotOperations _tandyBot = new Microsoft.Robotics.Services.TandyBot.Proxy.TandyBotOperations(); /// <summary> /// Microsoft.Robotics.Services.Drive.TandyDifferentialDrive Partner /// </summary> [Partner("SubMgr", Contract="http://schemas.microsoft.com/xw/2005/01/subscriptionmanager.html", CreationPolicy=PartnerCreationPolicy.CreateAlways, Optional=false)] private SubscriptionManagerPort _subMgrPort = new Microsoft.Dss.Services.SubscriptionManager.SubscriptionManagerPort(); /// <summary> /// _main Port /// </summary> [ServicePort("/tandydifferentialdrive", AllowMultipleInstances=false)] private drive.DriveOperations _mainPort = new Microsoft.Robotics.Services.Drive.Proxy.DriveOperations(); /// <summary> /// Microsoft.Robotics.Services.TandyBot.DiffDrive.TandyDifferentialDriveService State /// </summary> [InitialStatePartner(Optional=true, ServiceUri="Microsoft.Robotics.Services.TandyBot.DiffDrive.TandyDifferentialDriveService.Conf" + "ig.xml")] private drive.DriveDifferentialTwoWheelState _state = new Microsoft.Robotics.Services.Drive.Proxy.DriveDifferentialTwoWheelState(); /// <summary> /// Default Service Constructor /// </summary> public TandyDifferentialDriveService(DsspServiceCreationPort creationPort) : base(creationPort)

106 | P a g e

{ } tandybot.Proxy.SetDrive request = new Microsoft.Robotics.Services.TandyBot.Proxy.SetDrive(); /// <summary> /// Service Start /// </summary> protected override void Start() { _state = new Microsoft.Robotics.Services.Drive.Proxy.DriveDifferentialTwoWheelState(); base.Start(); _state.IsEnabled = true; } /// <summary> /// Get Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> GetHandler(drive.Get get) { get.ResponsePort.Post(_state); yield break; } /// <summary> /// HttpGet Handler /// </summary> /// <param name="get"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> HttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet get) { get.ResponsePort.Post(new dssphttp.HttpResponseType(_state)); yield break; } /// <summary> /// HttpPost Handler /// </summary> /// <param name="submit"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)] public virtual IEnumerator<ITask> HttpPostHandler(dssphttp.HttpPost submit) { // TODO: Implement Submit operations here. throw new NotImplementedException("TODO: Implement Submit operations here."); } /// <summary> /// ReliableSubscribe Handler /// </summary> /// <param name="subscribe"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> ReliableSubscribeHandler(drive.ReliableSubscribe subscribe) { base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort); yield break; }

107 | P a g e

/// <summary> /// Subscribe Handler /// </summary> /// <param name="subscribe"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> SubscribeHandler(drive.Subscribe subscribe) { base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort); yield break; } /// <summary> /// Update Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> UpdateHandler(drive.Update update) { _state = update.Body; yield break; } /// <summary> /// EnableDrive Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> EnableDriveHandler(drive.EnableDrive update) { _state.IsEnabled = update.Body.Enable; yield break; } /// <summary> /// SetDrivePower Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower update) { if (_state.IsEnabled) { request.Body.LeftMotor = update.Body.LeftWheelPower; request.Body.RightMotor = update.Body.RightWheelPower; _tandyBot.Post(request); } yield break; } /// <summary> /// SetDriveSpeed Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> SetDriveSpeedHandler(drive.SetDriveSpeed update) { // TODO: Implement Update operations here. throw new NotImplementedException("TODO: Implement Update operations here."); } /// <summary>

108 | P a g e

/// RotateDegrees Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees update) { // TODO: Implement Update operations here. throw new NotImplementedException("TODO: Implement Update operations here."); } /// <summary> /// DriveDistance Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance update) { // TODO: Implement Update operations here. throw new NotImplementedException("TODO: Implement Update operations here."); } /// <summary> /// AllStop Handler /// </summary> /// <param name="update"></param> /// <returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public virtual IEnumerator<ITask> AllStopHandler(drive.AllStop update) { if (_state.IsEnabled) { request.Body.LeftMotor = 0; request.Body.RightMotor = 0; _tandyBot.Post(request); } yield break; } } }

109 | P a g e

C.4 Magellan Firmware //========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: main.c $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Communication and Command Line Interpreter //============================================================================ #define F_CPU 14745600UL #define F_OSC 14745600UL #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "uart.h" #include "servo.h" #include "gp2d02.h" #include "srf04.h" #include "odometry.h" //Global UART Variables extern volatile uint8_t urx0; extern volatile uint8_t urx_recv0; extern volatile uint8_t urx1; extern volatile uint8_t urx_recv1; //Command Interpreter Pre-Defined Strings const char s_annc[] PROGMEM = "\n\n\nROBOMAGELLAN INTERFACE\n\n"; const char s_prompt[] PROGMEM = "\n"; const char s_invalid[] PROGMEM = "[]\n"; const char s_bksp[] PROGMEM = "\b \b"; const char s_usage[] PROGMEM = "\n" "Valid Commands:\n\n" " st\n" " - prints current servo raw and relative-from-center\n" " positions\n" "\n" " sa <s1> <s2> <s3> <s4> <s5> <s6>\n" " - set all six servos to the specified positions.\n" " Typical: Zero = center, max = 1000, min = -1000\n" "\n" " s# <s#> (# = Servo Number : 1-6)\n" " - set a single servo to a specified position.\n" " Typical: Zero = center, max = 1000, min = -1000\n" "\n" " ir# (# = Ir Range Finder : 1-2)\n" " - take a reading from a signal IR Range Finder\n" "\n" " sr# (# = Sonar Range Finder : 1-2)\n" " - take a reading from a signal Sonar Range Finder\n" "\n" " odr\n" " - Get the value of the current odometry count\n"

110 | P a g e

"\n" " odc\n" " - Clear the value of the current odometry count\n" "\n" " ? - this help message\n" "\n"; // parse an integer from a string uint16_t get_num(char * s, char ** next, int8_t * err) { char * e; uint16_t n; n = strtoul(s, &e, 0); if (e == s) { printf("error: get_num(): can't scan arg: \"%s\" \n", s); *next = s; if (err) *err = 1; return 0xffff; } while (*e == ' ') e++; *next = e; if (err) *err = 0; return n; } // Decode and execute simple commands static char do_cmdbuf[20]; static __inline__ void do_cmd(char * s) { static char * cmd = do_cmdbuf; uint8_t index; char * args, * p; int8_t rc; if (s[0] == 0) return; // parse the command line, seperating the command from arguments cmd[0] = 0; index = 0; while ((index < sizeof(do_cmdbuf)) && s[index] && (s[index] != ' ')) { cmd[index] = s[index]; index++; } if (index < sizeof(do_cmdbuf)) { cmd[index] = 0; args = &s[index]; while (*args && (*args == ' ')) args++; if (*args == 0) args = NULL; } else { cmd[sizeof(do_cmdbuf)-1] = 0; args = NULL; } if (cmd[0] == 0) { return;

111 | P a g e

} if (strcmp(cmd, "?") == 0) { printf_P(s_usage); } else if (strcmp(cmd, "st") == 0) { printf("[s1 %u (%d)]", SERVO1, SERVO1-SERVO1_MID); printf("[s2 %u (%d)]", SERVO2, SERVO2-SERVO2_MID); printf("[s3 %u (%d)]", SERVO3, SERVO3-SERVO3_MID); printf("[s4 %u (%d)]", SERVO4, SERVO4-SERVO4_MID); printf("[s5 %u (%d)]", SERVO5, SERVO5-SERVO5_MID); printf("[s6 %u (%d)]", SERVO6, SERVO6-SERVO6_MID); } else if (strcmp(cmd, "sa") == 0) { if (args) { int16_t s1, s2, s3, s4, s5, s6, err=0; p = args; s1 = get_num(p, &p, &rc); err += rc; s2 = get_num(p, &p, &rc); err += rc; s3 = get_num(p, &p, &rc); err += rc; s4 = get_num(p, &p, &rc); err += rc; s5 = get_num(p, &p, &rc); err += rc; s6 = get_num(p, &p, &rc); err += rc; if (err == 0) { servo(s1, s2, s3, s4, s5, s6); printf("[sa %u %u %u %u %u %u]\n", s1, s2, s3, s4, s5, s6); } } } else if (strcmp(cmd, "s1") == 0) { if (args) { int16_t s1, err=0; p = args; s1 = get_num(p, &p, &rc); //printf("[s1 %u]\n", s1); err += rc; if (err == 0) servo1(s1); } } else if (strcmp(cmd, "s2") == 0) { if (args) { int16_t s2, err=0; p = args; s2 = get_num(p, &p, &rc); //printf("[s2 %u]\n", s2); err += rc; if (err == 0) servo2(s2); } } else if (strcmp(cmd, "s3") == 0) { if (args) { int16_t s3, err=0;

112 | P a g e

p = args; s3 = get_num(p, &p, &rc); //printf("[s3 %u]\n", s3); err += rc; if (err == 0) servo3(s3); } } else if (strcmp(cmd, "s4") == 0) { if (args) { int16_t s4, err=0; p = args; s4 = get_num(p, &p, &rc); //printf("[s4 %u]\n", s4); err += rc; if (err == 0) servo4(s4); } } else if (strcmp(cmd, "s5") == 0) { if (args) { int16_t s5, err=0; p = args; s5 = get_num(p, &p, &rc); //printf("[s5 %u]\n", s5); err += rc; if (err == 0) servo5(s5); } } else if (strcmp(cmd, "s6") == 0) { if (args) { int16_t s6, err=0; p = args; s6 = get_num(p, &p, &rc); //printf("[s6 %u]\n", s6); err += rc; if (err == 0) servo6(s6); } } else if (strcmp(cmd, "on") == 0) { printf("[on]\n"); DDRB = 0b11100001; PORTB = 0b00000001; } else if (strcmp(cmd, "off") == 0) { printf("[off]\n"); DDRB = 0b11100001; PORTB = 0b00000000; } else if (strcmp(cmd, "gps") == 0) { DDRB = 0b11100001; PORTB = 0b00000001; _delay_ms(1200); PORTB = 0b00000000; _delay_ms(50); printf("[gps]"); } else if (strcmp(cmd, "go") == 0) { printf("[go]\n");

113 | P a g e

int tempread = 0; tempread = srf_range(0x10, 0x20); if(tempread > 200) servo2(300); while(tempread > 200) { tempread = srf_range(0x10, 0x20); _delay_ms(100); // } servo2(-1000); _delay_ms(800); // servo2(0); printf("[stop]\n"); } else if (strcmp(cmd, "ir1") == 0) { int tempread = 0; tempread = ir_range(0x01, 0x02); printf("[ir1 %u]\n", tempread); } else if (strcmp(cmd, "ir2") == 0) { int tempread = 0; tempread = ir_range(0x04, 0x08); printf("[ir2 %u]\n", tempread); } else if (strcmp(cmd, "sr1") == 0) { int tempread = 0; tempread = srf_range(0x10, 0x20); printf("[sr1 %u]\n", tempread); } else if (strcmp(cmd, "sr2") == 0) { int tempread = 0; tempread = srf_range(0x40, 0x80); printf("[sr2 %u]\n", tempread); } else if (strcmp(cmd, "odr") == 0) { int tempread = 0; tempread = odometry_read(); printf("[odr %u]\n", tempread); } else if (strcmp(cmd, "odc") == 0) { odometry_clear(); printf("[odc]\n"); } else { printf_P(s_invalid); //printf_P(s_usage); } return; } // accept characters and build a command line, when return is pressed, // pass the command to 'do_cmd()' #define CMD_BUF_LEN 128 static char recv_input_cmdbuf[CMD_BUF_LEN]; void recv_input(uint8_t ch) { static uint8_t idx=0; if ((ch == '\r')||(ch == '\n')) { // uart_putc0('\n'); recv_input_cmdbuf[idx] = 0;

114 | P a g e

do_cmd(recv_input_cmdbuf); //printf_P(s_prompt); idx = 0; } else if ((ch == '\b') && idx) { //printf_P(s_bksp); if (idx > 0) idx--; recv_input_cmdbuf[idx] = 0; } else if (ch == '[') { //uart_putc0('['); recv_input_cmdbuf[idx] = 0; } else if (ch == ']') { //uart_putc0(']'); recv_input_cmdbuf[idx] = 0; do_cmd(recv_input_cmdbuf); //printf_P(s_prompt); idx = 0; } else { //uart_putc0(ch); recv_input_cmdbuf[idx++] = ch; if (idx == CMD_BUF_LEN) { idx = 0; recv_input_cmdbuf[idx] = 0; //printf_P(s_invalid); //printf_P(s_prompt); } } } void Read_Port(void) { uint8_t ch; cli(); urx_recv0 = 0; ch = urx0; sei(); /* build a command line and execute commands when complete */ recv_input(ch); } int main(void) { uint8_t count; int value; /* initialize uart */ init_uart(); /* initialize servos */ init_servos(); /* enable interrupts */ sei(); /* output startup announcement message */ printf_P(s_annc); /* output a prompt */ printf_P(s_prompt);

115 | P a g e

/* loop - wait for commands from the serial interface */ count = 0; while (1) { /*count++; if (urx_recv0) Read_Port(); //servo positions printf("[s1 %u]\n", SERVO1); //_delay_ms(50); if (urx_recv0) Read_Port(); printf("[s2 %u]\n", SERVO2); //_delay_ms(50); if (urx_recv0) Read_Port(); printf("[s3 %u]\n", SERVO3); //_delay_ms(50); if (urx_recv0) Read_Port(); printf("[s4 %u]\n", SERVO4); //_delay_ms(50); if (urx_recv0) Read_Port(); printf("[s5 %u]\n", SERVO5); //_delay_ms(50); if (urx_recv0) Read_Port(); printf("[s6 %u]\n", SERVO6); //_delay_ms(50); if (urx_recv0) Read_Port(); //ir ranger readings value = ir_range(0x01, 0x02); printf("[ir1 %u]\n", value); //_delay_ms(50); if (urx_recv0) Read_Port(); value = ir_range(0x04, 0x08); printf("[ir2 %u]\n", value); //_delay_ms(50); if (urx_recv0) Read_Port(); //sonar readings value = srf_range(0x10, 0x20); printf("[sr1 %u]\n", value); //_delay_ms(50); if (urx_recv0) Read_Port(); value = srf_range(0x40, 0x80); printf("[sr2 %u]\n", value); //_delay_ms(50); if (urx_recv0) Read_Port(); //compass reading printf("[cmp 90]\n"); //, value); //_delay_ms(50); if (urx_recv0) Read_Port(); //odometer reading value = odometry_read(); //_delay_ms(50); printf("[odr %u]\n", value); if (urx_recv0) Read_Port(); if (count > 24) { DDRB = 0b11100001; PORTB = 0b00000001; for (count = 0; count<120; count++) { _delay_ms(10); if (urx_recv0) Read_Port(); } PORTB = 0b00000000; for (count = 0; count<10; count++) { _delay_ms(10); if (urx_recv0) Read_Port(); } printf("\n"); //_delay_ms(50); count = 0;

116 | P a g e

}*/ if (urx_recv0) Read_Port(); } }

117 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: globalvar.h $ // $Revision: 1 $ // $Modtime: 03 Apr 2008 $ // // Gloabal Variable Definitions //============================================================================ #ifndef __GLOBALVARS_H__ #define __GLOBALVARS_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** // servo.c /*extern int servo1_position; extern int servo2_position; extern int servo3_position; extern int servo4_position; extern int servo5_position; extern int servo6_position;*/ #endif

118 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: globalvar.c $ // $Revision: 1 $ // $Modtime: 03 Apr 2008 $ // // Gloabal Variable Definitions //============================================================================ #include "globalvars.h" /*int servo1_position=0; int servo2_position=0; int servo3_position=0; int servo4_position=0; int servo5_position=0; int servo6_position=0;*/

119 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: gp2d02.h $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Sharp IR GP2D02 Range Finder Device Driver //============================================================================ #ifndef __GP2D02_H__ #define __GP2D02_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** // Returns the distance of Sharp IR Ranger // is passed the mask of the port bit corrisponding // with the Vin and Vout of the sensor int ir_range(int, int); #endif

120 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: gp2d02.c $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Sharp IR GP2D02 Range Finder Device Driver //============================================================================ #define F_CPU 14745600UL #define PortUsed PORTC #define DDRUsed DDRC #define PinUsed PINC #include <avr/io.h> #include <util/delay.h> int ir_range(int pinOut, int pinIn) { int distance = 0; DDRUsed = pinOut; PortUsed = pinOut; // High on Vin for at 5 ms _delay_ms(5); PortUsed = 0x00; // Low on Vin until Input Pin Raises _delay_ms(10); while((PinUsed & pinIn) == 0x00) //Infinite Wait Loop { } PortUsed = pinOut; // High on Vin for 10us _delay_us(10); PortUsed = 0x00; // Low on Vin for 10us _delay_us(10); PortUsed = pinOut; // High on Vin for 10us _delay_us(10); if(PinUsed & pinIn) // If Vout on Sensor is High then Add Bit { distance = distance + 128; } PortUsed = 0x00; // Low on Vin for 10us _delay_us(10); PortUsed = pinOut; // High on Vin for 10us _delay_us(10); if(PinUsed & pinIn) // If Vout on Sensor is High then Add Bit { distance = distance + 64; } PortUsed = 0x00; // Low on Vin for 10us _delay_us(10); PortUsed = pinOut; // High on Vin for 10us _delay_us(10); if(PinUsed & pinIn) // If Vout on Sensor is High then Add Bit { distance = distance + 32; } PortUsed = 0x00; // Low on Vin for 10us _delay_us(10); PortUsed = pinOut; // High on Vin for 10us _delay_us(10); if(PinUsed & pinIn) // If Vout on Sensor is High then Add Bit {

121 | P a g e

distance = distance + 16; } PortUsed = 0x00; // Low on Vin for 10us _delay_us(10); PortUsed = pinOut; // High on Vin for 10us _delay_us(10); if(PinUsed & pinIn) // If Vout on Sensor is High then Add Bit { distance = distance + 8; } PortUsed = 0x00; // Low on Vin for 10us _delay_us(10); PortUsed = pinOut; // High on Vin for 10us _delay_us(10); if(PinUsed & pinIn) // If Vout on Sensor is High then Add Bit { distance = distance + 4; } PortUsed = 0x00; // Low on Vin for 10us _delay_us(10); PortUsed = pinOut; // High on Vin for 10us _delay_us(10); if(PinUsed & pinIn) // If Vout on Sensor is High then Add Bit { distance = distance + 2; } PortUsed = 0x00; // Low on Vin for 10us _delay_us(10); PortUsed = pinOut; // High on Vin for 10us _delay_us(10); if(PinUsed & pinIn) // If Vout on Sensor is High then Add Bit { distance = distance + 1; } PortUsed = pinOut; // High on Vin for 2ms _delay_ms(2); PortUsed = 0x00; // Low on Vin return distance; }

122 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: odometry.h $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Wheel Odometry Counter Device Driver //============================================================================ #ifndef __ODOMETRY_H__ #define __ODOMETRY_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** // Returns the count from the odometer int odometry_read(void); // Clear the count on the odometer to zero void odometry_clear(void); #endif

123 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: odometry.c $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Wheel Odometry Counter Device Driver //============================================================================ #define F_CPU 14745600UL #define PortUsed PORTA #define DDRUsed DDRA #define PinUsed PINA #include <avr/io.h> #include <util/delay.h> void odometry_clear(void) { DDRUsed = 0X01; PortUsed = 0X01; _delay_ms(10); PortUsed = 0X00; //Clear Odometry; } int odometry_read(void) { int distance = 0; DDRUsed = 0x01; distance = PinUsed & 0xFE; return distance; }

124 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: servo.h $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Sets the 6 Servos to be Timer Driven // Function calls to Modify Servo Positions Commands //============================================================================ #ifndef __SERVO_H__ #define __SERVO_H__ //******************************************************* // DEFINES: //******************************************************* #define SERVO1 OCR3A #define SERVO1_INIT 2600 #define SERVO1_MIN 1200 #define SERVO1_MID 2600 #define SERVO1_MAX 4000 #define SERVO1_CLIP(x) (x<SERVO1_MIN ? SERVO1_MIN : (x>SERVO1_MAX ? SERVO1_MAX : x)) #define OFFSET1(x) (SERVO1_CLIP(SERVO1_MID - x)) #define SERVO2 OCR3B #define SERVO2_INIT 2800 #define SERVO2_MIN 1800 #define SERVO2_MID 2800 #define SERVO2_MAX 3800 #define SERVO2_CLIP(x) (x<SERVO2_MIN ? SERVO2_MIN : (x>SERVO2_MAX ? SERVO2_MAX : x)) #define OFFSET2(x) (SERVO2_CLIP(SERVO2_MID - x)) #define SERVO3 OCR3C #define SERVO3_INIT 2900 #define SERVO3_MIN 1900 #define SERVO3_MID 2900 #define SERVO3_MAX 3900 #define SERVO3_CLIP(x) (x<SERVO3_MIN ? SERVO3_MIN : (x>SERVO3_MAX ? SERVO3_MAX : x)) #define OFFSET3(x) (SERVO3_CLIP(SERVO3_MID - x)) #define SERVO4 OCR1A #define SERVO4_INIT 2900 #define SERVO4_MIN 1900 #define SERVO4_MID 2900 #define SERVO4_MAX 3900 #define SERVO4_CLIP(x) (x<SERVO4_MIN ? SERVO4_MIN : (x>SERVO4_MAX ? SERVO4_MAX : x)) #define OFFSET4(x) (SERVO4_CLIP(SERVO4_MID + x)) #define SERVO5 OCR1B #define SERVO5_INIT 3500 #define SERVO5_MIN 2000 #define SERVO5_MID 3000 #define SERVO5_MAX 4000 #define SERVO5_CLIP(x) (x<SERVO5_MIN ? SERVO5_MIN : (x>SERVO5_MAX ? SERVO5_MAX : x)) #define OFFSET5(x) (SERVO5_CLIP(SERVO5_MID + x)) #define SERVO6 OCR1C #define SERVO6_INIT 2400 #define SERVO6_MIN 1900 #define SERVO6_MID 2900 #define SERVO6_MAX 3900 #define SERVO6_CLIP(x) (x<SERVO6_MIN ? SERVO6_MIN : (x>SERVO6_MAX ? SERVO6_MAX : x)) #define OFFSET6(x) (SERVO6_CLIP(SERVO6_MID - x))

125 | P a g e

//******************************************************* // FUNCTIONS //******************************************************* void init_servos(void); void servo(int16_t, int16_t, int16_t, int16_t, int16_t, int16_t); void servo1(int16_t); void servo2(int16_t); void servo3(int16_t); void servo4(int16_t); void servo5(int16_t); void servo6(int16_t); #endif

126 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: servo.c $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Sets the 6 Servos to be Timer Driven // Function calls to Modify Servo Positions Commands //============================================================================ #include <avr/interrupt.h> #include "servo.h" #include "globalvars.h" void servo(int16_t s1, int16_t s2, int16_t s3, int16_t s4, int16_t s5, int16_t s6) { SERVO1 = OFFSET1(s1); SERVO2 = OFFSET2(s2); SERVO3 = OFFSET3(s3); SERVO4 = OFFSET4(s4); SERVO5 = OFFSET5(s5); SERVO6 = OFFSET6(s6); } void servo1(int16_t s1) { SERVO1 = OFFSET1(s1); } void servo2(int16_t s2) { SERVO2 = OFFSET2(s2); } void servo3(int16_t s3) { SERVO3 = OFFSET3(s3); } void servo4(int16_t s4) { SERVO4 = OFFSET4(s4); } void servo5(int16_t s5) { SERVO5 = OFFSET5(s5); } void servo6(int16_t s6) { SERVO6 = OFFSET6(s6); } // SIG_OVERFLOW3 - this interrupt handler starts the pulse for servos // 1, 2, & 3; the timer 3 output compare function automatically ends // the pulse precisely as specified by the OCR3x register which // represents the servo position SIGNAL(SIG_OVERFLOW3) { TCNT3 = 0; // configure to set outputs on compare match so we can turn on the

127 | P a g e

// pulse in the next statement TCCR3A |= _BV(COM3A1)|_BV(COM3A0)|_BV(COM3B1)|_BV(COM3B0)|_BV(COM3C1)|_BV(COM3C0); // force compare match to set outputs TCCR3C |= _BV(FOC3A)|_BV(FOC3B)|_BV(FOC3C); // configure to clear outputs on compare match so that the output // compare function ends the pulse TCCR3A &= ~(_BV(COM3A0)|_BV(COM3B0)|_BV(COM3C0)); } // SIG_OVERFLOW1 - this interrupt handler starts the pulse for servos // 4, 5, & 6; the timer 1 output compare function automatically ends // the pulse precisely as specified by the OCR1x register which // represents the servo position SIGNAL(SIG_OVERFLOW1) { TCNT1 = 0; // configure to set outputs on compare match so we can turn on the // pulse in the next statement TCCR1A |= _BV(COM1A1)|_BV(COM1A0)|_BV(COM1B1)|_BV(COM1B0)|_BV(COM1C1)|_BV(COM1C0); // force compare match to set outputs TCCR1C |= _BV(FOC1A)|_BV(FOC1B)|_BV(FOC1C); // configure to clear outputs on compare match so that the output // compare function ends the pulse TCCR1A &= ~(_BV(COM1A0)|_BV(COM1B0)|_BV(COM1C0)); } void init_servos(void) { // Use Timers 1 and 3 to generate the pulses for 6 R/C servos; each // timer can do up to 3 servos. // configure OC3A for mode 0: normal, top=0xffff prescale=8 (f~=30): // WGM33=0, WGM23=0, WGM13=0, WGM03=0, CS32=0, CS31=1, CS30=0 DDRE |= _BV(PORTE3) | _BV(PORTE4) | _BV(PORTE5); TCCR3A &= ~(_BV(WGM31) | _BV(WGM30) | _BV(COM3A1) | _BV(COM3B1) | _BV(COM3C1)); TCCR3A |= _BV(COM3A0) | _BV(COM3B0) | _BV(COM3C0); TCCR3B &= ~(_BV(WGM33) | _BV(WGM32) | _BV(CS32) | _BV(CS30)); TCCR3B |= _BV(CS31); TCNT3 = 0; TCCR3C |= _BV(FOC3A) | _BV(FOC3B) | _BV(FOC3C); ETIMSK |= _BV(TOIE3); // configure OC1A for mode 0: normal, top=0xffff prescale=8 (f~=30): // WGM33=0, WGM23=0, WGM13=0, WGM03=0, CS32=0, CS31=1, CS30=0 DDRB |= _BV(PORTB5) | _BV(PORTB6) | _BV(PORTB7); TCCR1A &= ~(_BV(WGM11) | _BV(WGM10) | _BV(COM1A1) | _BV(COM1B1) | _BV(COM1C1)); TCCR1A |= _BV(COM1A0) | _BV(COM1B0) | _BV(COM1C0); TCCR1B &= ~(_BV(WGM13) | _BV(WGM12) | _BV(CS12) | _BV(CS10)); TCCR1B |= _BV(CS11); TCNT1 = 0; TCCR1C |= _BV(FOC1A) | _BV(FOC1B) | _BV(FOC1C); TIMSK |= _BV(TOIE1); // set all servos to their initial positions SERVO1 = SERVO1_INIT; SERVO2 = SERVO2_INIT; SERVO3 = SERVO3_INIT; SERVO4 = SERVO4_INIT; SERVO5 = SERVO5_INIT; SERVO6 = SERVO6_INIT; }

128 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: srf04.h $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Devantech SRF04 Ultrasonic Range Finder Device Driver //============================================================================ #ifndef __SRF04_H__ #define __SRF04_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** // Returns the distance read from the SRF04 Ultrasonic Ranger // is passed the mask of the port bit corrisponding // with the Init and Echo of the sensor int srf_range(int, int); #endif

129 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: srf04.c $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // Devantech SRF04 Ultrasonic Range Finder Device Driver //============================================================================ #define F_CPU 14745600UL #define PortUsed PORTC #define DDRUsed DDRC #define PinUsed PINC #include <avr/io.h> #include <util/delay.h> #include <avr/interrupt.h> //start timer1 with prescale #define start_timer1() TCCR1B = 1<<CS12 //stop timer1 #define stop_timer1() TCCR1B = 0; //unsigned int TIM16_ReadTCNT1(void); int srf_range(int pinOut, int pinIn) { int count = 0; int distance = 0; int starttime = 0; int stoptime = 0; DDRUsed = pinOut; PortUsed = pinOut; _delay_us(30); PortUsed = 0x00; _delay_us(10); //let the sonar fire blind while((PinUsed & pinIn) == 0x00) { //DDRB = 0b11100001; //Blink On the LED //PORTB = 0b00000001; }//Infinite Loop until PinIn goes High //Start or record timer //start counting time until echo returns starttime = TCNT1;//(int)TIM16_ReadTCNT1(); start_timer1(); _delay_us(100); while(((PinUsed & pinIn) == pinIn)&&(count<30000)) { count++; DDRB = 0b11100001; //Blink Off the LED PORTB = 0b00000000; }//Infinite Loop until PinIn goes Low //Stop or calculate timer //stop timer from counting stop_timer1(); stoptime = TCNT1;//(int)TIM16_ReadTCNT1(); distance = stoptime-starttime;

130 | P a g e

//Re-Initialize the Timer Driven Servos TCCR1B &= ~(_BV(WGM13) | _BV(WGM12) | _BV(CS12) | _BV(CS10)); TCCR1B |= _BV(CS11); TCNT1 = 0; return distance; } /* unsigned int TIM16_ReadTCNT1(void) { unsigned int i; unsigned char sreg; // Save Global Interrupt Flag sreg = SREG; // Disable interrupts cli(); // Set i to TCNT1 i = TCNT1; // Restore Global Interrupt Flag SREG = sreg; //Return i return i; } */

131 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: uart.h $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // UART Communication Driver //============================================================================ #ifndef __UART_H__ #define __UART_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** // Initializes the UART void init_uart(void); // Puts a Char to UART0 Polled int uart_putc0(char); // Puts a Char to UART0 int uputc0(char, FILE *); // Puts a Char to UART1 Polled int uart_putc1(char); // Puts a Char to UART1 int uputc1(char, FILE *); #endif

132 | P a g e

//========================= File Information =============================== // // Robo-Magellan Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // David McDougall dwmcdougall@gmail.com // Utah State University // // $Workfile: uart.c $ // $Revision: 1 $ // $Modtime: 14 Dec 2007 $ // // UART Communication Driver //============================================================================ #define BAUD0 95 /* 9600 BAUD at 14... MHz */ #define BAUD1 191 /* times two 2400 BAUD at 14... MHz */ #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> //Global UART Variables volatile uint8_t urx0; volatile uint8_t urx_recv0; volatile uint8_t urx1; volatile uint8_t urx_recv1; // UART0 character output routine (polled) int uart_putc0(char c) { while ((UCSR0A & _BV(UDRE)) == 0) { } UDR0 = c; return 0; } // UART1 character output routine (polled) int uart_putc1(char c) { while ((UCSR1A & _BV(UDRE)) == 0) { } UDR1 = c; return 0; } int uputc0(char c, FILE* f) { return uart_putc0(c); } int uputc1(char c, FILE* f) { return uart_putc1(c); } void init_uart(void) { // Enable UART0 UBRR0H = 0; UBRR0L = BAUD0; UCSR0B = _BV(RXEN)|_BV(TXEN)|_BV(RXCIE);

133 | P a g e

// Enable UART1 UBRR1H = 0; UBRR1L = BAUD1; UCSR1B = _BV(RXEN)|_BV(TXEN)|_BV(RXCIE); // Enable interrupts //sei(); // initialize stdio for printf fdevopen(uputc0, NULL); } // UART0 byte received interrupt handler - fetch the character, // check for framing error, flag the event, return. SIGNAL(SIG_UART0_RECV) { uint8_t s; s = UCSR0A; urx0 = UDR0; if (bit_is_clear(s, FE)) { urx_recv0 = 1; } } // UART1 byte received interrupt handler - fetch the character, // check for framing error, flag the event, return. SIGNAL(SIG_UART1_RECV) { uint8_t t; t = UCSR1A; urx1 = UDR1; if (bit_is_clear(t, FE)) { urx_recv1 = 1; } }

134 | P a g e

C.5 Tandy-Bot Firmware //========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: main.h $ // $Revision: 1 $ // $Modtime: 26 April 2008 $ // // Communication and Command Line Interpreter //============================================================================ #ifndef __MAIN_H__ #define __MAIN_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** #endif

135 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: main.c $ // $Revision: 1 $ // $Modtime: 26 April 2008 $ // // Communication and Command Line Interpreter //============================================================================ #include "defines.h" #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "main.h" #include "uart.h" #include "command.h" #include "router_in.h" #include "router_out.h" #include "router_control.h" int main(void) { char ch; init_inports(); init_outports(); init_controlports(); /* initialize ports */ while(0) { TXCTS_Set(0xF0); //toggle_port(); _delay_ms(1000); TXCTS_Set(0x00); //toggle_port(); _delay_ms(1000); TXCTS_Set(0xF0); //toggle_port(); _delay_ms(1000); TXCTS_Set(0x00); //toggle_port(); _delay_ms(1000); TXCTS_Set(0xF0); //toggle_port(); _delay_ms(1000); TXCTS_Set(0x00); //toggle_port(); _delay_ms(1000); } /* initialize uart */ init_uart(); TX_PC_Enable(); //This enables the Command Module to Transmit /* enable interrupts */ sei(); //This is done in init_uart(); /* output startup announcement message */ printf_P(s_annc);

136 | P a g e

/* output a prompt */ printf_P(s_prompt); TX_PC_Disable(); //This disables the Command Modules ability to Transmit /* loop - wait for commands from the serial interface */ //TX_PC_Enable(); //This enables the Command Module to Transmit while (1) { ch = uart_getc(); if (ch) { //uart_putc(ch); recv_input(ch); } } } //asdasdasdasdsad</ma,m1,m2,m5,m6,m7>asdasdasdasdasdasdasdsa

137 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: defines.h $ // $Revision: 1 $ // $Modtime: 26 April 2008 $ // // Compile Time Defines //============================================================================ #ifndef __DEFINES_H__ #define __DEFINES_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** #define F_CPU 8000000UL #define F_OSC 8000000UL #define CMD_BUF_LEN 32 #define BAUDRATE 207 // 2400 BAUD at 8 MHz //#define BAUDRATE 103 // 4800 BAUD at 8 MHz //#define BAUDRATE 51 // 9600 BAUD at 8 MHz //#define BAUDRATE 34 // 14400 BAUD at 8 MHz //#define BAUDRATE 25 // 19200 BAUD at 8 MHz #define _ECHO_INPUT_ 1 #endif

138 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: globalvars.h $ // $Revision: 1 $ // $Modtime: 26 April 2008 $ // // Global Variable Definitions //============================================================================ #ifndef __GLOBALVARS_H__ #define __GLOBALVARS_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** // uart.c extern char urx; extern char urx_recv; //command.c extern char recv_input_cmdbuf[CMD_BUF_LEN]; extern char do_cmdbuf[20]; // command.c extern char s_annc[]; extern char s_prompt[]; extern char s_invalid[]; extern char s_bksp[]; // router.c extern uint8_t RX_Port_Status; extern uint8_t TXCTS_Port_Status; extern uint8_t RTS_Port_Status; extern uint8_t RTS_Host_Last; extern uint8_t RTS_Module_Last; extern uint8_t Request_Host_Register; #endif

139 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: globalvars.c $ // $Revision: 1 $ // $Modtime: 26 April 2008 $ // // Global Variable Instantiations //============================================================================ #include "defines.h" #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "main.h" #include "uart.h" #include "command.h" #include "router_in.h" #include "router_out.h" #include "router_control.h" // uart.c char urx; char urx_recv; //command.c char recv_input_cmdbuf[CMD_BUF_LEN]; char do_cmdbuf[20]; // command.c char s_annc[] PROGMEM = "\n\nMODULAR ROBOT INTERFACE\n\n"; char s_prompt[] PROGMEM = "\n"; char s_invalid[] PROGMEM = "[]\n"; char s_bksp[] PROGMEM = "\b \b"; // router.c uint8_t RX_Port_Status; uint8_t TXCTS_Port_Status; uint8_t RTS_Port_Status; uint8_t RTS_Host_Last; uint8_t RTS_Module_Last; uint8_t Request_Host_Register;

140 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // Adapted from: gp2d12.zip www.bdmicro.com // BDMicro Sample Code // // $Workfile: command.h $ // $Revision: 1 $ // $Modtime: 26 April 2008 $ // // Command Interpreter for Atmega16 //============================================================================ #ifndef __COMMAND_H__ #define __COMMAND_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** // Decode and execute simple commands //static __inline__ void do_cmd(char *); // accept characters and build a command line, when return is pressed, // pass the command to 'do_cmd()' void recv_input(uint8_t); //Parse a interger froma string uint16_t get_num(char*, char**, int8_t*); #endif

141 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // Adapted from: gp2d12.zip www.bdmicro.com // BDMicro Sample Code // // $Workfile: command.c $ // $Revision: 1 $ // $Modtime: 26 April 2008 $ // // Command Interpreter for Atmega16 //============================================================================ #include "defines.h" #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "main.h" #include "uart.h" #include "command.h" #include "router_in.h" #include "router_out.h" #include "router_control.h" // Decode and execute simple commands //static __inline__ void do_cmd(char * s) { static char * cmd = do_cmdbuf; uint8_t index; char * args, * p; int8_t rc; if (s[0] == 0) return; // parse the command line, seperating the command from arguments cmd[0] = 0; index = 0; while ((index < sizeof(do_cmdbuf)) && s[index] && (s[index] != ' ')) { cmd[index] = s[index]; index++; } if (index < sizeof(do_cmdbuf)) { cmd[index] = 0; args = &s[index]; while (*args && (*args == ' ')) args++; if (*args == 0) args = NULL; } else { cmd[sizeof(do_cmdbuf)-1] = 0; args = NULL;

142 | P a g e

} if (cmd[0] == 0) { return; } if (strcmp(cmd, "?") == 0) { printf_P(s_annc); } /* else if (strcmp(cmd, "sa") == 0) { if (args) { int16_t s1, s2, s3, s4, s5, s6, err=0; p = args; s1 = get_num(p, &p, &rc); err += rc; s2 = get_num(p, &p, &rc); err += rc; s3 = get_num(p, &p, &rc); err += rc; s4 = get_num(p, &p, &rc); err += rc; s5 = get_num(p, &p, &rc); err += rc; s6 = get_num(p, &p, &rc); err += rc; if (err == 0) { // servo(s1, s2, s3, s4, s5, s6); printf("[sa %u %u %u %u %u %u]\n", s1, s2, s3, s4, s5, s6); } } } else if (strcmp(cmd, "s1") == 0) { if (args) { int16_t s1, err=0; p = args; s1 = get_num(p, &p, &rc); printf("[s1 %u]\n", s1); err += rc; // if (err == 0) // servo1(s1); } }*/ else if (strcmp(cmd, "tm") == 0) { if (args) { int16_t s1, err=0; p = args; s1 = get_num(p, &p, &rc); //printf("[s1 %u]\n", s1); err += rc; if (err == 0) TXCTS_Set(s1); } } else if (strcmp(cmd, "r0") == 0) { //TXCTS_Set(0x00); CTS_PC_Set(); } else if (strcmp(cmd, "r1") == 0) { //CTS_PC_Clear(); TXCTS_Set(0x01);

143 | P a g e

//Add_Service_Request(0x01); } else if (strcmp(cmd, "r2") == 0) { //CTS_PC_Clear(); TXCTS_Set(0x02); //Add_Service_Request(0x02); } else if (strcmp(cmd, "r3") == 0) { //CTS_PC_Clear(); TXCTS_Set(0x04); //Add_Service_Request(0x04); } else if (strcmp(cmd, "r4") == 0) { //CTS_PC_Clear(); TXCTS_Set(0x08); //Add_Service_Request(0x08); } else if (strcmp(cmd, "r5") == 0) { //CTS_PC_Clear(); TXCTS_Set(0x10); //Add_Service_Request(0x10); } else if (strcmp(cmd, "r6") == 0) { //CTS_PC_Clear(); TXCTS_Set(0x20); //Add_Service_Request(0x20); } else if (strcmp(cmd, "r7") == 0) { //CTS_PC_Clear(); TXCTS_Set(0x40); //Add_Service_Request(0x40); } else if (strcmp(cmd, "r8") == 0) { //CTS_PC_Clear(); TXCTS_Set(0x80); //Add_Service_Request(0x80); } else if (strcmp(cmd, "wa") == 0) { RX_Enable(0xFF); } else if (strcmp(cmd, "/wa") == 0) { RX_Disable(0xFF); } else if (strcmp(cmd, "w0") == 0) { //Enables Comm Module CTS_PC_Set(); TX_PC_Enable(); //This enables the Command Module to Transmit } else if (strcmp(cmd, "/w0") == 0) { //Disables Comm Module TX_PC_Disable(); //This enables the Command Module to Transmit CTS_PC_Clear(); } else if (strcmp(cmd, "w1") == 0) { RX_Enable(0x01); } else if (strcmp(cmd, "/w1") == 0) { RX_Disable(0x01);

144 | P a g e

} else if (strcmp(cmd, "w2") == 0) { RX_Enable(0x02); } else if (strcmp(cmd, "/w2") == 0) { RX_Disable(0x02); } else if (strcmp(cmd, "w3") == 0) { RX_Enable(0x04); } else if (strcmp(cmd, "/w3") == 0) { RX_Disable(0x04); } else if (strcmp(cmd, "w4") == 0) { RX_Enable(0x08); } else if (strcmp(cmd, "/w4") == 0) { RX_Disable(0x08); } else if (strcmp(cmd, "w5") == 0) { RX_Enable(0x10); } else if (strcmp(cmd, "/w5") == 0) { RX_Disable(0x10); } else if (strcmp(cmd, "w6") == 0) { RX_Enable(0x20); } else if (strcmp(cmd, "/w6") == 0) { RX_Disable(0x20); } else if (strcmp(cmd, "w7") == 0) { RX_Enable(0x40); } else if (strcmp(cmd, "/w7") == 0) { RX_Disable(0x40); } else if (strcmp(cmd, "w8") == 0) { RX_Enable(0x80); } else if (strcmp(cmd, "/w8") == 0) { RX_Disable(0x80); } else { printf_P(s_invalid); //printf_P(s_usage); } return; } // accept characters and build a command line, when return is pressed, // pass the command to 'do_cmd()' void recv_input(uint8_t ch) {

145 | P a g e

static uint8_t idx=0; if ((ch == '\b') && idx) { if (_ECHO_INPUT_) printf_P(s_bksp); idx--; recv_input_cmdbuf[idx-1] = 0; } else if (ch == '<') { if (_ECHO_INPUT_) uart_putc(ch); idx = 1; //recv_input_cmdbuf[idx++] = ch; RX_Mute(); } else if ((ch == '>')&&(idx)) { if (_ECHO_INPUT_) uart_putc(ch); recv_input_cmdbuf[idx-1] = 0; do_cmd(recv_input_cmdbuf); idx = 0; RX_Restore(); } else if (((ch == ',')||(ch == ';'))&&(idx)) { if (_ECHO_INPUT_) uart_putc('+'); recv_input_cmdbuf[idx-1] = 0; do_cmd(recv_input_cmdbuf); idx = 1; } else if (idx) { if (_ECHO_INPUT_) uart_putc(ch); recv_input_cmdbuf[idx-1] = ch; idx++; if (idx == CMD_BUF_LEN) //Prevent Overflow { idx = 1; recv_input_cmdbuf[idx-1] = 0; } } else { //uart_putc(ch); // Do nothing, ignore traffic } } // parse an integer from a string uint16_t get_num(char * s, char ** next, int8_t * err) { char * e; uint16_t n; n = strtoul(s, &e, 0); if (e == s) { printf("error: get_num(): can't scan arg: \"%s\" \n", s); *next = s; if (err) *err = 1; return 0xffff; } while (*e == ' ') e++; *next = e; if (err)

146 | P a g e

*err = 0; return n; }

147 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: router.h $ // $Revision: 1 $ // $Modtime: 24 March 2008 $ // // Serial Traffic Controller //============================================================================ #ifndef __ROUTER_H__ #define __ROUTER_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** void init_ports(void); void toggle_port(void); void RX_Mute(void); void RX_Unmute(void); void RX_Enable(uint8_t mask); void RX_Disable(uint8_t mask); #endif

148 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: router.c $ // $Revision: 1 $ // $Modtime: 24 March 2008 $ // // Serial Traffic Controller //============================================================================ #include "defines.h" #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "main.h" #include "uart.h" #include "command.h" #include "router.h" /* #define DDR_RX_Port DDRC #define RX_Port PORTC #define RX_0 PINC0 #define RX_1 PINC1 #define RX_2 PINC2 #define RX_3 PINC3 #define RX_4 PINC4 #define RX_5 PINC5 #define RX_6 PINC6 #define RX_7 PINC7*/ void init_inports(void) { // Setup Port Directions DDR_RX_Port = 0b11111111; DDR_RTS_Port = 0b00000000; DDR_TXCTS_Port = 0b11111111; DDR_IO_Port = 0b11000010; //RX-IN, TX-OUT, UNUSED2, UNUSED3, UNUSED4, RTS-IN, CTS-OUT, TX-ENA // Initialize Port Values RX_Init(); TXCTS_Port = 0b11111111; TXOUT_Pin = 0b1; CTSOUT_Pin = 0b1; TXENA_Pin = 0b1; } void TX_Enable(void) { TXENA_Pin = 0b0; } void TX_Disable(void) { TXENA_Pin = 0b1;

149 | P a g e

} void RX_Init(void) { RX_Port_Status = 0xFF; RX_Port = RX_Port_Status; } void RX_Mute(void) { RX_Port_Status = RX_Port; RX_Port = 0x00; } void RX_Unmute(void) { RX_Port = RX_Port_Status; } void RX_Enable(uint8_t mask) { RX_Port_Status = RX_Port; RX_Port_Status = mask && RX_Port_Status; RX_Port = RX_Port_Status; } void RX_Disable(uint8_t mask) { RX_Port_Status = RX_Port; RX_Port_Status = mask || RX_Port_Status; RX_Port = RX_Port_Status; }

150 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: router_control.h $ // $Revision: 1 $ // $Modtime: 31 March 2008 $ // // Serial Traffic Controller - Control Pins //============================================================================ #ifndef __ROUTER_CONTROL_H__ #define __ROUTER_CONTROL_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** void init_controlports(void); void CTS_PC_Set(void); void CTS_PC_Clear(void); void TX_PC_Enable(void); void TX_PC_Disable(void); uint8_t Check_RTS_PC(void); #endif

151 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: router_control.c $ // $Revision: 1 $ // $Modtime: 31 March 2008 $ // // Serial Traffic Controller - Control Pins //============================================================================ #include "defines.h" #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "main.h" #include "uart.h" #include "command.h" #include "router_in.h" #include "router_out.h" #include "router_control.h" #define DDR_CONTROL_Port DDRD #define CONTROL_Port PORTD #define RXIN_Pin PIND0 #define DDR_RXIN DDD0 #define TXOUT_Pin PIND1 #define DDR_TXOUT DDD1 #define UNUSED2_Pin PIND2 #define DDR_UNUSED2 DDD2 #define UNUSED3_Pin PIND3 #define DDR_UNUSED3 DDD3 #define UNUSED4_Pin PIND4 #define DDR_UNUSED4 DDD4 #define RTSIN_Pin PIND5 #define DDR_RTSIN DDD5 #define CTSOUT_Pin 0b01000000 //PIND6 #define DDR_CTSOUT DDD6 #define TXENA_Pin 0b10000000 //PIND7 #define DDR_TXENA DDD7 void init_controlports(void) { // Setup Port Directions DDR_CONTROL_Port = 0b11000010; //TXENA, CTSOUT, RTSIN, UNUSED4, UNUSED3, UNUSED2, TXOUT, RXIN // Initialize Port Values //TXOUT_Pin = 0b1;//THis is the Uart TX Pin CTS_PC_Clear(); TX_PC_Disable(); } void CTS_PC_Set(void) { CONTROL_Port = CONTROL_Port & ~CTSOUT_Pin;// = 0b0; }

152 | P a g e

void CTS_PC_Clear(void) { CONTROL_Port = CONTROL_Port | CTSOUT_Pin;// = 0b1; } void TX_PC_Enable(void) { CONTROL_Port = CONTROL_Port & ~TXENA_Pin;// = 0b0; } void TX_PC_Disable(void) { CONTROL_Port = CONTROL_Port | TXENA_Pin;// = 0b1; } uint8_t Check_RTS_PC(void) { uint8_t pinstate; pinstate = RTSIN_Pin; if(pinstate==0x00) return 1; else return 0; }

153 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: router_in.h $ // $Revision: 1 $ // $Modtime: 31 March 2008 $ // // Serial Traffic Controller - Receive Data Pins //============================================================================ #ifndef __ROUTER_IN_H__ #define __ROUTER_IN_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** void init_inports(void); void RX_Init(void); void RX_Mute(void); void RX_All(void); void RX_Restore(void); void RX_Enable(uint8_t); void RX_Disable(uint8_t); #endif

154 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: router_in.c $ // $Revision: 1 $ // $Modtime: 31 March 2008 $ // // Serial Traffic Controller - Receive Data Pins //============================================================================ #include "defines.h" #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "main.h" #include "uart.h" #include "command.h" #include "router_in.h" #include "router_out.h" #include "router_control.h" #define DDR_RX_Port DDRC #define RX_Port PORTC #define RX_0 PINC0 #define RX_1 PINC1 #define RX_2 PINC2 #define RX_3 PINC3 #define RX_4 PINC4 #define RX_5 PINC5 #define RX_6 PINC6 #define RX_7 PINC7 void init_inports(void) { // Setup Port Directions DDR_RX_Port = 0b11111111; // Initialize Port Values RX_Init(); } void RX_Init(void) { RX_Port_Status = 0xFF; RX_Port = RX_Port_Status; } void RX_Mute(void) { //RX_Port_Status = RX_Port; RX_Port = 0xFF; } void RX_All(void) { //RX_Port_Status = RX_Port;

155 | P a g e

RX_Port = 0x00; } void RX_Restore(void) { RX_Port = RX_Port_Status; } void RX_Enable(uint8_t mask) { mask = ~mask; //RX_Port_Status = RX_Port; RX_Port_Status = mask & RX_Port_Status; //RX_Port = RX_Port_Status; } void RX_Disable(uint8_t mask) { //RX_Port_Status = RX_Port; RX_Port_Status = mask | RX_Port_Status; //RX_Port = RX_Port_Status; }

156 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: router_out.h $ // $Revision: 1 $ // $Modtime: 25 April 2008 $ // // Serial Traffic Controller - Transmit Data Pins //============================================================================ #ifndef __ROUTER_OUT_H__ #define __ROUTER_OUT_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** void init_outports(void); void Clear_Service_Requests(void); void Check_and_Service_Requests(void); uint8_t Poll_Service_Busy(void); uint8_t Poll_Service_Request_Host(void); uint8_t Poll_Service_Request_Module(void); void Add_Service_Request(uint8_t value); void Remove_Service_Request(uint8_t value); uint8_t Query_Service_Request(void); //void TXCTS_Set(uint8_t value); //void TXCTS_Disable(void); #endif

157 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // // $Workfile: router_out.c $ // $Revision: 1 $ // $Modtime: 25 April 2008 $ // // Serial Traffic Controller - Transmit Data Pins //============================================================================ #include "defines.h" #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "main.h" #include "uart.h" #include "command.h" #include "router_in.h" #include "router_out.h" #include "router_control.h" #define DDR_TXCTS_Port DDRB #define TXCTS_Port PORTB #define DDR_RTS_Port DDRA #define RTS_Port PORTA void init_outports(void) { // Setup Port Directions DDR_RTS_Port = 0b00000000; DDR_TXCTS_Port = 0b11111111; // Initialize Port Values TXCTS_Port = 0b11111111; } void Clear_Service_Requests(void) { Request_Host_Register = 0x00; RTS_Host_Last = 0x01; RTS_Module_Last = 0x01; } void Check_and_Service_Requests(void) { uint8_t status; //Checks if any Requests are being serviced /*if(1) status = Poll_Service_Busy();*/ //If None, then checks if any Requests have been requested by the host if(status == 0x00) status = Poll_Service_Request_Host(); //If None, then checks if any Requests have been requested by the modules if(status == 0x00) status = Poll_Service_Request_Module(); //Exits if no requests //or gives service to any returned routine in register "status"

158 | P a g e

TXCTS_Set(status); } uint8_t Poll_Service_Busy(void) { uint8_t status; RTS_Port_Status = RTS_Port; status = TXCTS_Port_Status | RTS_Port_Status; return status; } uint8_t Poll_Service_Request_Host(void) { uint8_t count; uint8_t mask; //Request_Host_Register count = 1; while(count<8) { if (RTS_Host_Last >= 8) { RTS_Host_Last = 0; } RTS_Host_Last++; mask = (0x01 << RTS_Host_Last); if(Request_Host_Register & mask) { Request_Host_Register = Request_Host_Register & ~mask; return RTS_Host_Last; } count++; } return 0; } uint8_t Poll_Service_Request_Module(void) { uint8_t count; uint8_t mask; //Request_Host_Register RTS_Port_Status = RTS_Port; count = 0; while(count<8) { if (RTS_Module_Last >= 8) { RTS_Module_Last = 0; } RTS_Module_Last++; mask = (0x01 << RTS_Module_Last); if(RTS_Port_Status & mask) { return RTS_Module_Last; } count++; } return 0; } void Add_Service_Request(uint8_t value) { Request_Host_Register = Request_Host_Register & ~value; } void Remove_Service_Request(uint8_t value) { Request_Host_Register = Request_Host_Register | value; } uint8_t Query_Service_Request(void) {

159 | P a g e

return Request_Host_Register; } void TXCTS_Set(uint8_t value) { TXCTS_Port_Status = ~value; //TXCTS_Port_Status = mask && TXCTS_Port_Status; TXCTS_Port = TXCTS_Port_Status; } void TXCTS_Disable(void) { TXCTS_Set(0b11111111); }

160 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // Adapted from: gp2d12.zip www.bdmicro.com // BDMicro Sample Code // // $Workfile: uart.h $ // $Revision: 1 $ // $Modtime: 07 March 2008 $ // // UART Communication Driver for Atmega16 //============================================================================ #ifndef __UART_H__ #define __UART_H__ // ***************************************************************************** // Function Prototypes // ***************************************************************************** // Initializes the UART void init_uart(void); // Puts a Char to UART Polled int uart_putc(char); // Puts a Char to UART int uputc(char, FILE *); // Checks the UART for a Charactor int check_getc(void); // Checks the UART for a Charactor char uart_getc(void); #endif

161 | P a g e

//========================= File Information =============================== // // Modular Robot Platform // // Created by: Robin Bailey robin@robinbailey.com // Utah State University // Adapted from: gp2d12.zip www.bdmicro.com // BDMicro Sample Code // // $Workfile: uart.c $ // $Revision: 1 $ // $Modtime: 07 March 2008 $ // // UART Communication Driver for Atmega16 //============================================================================ #include "defines.h" #include <avr/io.h> #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <util/delay.h> #include <stdio.h> #include <ctype.h> #include <stdlib.h> #include <string.h> #include "globalvars.h" #include "main.h" #include "uart.h" #include "command.h" #include "router_in.h" #include "router_out.h" #include "router_control.h" // UART character output routine (polled) int uart_putc(char c) { while ((UCSRA & _BV(UDRE)) == 0) { } UDR = c; return 0; } int uputc(char c, FILE* f) { return uart_putc(c); } void init_uart(void) { // Enable UART UBRRH = 0; UBRRL = BAUDRATE; UCSRB = _BV(RXEN)|_BV(TXEN)|_BV(RXCIE); // Enable interrupts //sei(); //This can be done in main // initialize stdio for printf fdevopen(uputc, NULL); } // UART byte received interrupt handler - fetch the character, // check for framing error, flag the event, return. SIGNAL(SIG_UART_RECV)

162 | P a g e

{ char s; s = UCSRA; urx = UDR; if (bit_is_clear(s, FE)) { urx_recv = 1; } } // Checks the UART for a Charactor int check_getc(void) { return urx_recv; } // Returns a Charactor from the UART char uart_getc(void) { char ch; if (urx_recv) { cli(); urx_recv = 0; ch = urx; sei(); return ch; } return 0; }