CENG 499 Final Report Autonomous Robotics Projectelec499/2003a/group12/docs/final.pdf · signing a robotics platform to meet the requirements we laid out during the inception phase

CENG 499 Final Report

Autonomous Robotics Project

Jordan Reynolds [email protected] Jacox - [email protected]

http://www.talisnet.ca/robot

August 1, 2003

mailto:[email protected]

mailto:[email protected]

CONTENTS CONTENTS

Contents

1 Purpose 1

2 Overall design discussion 22.1 Mechanical design . . . . . . . . . . . . . . . . . . . . . 22.2 Electrical design . . . . . . . . . . . . . . . . . . . . . . 22.3 software design . . . . . . . . . . . . . . . . . . . . . . . 22.4 Overall system . . . . . . . . . . . . . . . . . . . . . . . 3

3 CPU Board 33.1 Operating System . . . . . . . . . . . . . . . . . . . . . 3

3.1.1 Design System Conclusions . . . . . . . . . . . . 33.1.2 Future considerations . . . . . . . . . . . . . . . 3

3.2 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . 43.2.1 Hardware design conclusions . . . . . . . . . . . . 43.2.2 Future Considerations . . . . . . . . . . . . . . . 4

3.3 Electrical . . . . . . . . . . . . . . . . . . . . . . . . . . 43.3.1 Electrical Design Conclusions . . . . . . . . . . . 43.3.2 Future Considerations . . . . . . . . . . . . . . . 5

4 Remote Communications 5

5 Safety Systems 6

6 Battery 6

7 Bus Wiring 7

8 Sensor Network 78.1 Sonar . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

8.1.1 Sonar sensor conclusion . . . . . . . . . . . . . . 78.1.2 Sonar sensor future considerations . . . . . . . . . 9

8.2 Infrared . . . . . . . . . . . . . . . . . . . . . . . . . . . 98.2.1 Infrared sensor conclusion . . . . . . . . . . . . . 98.2.2 Infrared sensor future considerations . . . . . . . . 12

9 Mechanical Design 129.1 Drive train . . . . . . . . . . . . . . . . . . . . . . . . . 129.2 Chassis . . . . . . . . . . . . . . . . . . . . . . . . . . . 129.3 Mechanical design future considerations . . . . . . . . . . 14

i

CONTENTS CONTENTS

10 Satellite Processors 1410.1 Serial Protocol . . . . . . . . . . . . . . . . . . . . . . . 1410.2 PWM . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1710.3 Analogue and Digital . . . . . . . . . . . . . . . . . . . . 1710.4 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . 1810.5 Software . . . . . . . . . . . . . . . . . . . . . . . . . . 18

11 Control algorithms 2011.1 Software Framework . . . . . . . . . . . . . . . . . . . . 2011.2 Inter-process message structure . . . . . . . . . . . . . . 2111.3 Rules framework . . . . . . . . . . . . . . . . . . . . . . 23

11.3.1 Avoid behavior . . . . . . . . . . . . . . . . . . . 2411.3.2 Veer behavior . . . . . . . . . . . . . . . . . . . 2511.3.3 Side avoid behavior . . . . . . . . . . . . . . . . 2511.3.4 Skim behavior . . . . . . . . . . . . . . . . . . . 2511.3.5 Run behavior . . . . . . . . . . . . . . . . . . . . 25

11.4 PI control loop . . . . . . . . . . . . . . . . . . . . . . . 2511.4.1 Limitations of the control system . . . . . . . . . 26

12 Project budget 26

13 Future enhancements 27

14 Special thanks and final words 28

A Embedded PC Code 29A.1 Makefile . . . . . . . . . . . . . . . . . . . . . . . . . . 29A.2 Common Library . . . . . . . . . . . . . . . . . . . . . . 31A.3 Port Definitions . . . . . . . . . . . . . . . . . . . . . . 34A.4 Server Code . . . . . . . . . . . . . . . . . . . . . . . . 40

A.4.1 robot.h . . . . . . . . . . . . . . . . . . . . . . . 40A.4.2 robot.c . . . . . . . . . . . . . . . . . . . . . . . 40

A.5 Subsumption Controller . . . . . . . . . . . . . . . . . . 50A.5.1 subControl.h . . . . . . . . . . . . . . . . . . . . 50A.5.2 subControl.c . . . . . . . . . . . . . . . . . . . . 50

A.6 Motor Control Process . . . . . . . . . . . . . . . . . . . 63A.6.1 motorProcess.h . . . . . . . . . . . . . . . . . . 63A.6.2 motorProcess.c . . . . . . . . . . . . . . . . . . . 63A.6.3 motorPIControl.h . . . . . . . . . . . . . . . . . 71A.6.4 motorPIControl.c . . . . . . . . . . . . . . . . . . 71

A.7 IR Sensor Process . . . . . . . . . . . . . . . . . . . . . 74A.7.1 irProcess.h . . . . . . . . . . . . . . . . . . . . . 74A.7.2 irProcess.c . . . . . . . . . . . . . . . . . . . . . 74

ii

CONTENTS CONTENTS

A.8 Sonar Sensor Process . . . . . . . . . . . . . . . . . . . 79A.8.1 sonarProcess.h . . . . . . . . . . . . . . . . . . . 79A.8.2 sonarProcess.c . . . . . . . . . . . . . . . . . . . 79

A.9 Serial Read Process . . . . . . . . . . . . . . . . . . . . 85A.9.1 serialRead.h . . . . . . . . . . . . . . . . . . . . 85A.9.2 serialRead.c . . . . . . . . . . . . . . . . . . . . 85

A.10 Serial Write Process . . . . . . . . . . . . . . . . . . . . 90A.10.1 serialWrite.h . . . . . . . . . . . . . . . . . . . . 90A.10.2 serialWrite.c . . . . . . . . . . . . . . . . . . . . 90

A.11 Linux version of DOS PC.H . . . . . . . . . . . . . . . . 93A.11.1 pc.h . . . . . . . . . . . . . . . . . . . . . . . . 93A.11.2 pc.c . . . . . . . . . . . . . . . . . . . . . . . . 93

B Satellite processor code 95B.1 PIC Common Serial Module . . . . . . . . . . . . . . . . 95

B.1.1 aserial.h . . . . . . . . . . . . . . . . . . . . . . 95B.1.2 aserial.c . . . . . . . . . . . . . . . . . . . . . . 97

B.2 PIC PWM ISR . . . . . . . . . . . . . . . . . . . . . . . 104B.2.1 aserial.h . . . . . . . . . . . . . . . . . . . . . . 104B.2.2 aserial.c . . . . . . . . . . . . . . . . . . . . . . 104

B.3 PIC PWM . . . . . . . . . . . . . . . . . . . . . . . . . 107B.3.1 aserial.h . . . . . . . . . . . . . . . . . . . . . . 107B.3.2 aserial.c . . . . . . . . . . . . . . . . . . . . . . 109

B.4 PIC A/D ISR . . . . . . . . . . . . . . . . . . . . . . . . 117B.4.1 aserial.h . . . . . . . . . . . . . . . . . . . . . . 117B.4.2 aserial.c . . . . . . . . . . . . . . . . . . . . . . 117

B.5 PIC A/D . . . . . . . . . . . . . . . . . . . . . . . . . . 121B.5.1 aserial.h . . . . . . . . . . . . . . . . . . . . . . 121B.5.2 aserial.c . . . . . . . . . . . . . . . . . . . . . . 122

B.6 PIC Capture . . . . . . . . . . . . . . . . . . . . . . . . 132B.6.1 aserial.h . . . . . . . . . . . . . . . . . . . . . . 132B.6.2 aserial.c . . . . . . . . . . . . . . . . . . . . . . 132

C Schematics 133C.1 PC Power Supply Schematics . . . . . . . . . . . . . . . 134C.2 Sonar Power Supply Schematics . . . . . . . . . . . . . . 135C.3 Sonar Daughter Board Schematics . . . . . . . . . . . . 136C.4 Satellite processor Schematics . . . . . . . . . . . . . . . 137C.5 Motor Power Amplifier Schematics . . . . . . . . . . . . 138

iii

CONTENTS CONTENTS

D Mechanical Drawings 139D.1 Overall Platform drawings . . . . . . . . . . . . . . . . . 140D.2 Chassis bottom drawings . . . . . . . . . . . . . . . . . . 141D.3 Motor busing drawings . . . . . . . . . . . . . . . . . . . 142D.4 Motor mount drawings . . . . . . . . . . . . . . . . . . . 143D.5 Octagon edge drawings . . . . . . . . . . . . . . . . . . 144D.6 Top octagon drawings . . . . . . . . . . . . . . . . . . . 145

E progress report 1 146E.1 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . 147E.2 Progress . . . . . . . . . . . . . . . . . . . . . . . . . . 148

E.2.1 CPU Board . . . . . . . . . . . . . . . . . . . . . 148E.2.1.1 Operating System . . . . . . . . . . . . . . . 148E.2.1.2 Hardware . . . . . . . . . . . . . . . . . . . 148E.2.1.3 Electrical . . . . . . . . . . . . . . . . . . . 149

E.2.2 Sensor Network . . . . . . . . . . . . . . . . . . 149E.2.2.1 Sonar sensors . . . . . . . . . . . . . . . . . 149E.2.2.2 Infrared sensors . . . . . . . . . . . . . . . . 150

E.2.3 Mechanical Design . . . . . . . . . . . . . . . . . 150E.2.4 Motor Controller . . . . . . . . . . . . . . . . . . 151

E.2.4.1 Control Module . . . . . . . . . . . . . . . . 151E.2.4.2 Power Amplifier . . . . . . . . . . . . . . . . 153

E.2.5 Control algorithms . . . . . . . . . . . . . . . . . 153E.3 Timeline . . . . . . . . . . . . . . . . . . . . . . . . . . 154

F progress report 2 156F.1 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . 157F.2 Progress . . . . . . . . . . . . . . . . . . . . . . . . . . 158

F.2.1 CPU Board . . . . . . . . . . . . . . . . . . . . . 158F.2.1.1 Hardware . . . . . . . . . . . . . . . . . . . 158F.2.1.2 Electrical . . . . . . . . . . . . . . . . . . . 158

F.2.2 Sensor Network . . . . . . . . . . . . . . . . . . 158F.2.2.1 Sonar sensors . . . . . . . . . . . . . . . . . 158F.2.2.2 Infrared sensors . . . . . . . . . . . . . . . . 158

F.2.3 Mechanical Design . . . . . . . . . . . . . . . . . 159F.2.4 Motor Controller . . . . . . . . . . . . . . . . . . 159

F.2.4.1 Control Module . . . . . . . . . . . . . . . . 159F.2.4.2 Power Amplifier . . . . . . . . . . . . . . . . 159

F.2.5 Control algorithms . . . . . . . . . . . . . . . . . 159F.3 Changes since last report : . . . . . . . . . . . . . . . . . 160F.4 Timeline . . . . . . . . . . . . . . . . . . . . . . . . . . 160

iv

LIST OF FIGURES LIST OF FIGURES

List of Figures

1 CPU and hard drive mounted in case. . . . . . . . . . . . 52 Emergency Stop button. . . . . . . . . . . . . . . . . . . 63 Battery mounted on the platform. . . . . . . . . . . . . . 74 Bus wiring terminal block. . . . . . . . . . . . . . . . . . 85 Sonar transmitters mounted on front of chassis . . . . . . 96 Infrared modules mounted on side of platform. . . . . . . 107 Infrared linearization function. . . . . . . . . . . . . . . . 118 Drive train, side shot. . . . . . . . . . . . . . . . . . . . 139 Final fully assembled chassis. . . . . . . . . . . . . . . . 1310 Overall software architecture. . . . . . . . . . . . . . . . 2211 Subsumption rules architecture. . . . . . . . . . . . . . . 2412 PI controller block diagram. . . . . . . . . . . . . . . . . 2613 CPU and hard drive mounted in case. . . . . . . . . . . . 14914 Bottom half of chassis. . . . . . . . . . . . . . . . . . . 15115 Power amplifier board. . . . . . . . . . . . . . . . . . . . 15316 Outline of subsumption architecture. . . . . . . . . . . . 154

v

1 PURPOSE

1 Purpose

The intent of this project is to develop a low cost robotics research plat-form suitable for future research into autonomous control algorithms. Thisinitial development will also include a basic control algorithm to allow theplatform to traverse the engineering building in a random fashion, avoidingobstacles as it goes.The platform itself will be designed in such a way as to make later mod-ifications or additions simpler. This means modularity needs to be theprimary concern in the mechanical, electrical, and software designs.This platform needs to be developed in a low cost way as it is being fundedby the development team. This means that custom hardware will be usedwherever possible to attempt to keep the total project cost down.

1

2 OVERALL DESIGN DISCUSSION

2 Overall design discussion

Over the course of the previous term we tackled many hurdles while de-signing a robotics platform to meet the requirements we laid out during theinception phase of this project. The following report documents the finalsystem as it stands at the writing of this report. This section deals withsome of the larger issues that were faced during the course of development.

2.1 Mechanical design

As both members of the team are fairly inexperienced in developing andfabricating mechanical system a great deal of the development time wastaken up with learning the skills required to construct the physical plat-form. There were no major problems faced during the development of themechanical aspects, except for a few times when lack of experience slowedus down to almost a halt.The final platform is very close to the original design specified at the be-ginning of the term, with only a few minor tweaks made to fix some earlystage oversights (please refer to Appendix D for the original mechanicaldrawings).

2.2 Electrical design

The electrical design was much easier to handle then the mechanical design,as both members of the team have a great deal of experience developingelectrical systems. Please refer to Appendix C for the full set of schematicdrawings.The decision to use off-board satellite processors was made mainly due totime constraints. A better solution to the problem would have been to de-velop custom PC/104 modules to handle PWM generating and quadraturedecode. This design could have fairly easily be realized in a basic CPLD orFPGA device. The big factor that made us choose the satellite processorroute was the additional cost of having printed circuit boards made by amanufacturer who would be able to use plated through holes. With theroute that was followed the circuit board design was much simpler allowingfor the production of boards at home.

2.3 software design

Early on in the development of the software system it was realized thatthe real time performance of Linux would not allow the system to be asresponsive as we would have liked. Unfortunately due to time constraintswe were forced to continue on the path of using Linux as the operating

2

2.4 Overall system 3 CPU BOARD

system. Other then the non real-time nature of the Linux kernel there wereno real hurdles faced in the development of the systems software, and thefinal software met or exceeded all of the original design requirements.

2.4 Overall system

We can be proud to say that the system we had developed by the endof the term met or exceeded every requirement that was laid out at thebegging of the term. There is however, room to improve on the systemstill, but mostly these improvements are incremental, and will only serverto make the whole system more robust or more expandable.

3 CPU Board

The CPU board acts as the main control unit for the robotic platform andis responsible for control of all operational facets of the platform, as wellas logging test data. The board used for this project was a WinsystemsLBC-586 Plus single board computer. For cost reasons a laptop hard drivewas used to install the software as opposed to a disk on chip or flash device.Ultimately the flash device would be a better choice to make the platformmore immune to vibrations.

3.1 Operating System

3.1.1 Design System Conclusions

The operating system that has been installed on the PC/104 board is a cus-tom compiled version of Linux. This custom install is based on the GentooLinux distribution, which is a system that allows for complete recompilingof all kernel components. This ability to recompile all components allowsfor complete optimization for the target platform. This optimization isvery important in a real time system such as this platform, as it allows thecontrol system to respond to stimuli with minimal lag time.The Linux kernel being employed in this system is the 2.4.20 base kernel,with the POSIX message queue patch installed. This kernel was chosen asit is the first kernel branch to incorporate a preemptive scheduler, this willhelp the control algorithm to respond more quickly to external stimuli.

3.1.2 Future considerations

The preemptive Linux kernel chosen as the operating system turned out notto provide nearly enough real time performance to allow the whole system

3

3.2 Hardware 3 CPU BOARD

to respond quickly to real time events, due to this the system could notinteract with dynamic obstacles as quickly as would be liked. In the nextstep of development the software will be ported to an operating systemthat does afford real time performance, the leading candidate for this is theQnX operating system produced by QnX software systems ltd. The currentsoftware architecture has been designed to be 100% POSIX compliant sothe porting process should be fairly simple.

3.2 Hardware

3.2.1 Hardware design conclusions

The PC/104 board and attached hard drive have been mounted in a largealuminium case that will allow for :

• good electrical noise isolation.

• adequate protection of the electrical components.

• sufficient room for future expansion.

Figure 1 shows the processor board and hard drive mounted in the case.The power supply for the PC/104 board was mounted in a separate enclo-sure within the main box. This was to isolate the embedded PC from anyRF noise generated by the switching power supply, the final system hadabsolutely no problems with noise.

3.2.2 Future Considerations

There were no problems with the hardware mounting of the embedded PC.

3.3 Electrical

3.3.1 Electrical Design Conclusions

The power requirements for the embedded PC power supply were fairlystrict, 5V (±5%) it also needs to supply a current of 3 amps. This re-quirement on the power supply excludes using a linear supply as the powerdissipation would be too great:Assuming a worst case battery voltage of +14V.

P = IV

P = 4A · (14V − 5V )

P = 36W

4

4 REMOTE COMMUNICATIONS

Figure 1: CPU and hard drive mounted in case.

We would need to dissipate 36W of power in the liner regulator, so forthis application a high efficiency switching regulator was employed. Tosimplify the design, and keep the cost down, a Texas Instruments hybridswitcher module was chosen (model # PT6302C). This module requiredonly a few external components for operation, and the resulting outputripple and power fell well within the required tolerances. Please refer toAppendix C.1 for the complete schematic.

3.3.2 Future Considerations

There were no problems with the final power supply.

4 Remote Communications

To allow for remote software starting and debugging a standard 802.11bwireless access point was mounted on the platform. Through this link anoperator can start and stop the platform, rebuild or upload new software,or monitor real time data. The ability to monitor real time data is veryimportant during system design, as it is hard to tell exactly what is hap-pening at any point with just a visual inspection. The ability to start and

5

6 BATTERY

Figure 2: Emergency Stop button.

stop the system remotely also makes testing easier as one does not haveto wait for a complete re-boot to test a code change.

5 Safety Systems

The remote wireless link allows the operator to almost instantly shut downthe platform while it is running. But if this link is broken, or the operatorcannot respond quickly enough, a manual safety interlock, in the form of akill switch, is provided on the platform itself (Figure 2 shows this shutoff).This kill switch instantly cuts power to the motors and brings the platformto a halt within a few centimeters. It operates independently of any otherelectrical system, and will fail safe.

6 Battery

The battery used on the final platform is a sealed lead acid type batterywhich supplies 12V with a capacity of 18AH. Based on this storage capac-ity the battery should allow for approximately 5 hours of operation timebetween charges. With a good quality battery charger full capacity shouldbe reached within 3 to 4 hours. Initially a less expensive car led acid batterywas employed, but this posed a safety risk as it emitted gas and tended toleak while charging. This leaking and emitting of gas is unacceptable in aninside environment. Figure 3 shows the battery mounted on the platform.

6

8 SENSOR NETWORK

Figure 3: Battery mounted on the platform.

7 Bus Wiring

The platforms bus wiring was achieved using a terminal block as shownin Figure 4. This allows for ease of rewiring and future expansion. Thisterminal block also contains the master fuse for the system, as the batteryis capable of delivering several hundred amps in short circuit conditionsthis master fuse prevents damaging any components in the case of a shortcircuit.

8 Sensor Network

8.1 Sonar

8.1.1 Sonar sensor conclusion

The sonar sensors that are being used on the platform are polaroid rangingtype sonar sensors. These sensing modules consists of an instrument gradesonar transducer and a processor board and are able to determine theapproximate distance to an object within a 15o arc extending from thecenter of the transducer at a range between 6 inches and 35 feet. Thesesonar sensors will be mounted in the front of the unit in such a way asto slightly overlap the main lobes of sensitivity, this should result in sonar

7

8.1 Sonar 8 SENSOR NETWORK

Figure 4: Bus wiring terminal block.

coverage extending out in a 30o arc from the front of the platform. Carewas needed to assure that a late return from one modules transmit cyclewill not look like a target to the second module. These modules havealready been purchased and are ready for mounting.The initial design for the sonar Power supply was simply a linear regulatorwith a large output capacitor to dampen the voltage drop when the sonartransmits. The initial design simply passed the digital signals from theembedded PC board, unfortunately the IO pins on the embedded PC boardcould only drive 500µA each, so a small daughter board needed to beconstructed to act as a buffer to the signal between the board and the sonarmodules. Once installed the power supply worked as expected. Figure5 shows the sonar modules mounted on the front of the housing. Themounting of the sonar transducers allows for the whole module to be tiltedup and down to find the optimal positioning. Based on testing the sonarmodules performed best when positioned facing slightly downward at anangle of approximately 5o.To simplify the design of the sonar software, it was decided that the driverwould run in user space, this means that it was not possible to use interruptsto detect the return signal, instead the driver simply polled the echo pin.Unfortunately some granularity was lost as the polling loop needed tocontinually check the system time, which was a slow system call, ultimatelythe system could judge the distance to an object within about a ±20cmrange.

8

8.2 Infrared 8 SENSOR NETWORK

Figure 5: Sonar transmitters mounted on front of chassis

8.1.2 Sonar sensor future considerations

While the polled software performed adequately in this application, oncethe code has been ported to its new operating system, the sonar driverwill be moved over to an interrupt driven kernel space driver. This shouldultimately produce a cm accurate measurement.The next step in the sonar system is to increase the number of sensors,it should be possible to completely surround the platform with 24 sensors,though 12 would most likely provide adequate coverage for obstacle detec-tion. Since it is not possible to transmit from multiple sensors at the sametime due to interference effects, it should be possible to multiplex manytransducers onto a single control board to keep the cost down.

8.2 Infrared

8.2.1 Infrared sensor conclusion

The infrared ranging modules that are being employed on this platform areSharp GP2D02 type sensors. These sensors are able to detect an object inthe range of 10cm to approximately 80cm.These modules are mounted on the periphery of the sonar modules to de-tect obstacles coming up at an angle to the platform. As well, they are

9


Figure 6: Infrared modules mounted on side of platform.

mounted on each side of the platform to allow for wall following, and hall-way traversal, this will be possible by keeping the distance signal on eachside of the platform the same.These infrared modules produce a non linear analog voltage that is con-verted to a digital level by one of the satellite processors. The PIC micro-controller is equipped with a 10 bit analog to digital module, the resultsproduced by these modules are down converted to an 8 bit value, and av-eraged over 256 readings to help eliminate any noise on the signal. Theresulting values are then stored on the satellite processor until requestedby the main CPU through the serial link.Once the AD module reading is passed to the embedded PC it needs to beconverted into a distance, Figure 7 shows the set of experimental data rep-resenting the distance voltage pairs. From this a fourth order polynomialapproximation was calculated and implemented to allow both linearizationand conversion to a range in a single step (Equation (1) represents thislinearization function). The final result was a sensor that was accurate towithin 1

2cm within 20cm and slowly lost resolution as the distance moved

out towards the maximum range of 70cm.

10


Figure 7: Infrared linearization function.

11

9 MECHANICAL DESIGN

x =

(5

255

)· VAD

Distance = 17.723 · x4 − 117.92 · x3 + 290.43 · x2 − 326.34 · x + 162.52(1)

8.2.2 Infrared sensor future considerations

The infrared modules functioned very well and do not require any futureconsiderations.

9 Mechanical Design

9.1 Drive train

There were some major hurdles that needed to be overcome in the designof the drive train, the biggest of these was finding a set of wheels thatcould be driven. Most inexpensive wheels come pre-assembled with a setof bearings that would simply allow them to turn freely on the shaft. Ourultimate solution to this problem was to purchase such a set of wheels,punch out the bearings, and turn a new set of bushings. These new bush-ings were pushed into the holes left by the bearings, and set screws wereused to attach them to the drive shafts.The motors employed in this design were Matsushita DC motors with in-tegrated optical encoders. These motors were able to deliver 100 in/lbof torque, and operated at a speed of 100 rpm. This speed proved to bealmost perfect in this application, as the resulting system travelled a nearperfect speed.The drive shaft was linked to the motors through a chain drive employingtwo sprockets on a 1:1 ration. This chain drive helped isolate the motorshafts from the drive shafts. in the case of an accident the less expensivedrive shaft would be damaged and not the much more expensive motorshaft. Figure 8 shows one half of the drive train.

9.2 Chassis

The final chassis performed exactly as expected. It provides a very stablebase to attach all the components to. The platform is designed so that itwill rock back onto the rear trundle wheel, this is done so that it can easilypass between floor surfaces of different heights. Figure 14 shows the finalchassis configuration with all the components attached.

12

9.2 Chassis 9 MECHANICAL DESIGN

Figure 8: Drive train, side shot.

Figure 9: Final fully assembled chassis.

13

9.3 Mechanical design future considerations10 SATELLITE PROCESSORS

9.3 Mechanical design future considerations

The chassis and drive chain provide a very robust and stable platform fortesting. At the time of this report there have been no mechanical problemswith the platform.

10 Satellite Processors

A decision was made early on to reduce the load on the PC/104 board asmuch as possible. This was partly due to the fact that the performance ofthe PC/104 was unknown as well as the capabilities of the Gentoo Linux itwas running. Because Gentoo is not a real-time operating system, any timecritical applications were better suited to independent, off-board, satelliteprocessors.

An early analysis of robot functions revealed that the pulse width mod-ulation (PWM) to power the motors, the time capture for the quadratureencoders, and the analogue to digital conversion (ADC) of the infraredsensors were all good candidates to be moved off-board.

Because of their low cost, good selection of hardware peripherals, andexisting programming and debugging tools, the Microchip 16F876 PIC mi-crocontroller was chosen to be the processor for these external function.

10.1 Serial Protocol

The first issue in moving functions to an external processor is the inabilityof the host or master processor to have direct control and feedback ofthe remote functions. A robust serial protocol was designed for this inner-processor communication.

Packet Format:

• All control sequences start with 0x10

– 0x01 ⇒ start of message

– 0x04 ⇒ end of message

14

10.1 Serial Protocol 10 SATELLITE PROCESSORS

Comm ParametersParameter ValueProtocol RS-232 (maybe 422/485)Speed 57600 bpsData bits 8Parity bit NoneStop bit 1

Table 1: Serial Protocol Parameters

0x10 0x01 Rx ADD Tx ADD CHK CMD Size Data 0x10 0x04

Table 2: Serial Protocol Packet Format

– 0x10 ⇒ byte stuffing for data value of 0x10

– Byte stuffing will be used when 0x10 appears in other non-command data. Ie: data = 0x40 0x55 0x10 0x10 0x36 0x43(repeated 0x10 not included in Size or CHK parameters)

• Rx ADD = receiving address

• Tx ADD = transmitting address

• CHK = checksum = XOR of all message bytesEXCLUDING:

– control sequences (ie: 0x10 0x01)

– CHK itself

– second byte-stuffed 0x10

• CMD = command (device dependant)

• Size = # of data bytes

• Data = message data

Device ListAddress Device0x20 PWM Controller0x30 Sonar Sensors

Table 3: Satellite Device Addresses

15

10.1 Serial Protocol 10 SATELLITE PROCESSORS

Device: 0x20 - PWM ControllerCommand Description Data0x00 PWM Off none0x01 PWM On 4 bytes: dir0, speed0, dir1, speed10x02 Read Status Flags none0x03 Set E-Brake 1 byte: 0=0ff, 1=on

Table 4: PWM Command Structure

Device: 0x30 - Analogue ControllerCommand Description Data0x00 Read Quadrature none0x01 Read A/D none

Table 5: A/D and Quadrature Command Structure

With no hardware or software flow control, it is wise to have precau-tions to ignore garbage data on the serial port. All serial transmissionsbegin and end with special control codes in order to filter out garbage. Allcontrol codes are start with 0x10.

Source and destination address fields are not really necessary for RS-232 communications, which is a point-to-point protocol. However, thePC/104 board has the ability to use RS-485 communications, which isa multi drop bus allowing many receivers and transmitters thus requiringsome addressing. Should this feature be desired in the future, the serialprotocol wont have to change.

Each device has a specific command set as shown in Tables 4 and 5and is transmitted via a one-byte command, which can be followed by 0or more data bytes. Finally, a simple checksum is calculated to detecttransmission errors.

The only trick to this protocol is with the control byte - 0x10. Shouldthis value appear anywhere else in the data packet, the receiving modulewill incorrectly interpret it as a control message. Therefore, an additionalcontrol message is required and a technique called byte-stuffing is used.When a value of 0x10 is anywhere but the start and end of the message,it is repeated. Thus 0x10 is sent twice consecutively.

16

10.2 PWM 10 SATELLITE PROCESSORS

10.2 PWM

The first remote module to be designed was the motor controller. Themotors, which were connected to the H-bridge PCB, had several controlsignals:

• PWM (input)

• Direction (input)

• Brake (input)

• ThermalShutdown (output)

• Current Load (output)

The PIC 16F876 has a built-in PWM module, which made implementa-tion easy. To put the signal of the PWM out of audible range, a frequencyof 20 kHz or higher was desired. However with with the sonar modulesrunning at 40 kHz, care was taken not to have any harmonic frequenciesthat may affect the performance, thus the PWM module was programmedto run at 19.53 kHz.

The direction pin on the motor controls the forward and reverse mo-tion of the motors. The robot was not designed to run backwards, and infact does not need to due to its design. However because the motors aresituated in opposite directions, one runs forward and one runs backwardall the time.

The brake signal is not normally needed because setting the PWMvalues to 0 stops the motors within 15-20 degrees of revolution anyway.However, should an emergency situation arise, enabling the brake signalstops the motors almost instantly. This is hard on the power train andshould be avoided.

The thermal shutdown signal is simply a flag to indicate if the motorshave shut themselves off because they were overloaded. Related to that,the current load signal is an analogue current representing the load of themotors. If so desired, this can be monitored with an A/D converter toobserve the current draw.

10.3 Analogue and Digital

The second remote module to be designed was the analogue-to-digital cap-ture and quadrature encoder capture controller. This circuit was to have

17

10.4 Hardware 10 SATELLITE PROCESSORS

dual functionality: to do an A/D conversion on the four infrared sensorsand to do a timing count from the motor quadrature encoders.

The infrared LEDs were biased to 5V and produced a maximum 2.5Vat close range. The 16F876 PIC has five ADC channels, which workedperfectly since the hardware had four infrared LEDs. (The 5th channel wasalso captured but not used. It will eventually be used to monitor batteryvoltage.)

The motors each have two quadrature signals for position decoding.Normally both quadrature signals are used to determine speed and direc-tion. However since the robot can only move forward, it was only necessaryto monitor one of the signals on each motor. The slight loss incurred inprecision by only reading one signal was negligible compared to the play inthe chains and power train.

10.4 Hardware

Unfortunately the 16F876 PIC uses the same pins for the PWM module asfor the capture compare module needed for the quadrature decoding. Someclever programming could have bypassed some of the hardware peripheralsand fit all three of the above modules onto a single PIC, but because of thelimited time for the software implementation, it was decided two separatePICs should be used.

Several custom PCBs were designed and created by the team membersincluding a simple one for the two satellite processors. The daughter PCBconnected to the PC/104 board via a 20-pin ribbon cable. A Maxim 232transceiver buffer converted the ±15V RS232 levels to TTL levels. 20MHz crystals were used for the two PIC oscillators. The rest of the com-ponents were various resistors, capacitors, and pin headers.

10.5 Software

Because of the powerful peripherals built in to the 16F876, the softwarewas rather easy to implement. The special serial communication routinewas common to both PICs and worked in an interrupt mode. Reception ofdata was automatic and the main loop continually polled the serial moduleto check for a completed message packet. When the closing commandwas received (0x10 0x04), the address was checked and compared versusthe address of the receiving device and if that matched the checksum was

18

10.5 Software 10 SATELLITE PROCESSORS

calculated. Only then would the reception flag be set, allowing the codeto handle the message and react accordingly.

The software for the PWM controller simply sets the PWM peripheralwithin the 16F876 according to the following calculations:

PWM ParametersFreqosc 20MHz

Periodosc 50ns

Timer Prescaler 1PR2 register 255

Table 6: PWM Peripheral Settings

PWMfreq =1

[(PR2) + 1] ∗ 4 ∗ Fosc ∗ (Prescale)= 19.53kHz (2)

Equation 2 shows the PWM frequency of 19.53kHz. This value can beused to determine how many resolution bits the PWM peripheral can use.10 bits is the maximum, which provides the finest scale for precision mea-surements.

PrecisionBits =log( Freqosc

Freqpwm)

log(2)= 10bits (3)

According to equation 3, the chosen configuration of the PWM pe-ripheral will allow 10 bits of resolution for duty cycle. It was determinedthat the robot only required 8 bits of resolution. This simplified the serialprotocol as the PWM value could be transmitted in one byte.

With the frequency set to 19.53kHz, the 8-bit value, as sent from thehost via the serial link, determined the duty cycle. A value of 0 meant 0%duty, which turned the motors fully off. A value of 255 meant 100% duty,which turned the motors fully on. Any value in between changed the speedlinearly.

The software for the A/D conversion had fewer parameters.There are essentially two timings of importance when using the ADC mod-ule: Acquisition time and A/D Conversion time. The acquisition time is

19

11 CONTROL ALGORITHMS

A/D ParametersFreqosc 20MHz

Periodosc 50ns

A/D Clock Source 32 ∗ Periodosc

TAD 1.6µs

Table 7: A/D Peripheral Settings

difficult to quantify precisely because it is based on various environmen-tal factors such as temperature and capacitor charge time. A good ruleof thumb is to give 20µs for the acquisition. This is important to knowbecause this restriction must be met using a timer or other delay routine.There is no automatic way to know when the acquisition is finished.

Once the acquisition is complete, the conversion process needs 12∗TAD

or 19.2µs. Fortunately there is a done flag that could be polled and sig-naled when the digital value was ready to be read. Each of the five ADCports was operated sequentially in this manor. After 256 samples were readand stored, they were averaged, making a simple and effective digital lowpass filter. This greatly reduced noise inherent in the LED sensors fromambient light.

The capture peripheral for the quadrature encoders had no real parame-ters to speak of. Once set, an interrupt fired when the capture pin detecteda rising edge. The interrupt routine kept track of the internal clock timervalues at each interrupt and subtracted them to get the quadrature period.When the motors were stopped or even moving extremely slowly, the timerreadings would not be accurate. If the timer overflowed the code couldmisinterpret that as the motors moving when in fact they were not. Somelogic was added to filter this garbage data and simply set the values to 0.

11 Control algorithms

11.1 Software Framework

The two main goals while developing the software framework were respon-siveness and modularity. Figure 10 shows the overall software structure.To achieve the modularity requirements the software structure was brokenup into components, and each of these components was implemented asa process. The interprocess communications was achieved using System

20

11.2 Inter-process message structure 11 CONTROL ALGORITHMS

V message queues. In this way each module is completely independent ofall other modules, and can be completely replaced without affecting anyother module. As an added feature any process can be stopped and re-placed dynamically without affecting the rest of the system. This dynamicswapping could be used to trade out the control system without shuttingdown the system, or shutting down a deadlocked process and bringing itback up without affecting the operation of the system.The division of the system into processes also allows for better real timeperformance, as important tasks can be run at a higher priority then therest of the system.Unfortunately as mentioned previously the real time performance of thekernel was not adequate to allow the system to respond as quickly as itmay otherwise be able to. The system V message queues are not thefastest way to implement the interprocess communications, but they pro-vide a very robust communications system. For better performance theQnX send, receive, reply architecture could be used, or depending on theimplementation POSIX message queues may provide better performance.The actual subsumption controller was simply implemented as a processwhich gathers information from all the sensor processes, determines a newmotor value and, then writes this value out the motor process.For static obstacle avoidance the system performed quite well with an up-date rate of approximately 100mS. Many tricks needed to be played toachieve this 100mS update rate, as by default the time slice size underthe Linux kernel is approximately 120mS. Once ported to a true real timeoperating system however the whole control system should become muchmore responsive and able to handle dynamic obstacles.The system start-up and shutdown sequencing needed to be handled by theserver to assure the correct order of program termination. For example themotor control process had to properly shut down, and turn the motors offbefore the serial processes shut down and cut the communications channelto the motor control satellite processors. The shutdown sequencing wasall achieved using specially formatted messages sent by the server process,and acknowledgments were returned using the same mechanism.

11.2 Inter-process message structure

Since the whole system has been decomposed into separate processes,some method for communicating between these processes is required. Forthis, as mentioned above, system V message queues were used. Someadditional advantages can be realized when message queues are used. Forexample message queues provide a simple method for serializing access tothe satellite processors. Since multiple processes may be trying to talk tothe same piece of hardware through a single serial line we need to insure

21

11.2 Inter-process message structure 11 CONTROL ALGORITHMS

Figure 10: Overall software architecture.

22

11.3 Rules framework 11 CONTROL ALGORITHMS

only one is actually talking at a time. We can achieve this by writing asingle process responsible for talking with the hardware and connect allprocesses that need to access the hardware to it through a message queue.Since the single hardware access process can only handle a single messageat a time, serialization is achieved with no additional code required.

11.3 Rules framework

The behavioral rules were implemented as a modified subsumption archi-tecture, modified in that the rules were applied in a linear fashion. Thislinearization of the rules allows for easier testing and debugging of the indi-vidual behaviors as well it simplifies the implementation. Figure 11 showsthe basic structure of the control system.

The subsumption approach to implementing a behavioral system allowsus to develop a pattern of behavior abstracted from the environment weare intending to interact with. In this way we can generate a more genericbehavioral algorithm, while avoiding developing a expert system, which isincapable of dealing with un-programmed stimuli. The subsumption ap-proach also allows for easier algorithm development and coding, as eachbehavior is discrete, and therefor can be implemented and tested indepen-dently of any other module.

Subsumption, an analogy:Our daily behaviors can be modelled using the subsumption architecture,take for example walking to the store to buy milk. When we decide to walkto the store, what is the first thing we do (assuming we have our wallet,keys, shoes etc.) we walk out the door, and begin to walk down the street.This is modelling our base behavior, and we can call this the walk behavior.As were walking along we run into a friend we havent seen in a while, wecan wave , or stop and say hello, either way, this can be modelled intoour system by adding a new behavior at a higher priority then walk, wellcall this greet. Now greet doesnt have to know what walk does, we dontcare how we were walking when we say hello, we just care that we havestopped walking and our now talking with our friend. To review our simplesystem now we have our base behavior of walk, which we will act on until ahigher priority behavior wishes to take over, in this example, greet. So wewill continue to walk to the store until we see someone we know, we willthen stop and greet them, once we are finished talking to them, we willcontinue on our way. Just to make things a little more interesting, lets saythe store is on the other side of the street and we have to cross to reach it.Being safety mined individuals (and this close to finals) we want to checkboth ways before we cross, if there are no cars coming its safe to cross,

23

11.3 Rules framework 11 CONTROL ALGORITHMS

Figure 11: Subsumption rules architecture.

but if there are cars coming well wait for them to pass. Well call this newbehavior Cross, and assign it a priority higher than either Walk or Greet.If the Cross behavior was at a lower priority then Greet we could run intoa problem, if we were to see a friend as we cross the street, according toour model, we would have to stop in the middle of the street to say hello(a very dangerous proposition). This is due to the fact the highest prior-ity behavior always has full control of the system, suppressing any lowerpriority behaviors. This simple subsumption model could be implementeddirectly in hardware, with each of the behaviors being triggered by stimuliproduced by one or a combination of sensors, yielding a system capable ofmaking a trip to the local corner store.

Next we will take a closer look at each of the different behaviors.

11.3.1 Avoid behavior

The avoid behavior runs at the highest priority and can suppress any otherbehavior.The avoid behavior is responsible for preventing head on impacts by mon-itoring the sonar range. Once the behavior is activated, the current turnstatus of the platform is determined. If the platform is already turning thisbehavior causes the platform to continue to turn in the same direction, justat a faster rate (to a hard limit). If the platform is not already turning,a decision needed to be made which way to go. To do this the side and

24

11.4 PI control loop 11 CONTROL ALGORITHMS

corner infra red sensors are checked, and the system turns in the directionthat provides the clearest path. Finally if no information can be garneredfrom the infrared sensors, the turn direction is determined in a completelyrandom way.

11.3.2 Veer behavior

The veer behavior monitors the two side infrared sensors, and are respon-sible for covering the blind spot between the sonar on the front of theplatform and the infrared sensors on the sides. Once activated this be-havior causes the platform to make a slow turn away from the obstacledetected.

11.3.3 Side avoid behavior

The side avoid behavior is responsible for handling objects that quicklyapproach from the side of the platform. It is activated when one of the twoside sensors detects an obstacle within an predefined range. Once activatedthis behavior steers the platform away from the obstacle detected. This issimply done by reducing the speed of the wheel opposite to the side thatthe object was detected.

11.3.4 Skim behavior

The skim behavior attempts to keep the distance to any obstacle on eachside of the platform equal (provided these distances do not become lessthen the threshold required to trigger the side avoid behavior). In this waythe platform can follow straight down the middle of a hallway by slightlyweaving back and forth, attempting to keep both walls the same distanceaway.

11.3.5 Run behavior

The run behavior is the lowest priority behavior, and is only active when nosensors can see any targets. When active this behavior attempts to drivethe platform in a completely straight line using a simple PI control loop.

11.4 PI control loop

To maintain equal wheel speeds a PI loop was implemented in the waydescribed in the following section. Due to process time constraints thiscontrol loop was removed from the demonstrated system, this did not

25

12 PROJECT BUDGET

Figure 12: PI controller block diagram.

cause a problem as the subsumption controller closed the control loopquite well, and overall the system performed well without it.The PI loop uses both proportional and Integral information gathered bythe quadrature decoder on one of the satellite processors. Figure 12 showsthe block diagram for the PI loop implemented in this system. A PI loopwas chosen as opposed to a PID controller as locking the two motorsin both velocity and acceleration more then suffices for this application.Locking position as well would bring no great advantage to the system.The work on optimizing the control system was done through trial anderror adjustment of the proportional and integral constants, as no realinformation on the transfer function of the motors used was available.

11.4.1 Limitations of the control system

This subsumption architecture makes for a very robust control system inmany environments, but in its current state can break down in less for-giving environments. Due to the poor real time behavior of the operatingsystem each behavior may take several hundred milliseconds to respond,and several hundred more to become fully active (the motors implement asoft speed change, to prevent stressing the mechanical components).In the future many more behaviors will be added to cover many more spe-cific cases, and hopefully allow the platform to better handle less academicenvironments.

12 Project budget

At the beginning of the term the project budget was set between $1000 and$1500 cdn to develop the entire system. The following section shows thecost breakdown of the final product. Ultimately the project came in just

26

13 FUTURE ENHANCEMENTS

under budget, with a large number of features not present in the originalspecification.

Item Units Unit Cost Extended Cost

Assorted Digikey components 1 130.00 130.00Wheels 2 19.99 39.98Assorted fasteners 1 13.60 13.60Angle aluminium for chassis 1 52.56 52.56Flat aluminium for chassis 1 14.76 14.76Angle aluminum for motor mounts 1 13.77 13.77Assorted aluminium stock 1 24.67 24.67Assorted aluminium stock 1 53.28 53.28Motors 2 97.42 194.85Assorted drive train components 1 83.28 83.28Case for CPU 1 30.70 30.70Memory for CPU 1 67.56 67.56PC/104 board 1 75.00 75.00Battery 1 95.97 95.97Chain for drive 1 28.30 28.30Assorted electronics components (Quale electronics) 1 182.81 182.81Sonar Modules 2 0.00 0.00IR modules 4 20.87 83.49Wireless router 1 75.14 75.14Wireless NIC card 1 114.00 114.00Misc 1 115.05 115.05TOTAL COST 1488.77

13 Future enhancements

The following is a list of upgrades planned for the near future:

• Port the software over to QnX to achieve better real time perfor-mance.

• Move the satellite processor function on to custom made PC/104bus cards.

– Move quadrature capture and PWM generation onto new mul-tifunction programmable board.

– Move A/D conversion functions onto custom mixed signal board.

– produce a USB pc/104 card to allow for connection of camerasfor image processing.

27

14 SPECIAL THANKS AND FINAL WORDS

• Add more sonar sensors around the periphery, the infrared rangershave a very good granularity, but have a very narrow beam angle.Because of this they miss thin targets such as table legs etc.

• Build more robustness into the control system, this will allow the sys-tem to recover from catastrophic software failures without affectingits operation. This can be done by implementing ia process watchdogthat replaces a process who fails to check in at regular intervals.

14 Special thanks and final words

We would like to thank Dr. Manolis Christodoulou for agreeing to be oursupervisor at the 11th hour, without his help this project would never havehappened.We would also like to thank the staff of the physics machine shop for pro-viding us access to the tools we needed to complete all the mechanicaltasks required to finish this project. Its nice to know that there is some-where the students can gain access to a machine shop on campus.I think we both agree that working on this project was a lot of fun, andwe had the opportunity to learn and experiment with a lot of things wewould never otherwise get the chance to play with. For this reason, anyonereading this report who is considering taking a 499 class, I can honestlysay it is well worth it, it will be one of the best experiences you will havethe opportunities to have during your undergrad studies.

28

A Embedded PC Code

A.1 Makefile

#make f i l e f o r un i t 00 autonomous r o b o t i c s p r o j e c t

CC=gccJC=j a v a cLD=l dAR=arNM=nmMAKE=make

LIBRARY PATH = −L\ l i b −L \ .

INCLUDE PATH = − I \ u s r \ i n c l u d e − I \ .

CFLAGS=−O1

PROCESSES = robo t motorProces s s e r i a l R e a d s e r i a l W r i t es ona rP r o c e s s i r P r o c e s s subCon t r o l

MODULES ROBOT = robot . o pc . oMODULES MOTORPROCESS = motorProces s . o moto rP ICont ro l . oMODULES SERIALREAD = s e r i a l R e a d . o s e r i a l C o n t r o l . oMODULES SERIALWRITE = s e r i a l W r i t e . o s e r i a l C o n t r o l . oMODULES SONARPROCESS = sona rP r o c e s s . oMODULES IRPROCESS = i r P r o c e s s . oMODULES SUBCONTROL = subCon t r o l . o

a l l : $ (PROCESSES)@echo ”BUILD COMPLETE ,ROCK ! ! ! ! ”

robo t : $ (MODULES ROBOT)$ (CC) $ (CFLAGS) $ (LIBRARY PATH) $ (INCLUDE PATH) $ (

MODULES ROBOT) −o robo t

s e r i a l R e a d : $ (MODULES SERIALREAD)$ (CC) $ (CFLAGS) $ (LIBRARY PATH) $ (INCLUDE PATH) $ (

MODULES SERIALREAD) −o s e r i a l R e a d

s e r i a l W r i t e : $ (MODULES SERIALWRITE)$ (CC) $ (CFLAGS) $ (LIBRARY PATH) $ (INCLUDE PATH) $ (

MODULES SERIALWRITE) −o s e r i a l W r i t e

29

motorProces s : $ (MODULES MOTORPROCESS)$ (CC) $ (CFLAGS) $ (LIBRARY PATH) $ (INCLUDE PATH) $ (

MODULES MOTORPROCESS) −o motorProces s

s ona rP r o c e s s : $ (MODULES SONARPROCESS)$ (CC) $ (CFLAGS) $ (LIBRARY PATH) $ (INCLUDE PATH) $ (

MODULES SONARPROCESS) −o s ona rP r o c e s s

i r P r o c e s s : $ (MODULES IRPROCESS)$ (CC) $ (CFLAGS) $ (LIBRARY PATH) $ (INCLUDE PATH) $ (

MODULES IRPROCESS) −o i r P r o c e s s

subCon t r o l : $ (MODULES SUBCONTROL)$ (CC) $ (CFLAGS) $ (LIBRARY PATH) $ (INCLUDE PATH) $ (

MODULES SUBCONTROL) −o subCon t r o l

%.o :%. c

$ (CC) $ (LIBRARY PATH) $ (INCLUDE PATH) −c $ (CFLAGS)−o $@ $<

c l e a n :rm − f $ (PROCESSES)rm − f ∗ . orm − f ∗˜

i n s t a l l :

cp $ (PROCESSES) . . / b i n

30

A.2 Common Library

//#d e f i n e TRUE ( 1 )//#d e f i n e FALSE ( 0 )

#i fnde f SUCCESS#d e f i n e SUCCESS 1

#end i f

//motor speed c on s t a n t s#def ine MAX SPEED 0xDF

//message t yp e s :#def ine UPDATE MOTOR 1#def ine SERIAL WRITE 2#def ine IR UPDATE 3#def ine SONAR UPDATE 4#def ine IR REQUEST 5#def ine IR RESPONSE 6#def ine QUAD REQUEST 7#def ine QUAD RESPONSE 8#def ine UPDATE MOTOR 1#def ine SERIAL WRITE 2#def ine IR UPDATE 3#def ine SONAR UPDATE 4

#def ine KILL 9#def ine KILL MOTOR 66#def ine KILL MOTOR ACK 67

typedef enum {FALSE=0 , TRUE} boo l ;

typedef s t ruc t {long mtype ;i n t a r g s ;

} k i l l m e s s a g e b u f f ;

#i fnde f FORWARD#de f i n e FORWARD 0 x01

#end i f

#i f nde f REVERSE#d e f i n e REVERSE 0 x00

#end i f

31

#i f n d e f BREAK ON#de f i n e BREAK ON 1

#end i f

#i f nde f BREAK OFF#d e f i n e BREAK OFF 0

#end i f

typedef s t ruc t {long mtype ;i n t motorSpeed ;i n t b i a s ;i n t l e f t D i r e c t i o n ;i n t r i g h t D i r e c t i o n ;i n t b r e akCond i t i o n ;

} moto r mes sage bu f f ;

typedef s t ruc t {long mtype ;char s e r i a l s t r i n g [ 1 3 ] ;i n t s i z e ;i n t po r t ;

} s e r i a lW r i t e m e s s a g e b u f f ;

typedef s t ruc t {long mtype ;unsigned i n t i r 0 ;unsigned i n t i r 1 ;unsigned i n t i r 2 ;unsigned i n t i r 3 ;

} i r u p d a t e me s s a g e b u f f ;

typedef s t ruc t {long mtype ;f l o a t i r 0 ;f l o a t i r 1 ;f l o a t i r 2 ;f l o a t i r 3 ;

} i r v a l u e m e s s a g e b u f f ;

typedef s t ruc t {long mtype ;f l o a t s o n a r r a ng e ;

} s o n a r upda t e me s s a g e bu f f ;

typedef s t ruc t {long mtype ;

32

} r e a d r e q u e s t m e s s a g e b u f f ;

typedef s t ruc t {long mtype ;long l e f t q u a d v a l u e ;long r i g h t q u a d v a l u e ;

} quad mes sage bu f f ;

typedef s t ruc t {boo l a c t i v e ;i n t motor speed ;i n t b i a s ;

} sub packe t ;

33

A.3 Port Definitions

#define DEVICE_MAJOR 50#define DEVICE_NAME "PIO Port"

#define PARALLEL_NUM_PORTS 6

/* definitions for io port base addresses* and related registers** Note :* These definitions are valid across all pages */

#define IOBASE_ADD 0x120#define IO_BASE_ADD IOBASE_ADD

#define IO_PORT0_OFFSET 0x00#define IO_PORT1_OFFSET 0x01#define IO_PORT2_OFFSET 0x02#define IO_PORT3_OFFSET 0x03#define IO_PORT4_OFFSET 0x04#define IO_PORT5_OFFSET 0x05

#define INT_PENDING_OFFSET 0x06#define PAGE_LOCK_OFFSET 0x07

#define IO_PORT0 IOBASE_ADD + IO_PORT0_OFFSET#define IO_PORT1 IOBASE_ADD + IO_PORT1_OFFSET#define IO_PORT2 IOBASE_ADD + IO_PORT2_OFFSET#define IO_PORT3 IOBASE_ADD + IO_PORT3_OFFSET#define IO_PORT4 IOBASE_ADD + IO_PORT4_OFFSET#define IO_PORT5 IOBASE_ADD + IO_PORT5_OFFSET

#define INT_PENDING IOBASE_ADD + INT_PENDING_OFFSET#define PAGE_LOCK IOBASE_ADD + PAGE_LOCK_OFFSET

int portAddress[] = { IO_PORT0, IO_PORT1, IO_PORT2, IO_PORT3, IO_PORT4, IO_PORT5};

/* page specific offset values , the register affected will depend* entirely on the current page, great care must be taken not to* read or write to / from the wrong page register */

34

// interupt polarity select, only valid on page 1#define POL0_OFFSET 0x08#define POL1_OFFSET 0x09#define POL2_OFFSET 0x0A

#define POL0 IOBASE_ADD + POL0_OFFSET#define POL1 IOBASE_ADD + POL1_OFFSET#define POL2 IOBASE_ADD + POL2_OFFSET

int polAddress[] = {POL0, POL1, POL2};

// enable edge detect interupt valid on page 2#define ENAB0_OFFSET 0x08#define ENAB1_OFFSET 0x09#define ENAB2_OFFSET 0x0A

#define ENAB0 IOBASE_ADD + ENAB0_OFFSET#define ENAB1 IOBASE_ADD + ENAB1_OFFSET#define ENAB2 IOBASE_ADD + ENAB2_OFFSET

int enabAddress[] = {ENAB0, ENAB1, ENAB2};

// Indicates currently avalable edge interupts, page 3 only#define INT_ID0_OFFSET 0x08#define INT_ID1_OFFSET 0x09#define INT_ID2_OFFSET 0x0A

#define INT_ID0 IOBASE_ADD + INT_ID0_OFFSET#define INT_ID1 IOBASE_ADD + INT_ID1_OFFSET#define INT_ID2 IOBASE_ADD + INT_ID2_OFFSET

int intAddress[] = {INT_ID0, INT_ID1, INT_ID2};

/* PAGE LOCK bit specific masks */

#define PAGE0 0x00#define PAGE1 0x40#define PAGE2 0x80#define PAGE3 0xC0

#define LOCK_PORT0 0x01#define LOCK_PORT1 0x02#define LOCK_PORT2 0x04#define LOCK_PORT3 0x08#define LOCK_PORT4 0x10#define LOCK_PORT5 0x20

35

/* marcro definitions :* the following block contains all the required macros for the* driver*/

#define TYPE(dev) (MINOR(dev) >> 4)#define NUM(dev) (MINOR(dev) & 0x0f)

/** port specific commands.** all work will be done through ioperm calls (thanks Darryl for making me think in this* kludgy hacky way** but it simplifies ioport access by a huge amount*/

/* read write port commands */#define W_PORT0 0#define W_PORT1 1#define W_PORT2 2#define W_PORT3 3#define W_PORT4 4#define W_PORT5 5

#define R_PORT0 10#define R_PORT1 11#define R_PORT2 12#define R_PORT3 13#define R_PORT4 14#define R_PORT5 15

#def ine DEVICE MAJOR 50#def ine DEVICE NAME ”PIO Port ”

#def ine PARALLEL NUM PORTS 6

36

/∗ d e f i n i t i o n s f o r i o po r t base a d d r e s s e s∗ and r e l a t e d r e g i s t e r s∗∗ Note :∗ These d e f i n i t i o n s a r e v a l i d a c r o s s a l l pages ∗/

#def ine IOBASE ADD 0x120#def ine IO BASE ADD IOBASE ADD

#def ine IO PORT0 OFFSET 0x00#def ine IO PORT1 OFFSET 0x01#def ine IO PORT2 OFFSET 0x02#def ine IO PORT3 OFFSET 0x03#def ine IO PORT4 OFFSET 0x04#def ine IO PORT5 OFFSET 0x05

#def ine INT PENDING OFFSET 0x06#def ine PAGE LOCK OFFSET 0x07

#def ine IO PORT0 IOBASE ADD + IO PORT0 OFFSET#def ine IO PORT1 IOBASE ADD + IO PORT1 OFFSET#def ine IO PORT2 IOBASE ADD + IO PORT2 OFFSET#def ine IO PORT3 IOBASE ADD + IO PORT3 OFFSET#def ine IO PORT4 IOBASE ADD + IO PORT4 OFFSET#def ine IO PORT5 IOBASE ADD + IO PORT5 OFFSET

#def ine INT PENDING IOBASE ADD + INT PENDING OFFSET#def ine PAGE LOCK IOBASE ADD + PAGE LOCK OFFSET

i n t po r tAdd r e s s [ ] = { IO PORT0 , IO PORT1 , IO PORT2 ,IO PORT3 , IO PORT4 , IO PORT5 } ;

/∗ page s p e c i f i c o f f s e t v a l u e s , the r e g i s t e r a f f e c t e d w i l ldepend

∗ e n t i r e l y on the c u r r e n t page , g r e a t c a r e must be takennot to

∗ r ead or w r i t e to / from the wrong page r e g i s t e r∗/

// i n t e r u p t p o l a r i t y s e l e c t , on l y v a l i d on page 1#def ine POL0 OFFSET 0x08

37

#def ine POL1 OFFSET 0x09#d e f i n e POL2 OFFSET 0x0A

#def ine POL0 IOBASE ADD + POL0 OFFSET#def ine POL1 IOBASE ADD + POL1 OFFSET#def ine POL2 IOBASE ADD + POL2 OFFSET

i n t po lAdd r e s s [ ] = {POL0 , POL1 , POL2} ;

// enab l e edge d e t e c t i n t e r u p t v a l i d on page 2#def ine ENAB0 OFFSET 0x08#def ine ENAB1 OFFSET 0x09#def ine ENAB2 OFFSET 0x0A

#def ine ENAB0 IOBASE ADD + ENAB0 OFFSET#def ine ENAB1 IOBASE ADD + ENAB1 OFFSET#def ine ENAB2 IOBASE ADD + ENAB2 OFFSET

i n t enabAddress [ ] = {ENAB0 , ENAB1 , ENAB2} ;

// I n d i c a t e s c u r r e n t l y a v a l a b l e edge i n t e r u p t s , page 3 on l y#def ine INT ID0 OFFSET 0x08#def ine INT ID1 OFFSET 0x09#def ine INT ID2 OFFSET 0x0A

#def ine INT ID0 IOBASE ADD + INT ID0 OFFSET#def ine INT ID1 IOBASE ADD + INT ID1 OFFSET#def ine INT ID2 IOBASE ADD + INT ID2 OFFSET

i n t i n tAdd r e s s [ ] = { INT ID0 , INT ID1 , INT ID2 } ;

/∗ PAGE LOCK b i t s p e c i f i c masks ∗/

#def ine PAGE0 0x00#def ine PAGE1 0x40#def ine PAGE2 0x80#def ine PAGE3 0xC0

#def ine LOCK PORT0 0x01#def ine LOCK PORT1 0x02#def ine LOCK PORT2 0x04#def ine LOCK PORT3 0x08#def ine LOCK PORT4 0x10#def ine LOCK PORT5 0x20

38

/∗ marcro d e f i n i t i o n s :∗ the f o l l o w i n g b l o ck c o n t a i n s a l l the r e q u i r e d macros f o r

the∗ d r i v e r

∗/

#def ine TYPE( dev ) (MINOR( dev ) >> 4)#def ine NUM( dev ) (MINOR( dev ) & 0 x0 f )

/∗∗ po r t s p e c i f i c commands .∗∗ a l l work w i l l be done through ioperm c a l l s ( thanks

Da r r y l f o r making me t h i n k i n t h i s∗ k ludgy hacky way∗∗ but i t s i m p l i f i e s i o p o r t a c c e s s by a huge amount∗/

/∗ r ead w r i t e po r t commands ∗/#def ine W PORT0 0#def ine W PORT1 1#def ine W PORT2 2#def ine W PORT3 3#def ine W PORT4 4#def ine W PORT5 5

#def ine R PORT0 10#def ine R PORT1 11#def ine R PORT2 12#def ine R PORT3 13#def ine R PORT4 14#def ine R PORT5 15

39

A.4 Server Code

A.4.1 robot.h

i n t c r e a t e s e r v e r s ( void ) ;i n t c l o s e s e r v e r s ( void ) ;i n t s t a r t p r o c e s s e s ( void ) ;

A.4.2 robot.c

#inc lude < r obo t . h>#inc lude < l i n u x /msg . h>#inc lude < l i n u x / i p c . h>#inc lude <commonlib . h>#inc lude < s t d i o . h>#inc lude < un i s t d . h>#inc lude < e r r n o . h>#inc lude < a s s e r t . h>#inc lude < s y s / wa i t . h>#inc lude < s i g n a l . h>#inc lude < l i n u x / t yp e s . h>#inc lude < l i n u x / f c n t l . h>#inc lude <pc . h>

// g l o b a l keys f o r motor c o n t r o l l e r message queuek e y t motor queue key ;i n t moto r queue i d ;

// g l o b a l keys f o r s e r i a l r ead p r o c e s s message queuek e y t s e r i a l R e a d qu e u e k e y ;i n t s e r i a l R e a d q u e u e i d ;

// g l o b a l keys f o r s e r i a l w r i t e p r o c e s s message queuek e y t s e r i a lW r i t e q u e u e k e y ;i n t s e r i a l W r i t e q u e u e i d ;

// g l o b a l keys f o r i r s e n s o r p r o c e s s message queuek e y t i r q u e u e k e y ;i n t i r q u e u e i d ;

// g l o b a l keys f o r sona r s e n s o r p r o c e s s message queuek e y t s ona r queue k e y ;

40

i n t s o n a r q u e u e i d ;

i n t main ( void ){

i n t kbva l ;

s e r i a lW r i t e m e s s a g e b u f f t e s t ;

i n i t k e y b o a r d ( ) ;

system ( ” c l e a r ” ) ;

p r i n t f ( ” \033[32m − STARTING MESSAGE SERVERS − \033[0m \n \n \n ” ) ;

c r e a t e s e r v e r s ( ) ;

p r i n t f ( ” \033[32m[SUCCESS ]\033 [ 0m \n” ) ;

p r i n t f ( ”\n \n \033 [32m − STARTING EXTERNAL PROCESSES− \033[0m \n \n \n” ) ;

s t a r t p r o c e s s e s ( ) ;


/∗ TODO : add i d l e code here , mabe p o l l f o r e s c key tok i l l main l oop

∗/

whi le (TRUE){

whi le ( ! k bh i t ( ) )u s l e e p (100000) ;

i f ( r eadch ( ) == 27)break ;

}

41

system ( ” c l e a r ” ) ;

p r i n t f ( ”\n \n \033 [32m − KILLING OFF ALL PROCESSES− \033[0m \n \n \n” ) ;

c l o s e s e r v e r s ( ) ;


c l o s e k e y b o a r d ( ) ;

return ( 1 ) ;}

/∗ a l l s e r v e r s to be c r e a t e d w i l lbe l i s t e d he r e

∗/

i n t c r e a t e s e r v e r s ( void ) // fn{

// c r e a t e message queue f o r motor p r o c e s s

p r i n t f ( ” C r e a t i n g motor message queue \n” ) ;

motor queue key = f t o k ( ”/mqkey/motorMQ” , ’ a ’ ) ;

moto r queue i d = msgget ( motor queue key , IPC CREAT| 0 7 7 7 ) ;

i f ( moto r queue i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r open ing message queue \n” ) ;e x i t (−1) ;

}p r i n t f ( ”Motor message queue s t a r t e d OK. Queue KEY : %

d ID : %d \n” , motor queue key , moto r queue i d ) ;

p r i n t f ( ” C r e a t i n g s e r i a l r ead message queue \n” ) ;

s e r i a l R e a d q u e u e k e y = f t o k ( ”/mqkey/ ser ia lReadMQ” , ’ a ’ );

42

s e r i a l R e a d q u e u e i d = msgget ( s e r i a l R e ad qu eu e k e y ,IPC CREAT | 0 7 7 7 ) ;

i f ( s e r i a l R e a d q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r open ing s e r i a l r ead messagequeue \n” ) ;

e x i t (−1) ;}p r i n t f ( ” s e r i a l r ead message queue s t a r t e d OK. Queue

KEY : %d ID : %d \n” , s e r i a l R e ad qu eu e k e y ,s e r i a l R e a d q u e u e i d ) ;

p r i n t f ( ” C r e a t i n g s e r i a l w r i t e message queue \n” ) ;

s e r i a lW r i t e q u e u e k e y = f t o k ( ”/mqkey/ se r i a lWr i t eMQ ” , ’ a’ ) ;

s e r i a l W r i t e q u e u e i d = msgget ( s e r i a lW r i t e q u e u e k e y ,IPC CREAT | 0 7 7 7 ) ;

i f ( s e r i a l W r i t e q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r open ing s e r i a l w r i t e messagequeue \n” ) ;

e x i t (−1) ;}p r i n t f ( ” s e r i a l w r i t e message queue s t a r t e d OK. Queue

KEY : %d ID : %d \n” , s e r i a lW r i t e q u e u e k e y ,s e r i a l W r i t e q u e u e i d ) ;

p r i n t f ( ” C r e a t i n g i r message queue \n” ) ;

i r q u e u e k e y = f t o k ( ”/mqkey/irMQ” , ’ a ’ ) ;

i r q u e u e i d = msgget ( i r q u e u e k e y , IPC CREAT | 0 7 7 7 ) ;

i f ( i r q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r open ing i r message queue \n”) ;

e x i t (−1) ;}

43

p r i n t f ( ” i r message queue s t a r t e d OK. Queue KEY : %dID : %d \n” , i r q u e u e k e y , i r q u e u e i d ) ;

p r i n t f ( ” C r e a t i n g sona r message queue \n” ) ;

s ona r queue k e y = f t o k ( ”/mqkey/sonarMQ” , ’ a ’ ) ;

s o n a r q u e u e i d = msgget ( sona r queue key , IPC CREAT| 0 7 7 7 ) ;

i f ( s o n a r q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r open ing sona r message queue\n” ) ;

e x i t (−1) ;}p r i n t f ( ” sona r message queue s t a r t e d OK. Queue KEY : %

d ID : %d \n” , sona r queue key , s o n a r q u e u e i d ) ;

return ( 1 ) ;}

i n t c l o s e s e r v e r s ( void ) // fn{

k i l l m e s s a g e b u f f k i l l p r o c e s s ;i n t i , tmp ;

k i l l p r o c e s s . mtype = KILL MOTOR ;k i l l p r o c e s s . a r g s = 9 ;

//we need to make s u r e the motor p r o c e s s d i e s o f f f i r s t, so i t can p r o p e r l y shut down the motors

msgsnd ( moto r queue id , ( s t ruc t msgbuf ∗ )&k i l l p r o c e s s ,s i z eo f ( k i l l m e s s a g e b u f f ) , IPC NOWAIT) ;

// wa i t f o r the ack to come back b e f o r e we move on , wew i l l hang u n t i l l the p r o c e s s d i e s o f f .

tmp = msgrcv ( moto r queue id , ( s t ruc t msgbuf ∗)&k i l l p r o c e s s , s i z eo f ( k i l l m e s s a g e b u f f ) ,KILL MOTOR ACK , 0 ) ;

44

s l e e p (1 ) ;k i l l p r o c e s s . mtype = KILL ;k i l l p r o c e s s . a r g s = 9 ;

// f l o o d a l l the queues w i th k i l l messages , t h i s shou lda s s u r e a l l p r o c e s s e s d i e o f f

f o r ( i = 0 ; i < 4 ; i++){

msgsnd ( s e r i a lW r i t e q u e u e i d , ( s t ruc t msgbuf ∗ )&k i l l p r o c e s s , s i z eo f ( k i l l m e s s a g e b u f f ) ,IPC NOWAIT) ;

msgsnd ( moto r queue id , ( s t ruc t msgbuf ∗ )&k i l l p r o c e s s , s i z eo f ( k i l l m e s s a g e b u f f ) ,IPC NOWAIT) ;

msgsnd ( i r q u e u e i d , ( s t ruc t msgbuf ∗ )&k i l l p r o c e s s, s i z eo f ( k i l l m e s s a g e b u f f ) , IPC NOWAIT) ;

msgsnd ( s e r i a l R e a d q u e u e i d , ( s t ruc t msgbuf ∗ )&k i l l p r o c e s s , s i z eo f ( k i l l m e s s a g e b u f f ) ,IPC NOWAIT) ;

msgsnd ( s ona r qu eu e i d , ( s t ruc t msgbuf ∗ )&k i l l p r o c e s s , s i z eo f ( k i l l m e s s a g e b u f f ) ,IPC NOWAIT) ;

s l e e p (1 ) ;

}

msgct l ( moto r queue id , IPC RMID , 0 ) ;msgc t l ( s e r i a l R e a d q u e u e i d , IPC RMID , 0 ) ;msgc t l ( s e r i a lW r i t e q u e u e i d , IPC RMID , 0 ) ;msgc t l ( i r q u e u e i d , IPC RMID , 0 ) ;msgc t l ( s ona r qu eu e i d , IPC RMID , 0 ) ;return (1 ) ;

}

i n t s t a r t p r o c e s s e s ( void ){

p i d t ch i d ;i n t execVa l ;

moto r mes sage bu f f motor msg ;

p r i n t f ( ”\ nS t a r t i n g motor P roce s s \n” ) ;

ch i d = f o r k ( ) ;

45

i f ( ch i d < 0){

f p r i n t f ( s t d e r r , ” e r r o r i n f o r k f o r motor p r o c e s s \n” ) ;

e x i t (−1) ;

}

i f ( ch i d == 0){

// i n c h i l dexecVa l = e x e c l ( ” . / motorProces s ” , ” motorProces s ” ,0 ) ;

i f ( execVa l < 0 ){

f p r i n t f ( s t d e r r , ” e r r o r i n e x e c l f o r MotorP roce s s \n” ) ;

e x i t (−1) ;}

}

p r i n t f ( ”Motor P r o c e s s e s CHID : %d \n” , ch i d ) ;

// s e t a l l motor speed s to z e r o and o f f s e t s too .motor msg . mtype = UPDATE MOTOR;motor msg . motorSpeed = 0 ;motor msg . b i a s = 0 ;motor msg . l e f t D i r e c t i o n = FORWARD;motor msg . r i g h t D i r e c t i o n = FORWARD;motor msg . b r e akCond i t i o n = BREAK OFF ;

msgsnd ( moto r queue id , ( s t ruc t msgbuf ∗ )&motor msg ,s i z eo f ( moto r mes sage bu f f ) , 0 ) ;

p r i n t f ( ”\ nS t a r t i n g i r P roce s s \n” ) ;

ch i d = f o r k ( ) ;

i f ( ch i d < 0){

f p r i n t f ( s t d e r r , ” e r r o r i n f o r k f o r i r p r o c e s s \n” ) ;e x i t (−1) ;

}

46

i f ( ch i d == 0){

// i n c h i l dexecVa l = e x e c l ( ” . / i r P r o c e s s ” , ” i r P r o c e s s ” ,0 ) ;


f p r i n t f ( s t d e r r , ” e r r o r i n e x e c l f o r i r P roce s s\n” ) ;

e x i t (−1) ;}

}

p r i n t f ( ” i r P r o c e s s e s CHID : %d \n” , ch i d ) ;

p r i n t f ( ”\ nS t a r t i n g sona r P roce s s \n” ) ;

ch i d = f o r k ( ) ;

i f ( ch i d < 0){

f p r i n t f ( s t d e r r , ” e r r o r i n f o r k f o r sona r p r o c e s s \n” ) ;

e x i t (−1) ;

}

i f ( ch i d == 0){

// i n c h i l dexecVa l = e x e c l ( ” . / s ona rP r o c e s s ” , ” s ona rP r o c e s s ” ,0 ) ;


f p r i n t f ( s t d e r r , ” e r r o r i n e x e c l f o r sona rP roce s s \n” ) ;

e x i t (−1) ;}

}

p r i n t f ( ” sona r P r o c e s s e s CHID : %d \n” , ch i d ) ;

47

p r i n t f ( ”\ nS t a r t i n g s e r i a l r ead Proce s s \n” ) ;

ch i d = f o r k ( ) ;

i f ( ch i d < 0){

f p r i n t f ( s t d e r r , ” e r r o r i n f o r k f o r s e r i a l r eadp r o c e s s \n” ) ;

e x i t (−1) ;

}

i f ( ch i d == 0){

// i n c h i l dexecVa l = e x e c l ( ” . / s e r i a l R e a d ” , ” s e r i a l R e a d ” ,0 ) ;


f p r i n t f ( s t d e r r , ” e r r o r i n e x e c l f o r s e r i a l R e a dProce s s \n” ) ;

e x i t (−1) ;}

}

p r i n t f ( ” s e r i a l r ead Proce s s CHID : %d \n” , ch i d ) ;

p r i n t f ( ”\ nS t a r t i n g s e r i a l w r i t e P roce s s \n” ) ;

ch i d = f o r k ( ) ;

i f ( ch i d < 0){

f p r i n t f ( s t d e r r , ” e r r o r i n f o r k f o r s e r i a l w r i t ep r o c e s s \n” ) ;

e x i t (−1) ;

}

i f ( ch i d == 0){

// i n c h i l d

48

execVa l = e x e c l ( ” . / s e r i a l W r i t e ” , ” s e r i a l W r i t e ” ,0 ) ;


f p r i n t f ( s t d e r r , ” e r r o r i n e x e c l f o r s e r i a lw r i t e P roce s s \n” ) ;

e x i t (−1) ;}

}

p r i n t f ( ” s e r i a l w r i t e P r o c e s s e s CHID : %d \n” , ch i d ) ;

p r i n t f ( ”\ nS t a r t i n g subsumpt ion c o n t r o l l e r P roce s s \n” );

ch i d = f o r k ( ) ;

i f ( ch i d < 0){

f p r i n t f ( s t d e r r , ” e r r o r i n f o r k f o r subsumpt ionc o n t r o l l e r p r o c e s s \n” ) ;

e x i t (−1) ;

}

i f ( ch i d == 0){

// i n c h i l dexecVa l = e x e c l ( ” . / subCon t r o l ” , ” subCon t r o l ” ,0 ) ;


f p r i n t f ( s t d e r r , ” e r r o r i n e x e c l f o r subsumpt ionc o n t r o l P roce s s \n” ) ;

e x i t (−1) ;}

}

p r i n t f ( ” subsumpt ion c o n t r o l P r o c e s s e s CHID : %d \n” ,ch i d ) ;

return ( 1 ) ;}

49

A.5 Subsumption Controller

A.5.1 subControl.h

#inc lude <commonlib . h>

// b e h a v i o u r a l t h r e s h o l d d e f i n e s

// 100 cm Range#def ine AVOID SONAR RANGE 125

#def ine VEER IR RANGE 50

#def ine SIDE BUMP RANGE 30

// behavo r a l motor speed d e f i n e s

#def ine AVOID MOTOR SPEED 0xB0#def ine AVOID TURN RATE 0xA0

#def ine VEER MOTOR SPEED 0xD0#def ine VEER TURN RATE 0x60

#def ine SKIM MOTOR SPEED 0xFE#def ine SKIM TURN RATE 0x10

#def ine SIDE BUMP MOTOR SPEED 0xB0#def ine SIDE BUMP TURN RATE 0x60

void i n i t ( void ) ;void run ( void ) ;void shut down ( void ) ;void r e c a l c s u b s ump t i o n ( i n t s i g a l a rm ) ;void dump te l emet ry ( void ) ;

boo l r u n b eha v i o u r ( sub packe t ∗ packe t ) ;boo l a v o i d b e h a v i o u r ( sub packe t ∗ packe t ) ;boo l v e e r b e h a v i o u r ( sub packe t ∗ packe t ) ;boo l s i d e bump behav i ou r ( sub packe t ∗ packe t ) ;boo l s k im behav i o u r ( sub packe t ∗ packe t ) ;

A.5.2 subControl.c

#inc lude < subCon t r o l . h>#inc lude < s t d i o . h>#inc lude < un i s t d . h>#inc lude < f c n t l . h>#inc lude < e r r n o . h>

50

#inc lude < t e rm i o s . h>//#i n c l u d e <commonlib . h> now i n c l u d e d i n heade r f i l e#inc lude < l i n u x /msg . h>#inc lude < l i n u x / i p c . h>#inc lude < s t r i n g . h>#inc lude < s i g n a l . h>#inc lude < s y s / t ime . h>#inc lude < s t d l i b . h>

// g l o b a l IPC queue ID v a r a i b l e sk e y t motor queue key ;i n t moto r queue i d ;

k e y t s ona r queue k e y ;i n t s o n a r q u e u e i d ;

k e y t i r q u e u e k e y ;i n t i r q u e u e i d ;

// g l o b a l v a r i a b l e s f o r s e n s o r r e s u l t s

f l o a t sonarRange = 0 ;f l o a t i r 0 r a n g e = 0 ; // l e f t s i d ef l o a t i r 1 r a n g e = 0 ; // l e f t c o r n e rf l o a t i r 2 r a n g e = 0 ; // r i g h t c o r n e rf l o a t i r 3 r a n g e = 0 ; // r i g h t s i d e

// g l o b a l v a r i a b l e s f o r i n t e r n a l r e f e r e n c e o f motor speeds ,b a r r i n g chages by o th e r

// p r o c e s s e s t h i s shou l d r e p r e s e n t the a c cua l speed o f themotor ( not t a k i n g i n t o

// account changes due to PID loop ) .i n t motor speed = 0 ;i n t b i a s = 0 ;

// c o n t a i n s a coun t e r r e p r e n t i n g the c u r r e n t l y a c i v eb ehav i ou r runn ing on the system

i n t b e h a v i o u r a c t i v e = 0 ;

// loop coun t e r to a l l ow us to dump r e a l t ime t e l eme t r y tothe t e rm i n a l .

//number o f 100mS updates to wa i t b e f o r e updat i ng d i s p l a yi n t d i s p l a y dump coun t e r = 10 ;//number o f 100mS updates to wa i t b e f o r e updat i ng d i s p l a yi n t d i s p l a y dump coun t e r = 5 ;

// t ime r c o n f i g f i l e s

51

s t a t i c s t ruc t i t i m e r v a l t ime r ;s t ruc t t ime v a l t im e r v a l u e s ;


i n i t ( ) ;

run ( ) ;

shut down ( ) ;

return (1 ) ;}

void i n i t ( void ){

s t ruc t t ime v a l t v ;// seed random number g e n e r a t o rge t t imeo fday (&tv ,NULL) ;s r and ( tv . t v s e c ˆ tv . t v u s e c ) ;

// open IPC comms channe l to s e r v e r p r o c e s s


i r q u e u e i d = msgget ( i r q u e u e k e y , 0 7 7 7 ) ;i f ( i r q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to i r messagequeue : subsumpt ion c o n t r o l l e r p r o c e s s \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 msubCon t r o l l e r a t t a c h i n g to i r messagequeue KEY : %d ID : %d \033 [0m \n” , i r q u e u e k e y ,i r q u e u e i d ) ;


s o n a r q u e u e i d = msgget ( sona r queue key , 0 7 7 7 ) ;i f ( s o n a r q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to sona r messagequeue : subsumpt ion c o n t r o l l e r p r o c e s s \n” ) ;

e x i t (−1) ;}

52

p r i n t f ( ” \033[31 msubCon t r o l l e r a t t a c h i n g to sona rmessage queue KEY : %d ID : %d \033 [0m \n” ,s ona r queue k e y , s o n a r q u e u e i d ) ;


moto r queue i d = msgget ( motor queue key , 0 7 7 7 ) ;i f ( moto r queue i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to motor messagequeue : subsumpt ion c o n t r o l l e r p r o c e s s \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 msubCon t r o l l e r a t t a c h i n g to motormessage queue KEY : %d ID : %d \033 [0m \n” ,motor queue key , moto r queue i d ) ;

// s t a r t the update t ime r and r e g i s t e r the even t hand l e r// r e g i s t e r even t h and l e rs i g n a l (SIGALRM , ∗ r e c a l c s u b s ump t i o n ) ;// s e t t ime r to 100 ,000 uS = 100mS f o r updatet im e r v a l u e s . t v s e c = 0 ;t im e r v a l u e s . t v u s e c = 100000;t ime r . i t i n t e r v a l = t im e r v a l u e s ;t ime r . i t v a l u e = t im e r v a l u e s ;

s e t i t i m e r ( ITIMER REAL,& t imer ,NULL) ;

}

void run ( void ){

i n t tmp ;k i l l m e s s a g e b u f f k i l l m e s s a g e ;i r v a l u e m e s s a g e b u f f i r me s s a g e ;s o n a r upda t e me s s a g e bu f f sona r mes sage ;

k i l l m e s s a g e b u f f k i l l m e s s a g e ;i r u p d a t e me s s a g e b u f f i r me s s a g e ;s o n a r upda t e me s s a g e bu f f sona r mes sage ;

// loop u n t i l l we f i n d a k i l l message i n the queue .whi le ( 1 )

53

{

u s l e e p (10000) ;

tmp = msgrcv ( i r q u e u e i d , ( s t ruc t msgbuf ∗)&i r me s s age , s i z eo f ( i r v a l u e m e s s a g e b u f f ) ,IR UPDATE , IPC NOWAIT) ;

i f ( tmp == s i z eo f ( i r v a l u e m e s s a g e b u f f ) ){

// d e a l w i th incoming IR messagei r 0 r a n g e = i r me s s a g e . i r 0 ; // l e f t s i d ei r 1 r a n g e = i r me s s a g e . i r 1 ; // l e f t ang l ei r 2 r a n g e = i r me s s a g e . i r 2 ; // r i g h t ang l ei r 3 r a n g e = i r me s s a g e . i r 3 ; // r i g h t s i d e

}

tmp = msgrcv ( s ona r qu eu e i d , ( s t ruc t msgbuf ∗)&sonar message , s i z eo f ( s o n a r upda t e me s s a g e bu f f ) ,SONAR UPDATE, IPC NOWAIT) ;

i f ( tmp == s i z eo f ( s o n a r upda t e me s s a g e bu f f ) ){

// d e a l w i th incoming IR message

sonarRange = sona r mes sage . s o n a r r a ng e ;}

tmp = msgrcv ( i r q u e u e i d , ( s t ruc t msgbuf ∗)&i r me s s age , s i z eo f ( i r u p d a t e me s s a g e b u f f ) ,IR UPDATE , IPC NOWAIT) ;

i f ( tmp == s i z eo f ( i r u p d a t e me s s a g e b u f f ) ){

//TODO: add code to d e a l w i th incoming IRmessage

i r 0 r a n g e = i r me s s a g e . i r 0 ;i r 1 r a n g e = i r me s s a g e . i r 1 ;i r 2 r a n g e = i r me s s a g e . i r 2 ;i r 3 r a n g e = i r me s s a g e . i r 3 ;

54

}

tmp = msgrcv ( s ona r qu eu e i d , ( s t ruc t msgbuf ∗)&sonar message , s i z eo f ( s o n a r upda t e me s s a g e bu f f ) ,SONAR UPDATE, IPC NOWAIT) ;

i f ( tmp == s i z eo f ( s o n a r upda t e me s s a g e bu f f ) ){

//TODO: add code to d e a l w i th incoming IRmessage

sonarRange = sona r mes sage . s o n a r r a ng e ;}

// have we r e c e i v e d a k i l l message ?tmp = msgrcv ( moto r queue id , ( s t ruc t msgbuf ∗)&

k i l l m e s s a g e , s i z eo f ( k i l l m e s s a g e b u f f ) , KILL ,IPC NOWAIT) ;

i f ( k i l l m e s s a g e . a r g s == 9){

p r i n t f ( ” K i l l i n g subsumpt ion c o n t r o l l e r P roce s s. \ n” ) ;

break ;}

}

}

void shut down ( void ){

}

void r e c a l c s u b s ump t i o n ( i n t s i g a l a rm ) // fn{

// subsumpt ion c o n t r o l packe t s .

55

s ub packe t avo i d ;s ub packe t v e e r ;s ub packe t s ide bump ;sub packe t skim ;sub packe t run ;

moto r mes sage bu f f motor message ;

i f ( d i s p l a y dump coun t e r == 0){

d i s p l a y dump coun t e r = 10 ;dump te l emet ry ( ) ;

}d i s p l a y dump coun t e r −−;

//TODO: add subsumt ion code he r e :

// run a l l subsumpt ion f u n c t i o n sa v o i d b e h a v i o u r (& avo i d ) ;v e e r b e h a v i o u r (& ve e r ) ;s i d e bump behav i ou r (& s ide bump ) ;s k im behav i o u r (& skim ) ;r u n b eha v i o u r (& run ) ;

i f ( avo i d . a c t i v e ){

motor speed=avo i d . motor speed ;b i a s = avo i d . b i a s ;b e h a v i o u r a c t i v e = 4 ;

}e l s e i f ( v e e r . a c t i v e ){

motor speed=ve e r . motor speed ;b i a s = ve e r . b i a s ;b e h a v i o u r a c t i v e =3;

}e l s e i f ( s ide bump . a c t i v e ){

motor speed=side bump . motor speed ;b i a s=s ide bump . b i a s ;b e h a v i o u r a c t i v e =2;

}e l s e i f ( skim . a c t i v e ){

motor speed=skim . motor speed ;b i a s=skim . b i a s ;

56

b e h a v i o u r a c t i v e =1;}e l s e i f ( run . a c t i v e ){

motor speed=run . motor speed ;b i a s=run . b i a s ;b e h a v i o u r a c t i v e =0;

}

motor message . mtype=UPDATE MOTOR;motor message . motorSpeed = motor speed ;motor message . b i a s = b i a s ;motor message . l e f t D i r e c t i o n=FORWARD;motor message . r i g h t D i r e c t i o n=FORWARD;motor message . b r e akCond i t i o n = BREAK OFF ;

msgsnd ( moto r queue id , ( s t ruc t msgbuf ∗ )&motor message , s i z eo f ( moto r mes sage bu f f ) ,IPC NOWAIT) ;

}

void dump te l emet ry ( void ) // fn{

system ( ” c l e a r ” ) ;p r i n t f ( ”− Cur r en t system t e l eme t r y −\n \n” ) ;

p r i n t f ( ” Sonar range to t a r g e t : \ t %5.4 f cm\n\n” ,sonarRange ) ;

p r i n t f ( ” IR 0 range to t a r g e t :\ t \ t %5.4 f cm \n” ,i r 0 r a n g e ) ;

p r i n t f ( ” IR 1 range to t a r g e t :\ t \ t %5.4 f cm\n” ,i r 1 r a n g e ) ;



p r i n t f ( ”\n \ nMotor Speed :\ t \ t \ t %5d \n” ,motor speed ) ;

p r i n t f ( ”Motor B ia s :\ t \ t \ t %5d \n” , b i a s ) ;

57

p r i n t f ( ”\n\ nCu r r e n t l y Ac t i v e b ehav i ou r : \ t %5d \n”, b e h a v i o u r a c t i v e ) ;

}

/∗

t y p ed e f s t r u c t {boo l a c t i v e ;i n t motor speed ;i n t b i a s ;

} sub packe t ;∗/

// implements the run behav i ou r w i t h i n the system// t h i s w i l l a lways be a c t i v e , and cau s e s the p l a t f o rm to// run p e r f e c t l y s t r a i g h t .boo l r u n b eha v i o u r ( sub packe t ∗ packe t ) // fn{

packet−>a c t i v e = TRUE;packet−>motor speed = MAX SPEED;packet−>b i a s = 0 ;return (TRUE) ;

}

// implements the avo i d behav i ou r// t h i s i s the most complex behav i ou r o f the bunch

boo l a v o i d b e h a v i o u r ( sub packe t ∗ packe t ) // fn{

i n t random ;i f ( sonarRange < AVOID SONAR RANGE){

// t h e r e i s someth ing w i t h i n our minimum range ,so we

// need to avo i d i t// nex t we check to s e e i f we a r e a l r e a d y

t u r n i n g i n some d i r e c t i o n

packet−>a c t i v e = TRUE; // the behav i ou r i sa c t i v e

packet−>motor speed = AVOID MOTOR SPEED ; // s lowdown a b i t

i f ( b i a s != 0){

//we might as w e l l keep t u r n i n g i n the samed i r e c t i o n

58

i f ( b i a s >0){

// s e t tu rn r a t epacket−>b i a s = AVOID TURN RATE ;

}e l s e{

// s e t n e g a t i v e tu rn r a t epacket−>b i a s = −1∗ AVOID TURN RATE ;

}

}// so we a r e not t u r n i n g so we need to d e c i d e

which way to go// l e t s check the IR to s e e i f one s i d e i s

c l e a r e r then the o th e ri f ( i r 3 r a n g e b i a s = −1 ∗ AVOID TURN RATE ;}i f ( i r 0 r a n g e b i a s = AVOID TURN RATE ;

}i f ( i r 0 r a n g e == i r 3 r a n g e ){

// we l l , we can ’ t t e l l any th i ng by thes i d e s e n s o r s

// l e t s check the c o r n e r s e n s o r si f ( i r 2 r a n g e b i a s = −1∗AVOID TURN RATE ;

}i f ( i r 1 r a n g e b i a s = AVOID TURN RATE ;

}i f ( i r 1 r a n g e == i r 2 r a n g e ){//we can ’ t make any s o r t o f i n fo rmed// d e c i s i o n so l e t s j u s t gue s s which way to

go

59

// l e t s use a random number to g en e r a t e ourd e c i s i o n

random = rand ( ) %1 ; // ge t a random #between 0 and 1

i f ( random == 1){

// tu rn r i g h tpacket−>b i a s = AVOID TURN RATE ;

}e l s e{

packet−>b i a s = −1 ∗ AVOID TURN RATE ;}

}}

}e l s e{

// s e t the a c t i v e f l a g to f a l s e and s e t a l l//motor v a l u e s to t h e i r c u r r e n t v a l u epacket−>a c t i v e = FALSE ;packet−>motor speed = motor speed ;packet−>b i a s = b i a s ;return (FALSE) ;

}

return (TRUE) ;}

boo l v e e r b e h a v i o u r ( sub packe t ∗ packe t ) // fn{

// implements the v e e r b ehav i ou ri f ( ( i r 1 r a n g e < VEER IR RANGE) | | ( i r 2 r a n g e <

VEER IR RANGE) ){

packet−>a c t i v e = TRUE;packet−>motor speed = VEER MOTOR SPEED;// t h e r e i s someth ing w i t h i n the minimum range o f

the IR s e n s o ri f ( i r 1 r a n g e b i a s = −1 ∗ VEER TURN RATE ;}e l s e{

packet−>b i a s = VEER TURN RATE ;

60

}}e l s e{

// no th ing i n rangepacket−>a c t i v e = FALSE ;packet−>motor speed = motor speed ;packet−>b i a s = b i a s ;return (FALSE) ;

}

return (TRUE) ;}

boo l s i d e bump behav i ou r ( sub packe t ∗ packe t ) // fn{

i f ( ( i r 0 r a n g e < SIDE BUMP RANGE) | | ( i r 3 r a n g e <SIDE BUMP RANGE) )

{// i s the l e f t s e n s o r s h o r t e r then the r i g hpacket−>a c t i v e = TRUE;packet−>motor speed = SIDE BUMP MOTOR SPEED ;i f ( i r 0 r a n g e b i a s = −1 ∗ SIDE BUMP TURN RATE ;

}e l s e{

// tu rn r i g h tpacket−>b i a s = SIDE BUMP TURN RATE ;

}}e l s e{

packet−>a c t i v e = FALSE ;packet−>motor speed = motor speed ;packet−>b i a s = b i a s ;return (FALSE) ;

}

return (TRUE) ;}

boo l s k im behav i o u r ( sub packe t ∗ packe t ) // fn{

i f ( i r 0 r a n g e != i r 3 r a n g e ){

packet−>a c t i v e =TRUE;

61

packet−>motor speed = SKIM MOTOR SPEED ;i f ( i r 0 r a n g e b i a s = SKIM TURN RATE ;

}e l s e{

// o t h e rw i s e we want to tu rn l e f t a l i t t l epacket−>b i a s = −1 ∗ SKIM TURN RATE ;

}}e l s e{

packet−>a c t i v e=FALSE ;packet−>motor speed = motor speed ;packet−>b i a s = b i a s ;return (FALSE) ;

}return (TRUE) ;

}

62

A.6 Motor Control Process

A.6.1 motorProcess.h

#def ine SPEED STEP 40

void i n i t ( void ) ;

void run ( void ) ;

void shut down ( void ) ;

void upd a t e t im e r h a n d l e r ( i n t signum ) ;

char ∗ mak e s e r i a l p a c k e t ( void ) ;

char ∗ mak e s e r i a l p a c k e t ( ) ;

A.6.2 motorProcess.c

#inc lude <motorProces s . h>#inc lude < s t d i o . h>#inc lude < un i s t d . h>#inc lude < f c n t l . h>#inc lude < e r r n o . h>#inc lude < t e rm i o s . h>#inc lude <commonlib . h>#inc lude < l i n u x /msg . h>#inc lude < l i n u x / i p c . h>#inc lude < s t r i n g . h>#inc lude < s i g n a l . h>#inc lude < s y s / t ime . h>

//message queue pa ramete r sk e y t motor queue key ;i n t moto r queue i d ;

k e y t s e r i a l q u e u e k e y ;i n t s e r i a l q u e u e i d ;

k e y t s e r i a l R e a d qu e u e k e y ;i n t s e r i a l R e a d q u e u e i d ;k e y t s e r i a l q u e u e k e y ;i n t s e r i a l q u e u e i d ;

// g l o b a l v a r i a b l e s , updated by message queuei n t motorSpeed = 0 ;i n t b i a s = 0 ;i n t l e f t S p e e d = 0 x00 ;

63

i n t r i g h tSp e ed = 0 x00 ;i n t l e f t D i r e c t i o n = FORWARD;i n t r i g h t D i r e c t i o n = FORWARD;i n t b r e akCond i t i o n = BREAK OFF ;i n t l e f t t a r g e t s p e e d = 0 x00 ;i n t r i g h t t a r g e t s p e e d = 0 x00 ;

s t a t i c s t ruc t i t i m e r v a l t ime r ;s t a t i c s t ruc t t ime v a l t im e r v a l u e s ;

boo l l a s t u p d a t e ;


i n i t ( ) ;

run ( ) ;

shut down ( ) ;

return (1 ) ;}




moto r queue i d = msgget ( motor queue key , 0 7 7 7 ) ;i f ( moto r queue i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to message queue \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 mmotorProcesses a t t a c h i n g to motorc o n t r o l message queue KEY : %d ID : %d \033 [0m \n” ,motor queue key , moto r queue i d ) ;

s e r i a l q u e u e k e y = f t o k ( ”/mqkey/ se r i a lWr i t eMQ ” , ’ a ’ ) ;

s e r i a l q u e u e i d = msgget ( s e r i a l q u e u e k e y , 0 7 7 7 ) ;i f ( s e r i a l q u e u e i d < 0)

64

{f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to s e r i a l message

queue \n” ) ;e x i t (−1) ;

}

p r i n t f ( ” \033[31 mmotorProcesses a t t a c h i n g to s e r i a lw r i t e message queue KEY : %d ID : %d \033 [0m \n” ,s e r i a l q u e u e k e y , s e r i a l q u e u e i d ) ;


s e r i a l R e a d q u e u e i d = msgget ( s e r i a l R e ad qu eu e k e y, 0 7 7 7 ) ;


f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to s e r i a l r eadmessage queue \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 mmotorProcesses a t t a c h i n g to s e r i a l r eadmessage queue KEY : %d ID : %d \033 [0m \n” ,

s e r i a l R e a d qu e u e k e y , s e r i a l R e a d q u e u e i d ) ;

s e r i a l q u e u e k e y = f t o k ( ”/mqkey/ se r i a lWr i t eMQ ” , ’ a ’ ) ;

s e r i a l q u e u e i d = msgget ( s e r i a l q u e u e k e y , 0 7 7 7 ) ;i f ( s e r i a l q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to s e r i a l messagequeue \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 mmotorProcesses a t t a c h i n g to s e r i a lw r i t e message queue KEY : %d ID : %d \033 [0m \n” ,s e r i a l q u e u e k e y , s e r i a l q u e u e i d ) ;

65

// s t a r t the update t ime r and r e g i s t e r the even t hand l e r// r e g i s t e r even t h and l e rs i g n a l (SIGALRM , ∗ upd a t e t im e r h a n d l e r ) ;// s e t t ime r to 100000uS = 100mS f o r updatet im e r v a l u e s . t v s e c = 0 ;t im e r v a l u e s . t v u s e c = 100000;t ime r . i t i n t e r v a l = t im e r v a l u e s ;t ime r . i t v a l u e = t im e r v a l u e s ;


}

void run ( void ){

i n t tmp ;k i l l m e s s a g e b u f f k i l l m e s s a g e ;moto r mes sage bu f f motor message ;

// loop u n t i l l we f i n d a k i l l message i n the queue .whi le ( 1 ){

u s l e e p (10000) ;

tmp = msgrcv ( moto r queue id , ( s t ruc t msgbuf ∗)&motor message , s i z eo f ( moto r mes sage bu f f ) ,UPDATE MOTOR, IPC NOWAIT) ;

i f ( tmp == s i z eo f ( moto r mes sage bu f f ) ){

//we have r e c e i v e d a motor update message

motorSpeed = motor message . motorSpeed ;b i a s = motor message . b i a s ;l e f t D i r e c t i o n = motor message . l e f t D i r e c t i o n ;r i g h t D i r e c t i o n = motor message . r i g h t D i r e c t i o n ;b r e akCond i t i o n = motor message . b r e akCond i t i o n ;

// c a l c u l a t e motor speedsi f ( b i a s < 0){

// t h i s i s an add because the b i a s i sa l r e a d y −vel e f t t a r g e t s p e e d = motorSpeed +

b i a s ;

66

//make s u r e we d idn ’ t drop belowze ro

i f ( l e f t t a r g e t s p e e d < 0)l e f t t a r g e t s p e e d = 0 ;

r i g h t t a r g e t s p e e d = motorSpeed ;}i f ( b i a s > 0){

l e f t t a r g e t s p e e d = motorSpeed ;r i g h t t a r g e t s p e e d = motorSpeed −

b i a s ;//make s u r e we d idn ’ t drop below 0i f ( r i g h t t a r g e t s p e e d < 0)

r i g h t t a r g e t s p e e d = 0 ;}i f ( b i a s == 0){

l e f t t a r g e t s p e e d = motorSpeed ;r i g h t t a r g e t s p e e d = motorSpeed ;

}}//TODO: make speed changes s low towards t a r g e t

i n s t e a d// o f qu i ck jumps

// have we r e c e i v e d a k i l l message ?tmp = msgrcv ( moto r queue id , ( s t ruc t msgbuf ∗)&

k i l l m e s s a g e , s i z eo f ( k i l l m e s s a g e b u f f ) ,KILL MOTOR , IPC NOWAIT) ;

i f ( tmp == s i z eo f ( k i l l m e s s a g e b u f f ) ){

p r i n t f ( ” K i l l i n g motor P roce s s . \ n” ) ;break ;

}

}

}

void shut down ( void ){k i l l m e s s a g e b u f f k i l l a c k ;

k i l l a c k . mtype = KILL MOTOR ACK ;k i l l a c k . a r g s = 9;

67

//TODO: Shut down motors he r e .

l e f t t a r g e t s p e e d = 0 ;r i g h t t a r g e t s p e e d = 0 ;l e f t S p e e d = 0 ;r i g h tSp e ed = 0 ;l a s t u p d a t e = FALSE ;// wa i t f o r the l a s t t ime r to tu rn o f f the motorss l e e p (1 ) ;

// we have to acknowlege the motor p r o c e s s has been k i l l e do f f

msgsnd ( moto r queue id , ( s t ruc t msgbuf ∗ )&k i l l a c k , s i z eo f (k i l l m e s s a g e b u f f ) , IPC NOWAIT) ;

}

void upd a t e t im e r h a n d l e r ( i n t signum ){

s e r i a lW r i t e m e s s a g e b u f f motor update ;r e a d r e q u e s t m e s s a g e b u f f quad read ;

quad read . mtype = QUAD REQUEST;

//msgsnd ( s e r i a l R e a d q u e u e i d , ( s t r u c t msgbuf ∗ )&quad read , s i z e o f ( r e a d r e q u e s t m e s s a g e b u f f ) ,IPC NOWAIT) ;

// s e e i f we a r e at the d e s i r e d speedi f ( r i g h tSp e ed != r i g h t t a r g e t s p e e d ){

i f ( r i g h tSp e ed < r i g h t t a r g e t s p e e d ){

r i g h tSp e ed += SPEED STEP ;// d id we j u s t o v e r s hoo t the speed ?i f ( r i g h tSp e ed > r i g h t t a r g e t s p e e d )

r i g h tSp e ed = r i g h t t a r g e t s p e e d ;}e l s e{

r i g h tSp e ed −= SPEED STEP ;// d id we j u s t unde r shoot the speed ?

68

i f ( r i g h tSp e ed < r i g h t t a r g e t s p e e d )r i g h tSp e ed = r i g h t t a r g e t s p e e d ;

}}// see i f we a r e at the d e s i r e d speed

i f ( l e f t S p e e d != l e f t t a r g e t s p e e d ){

i f ( l e f t S p e e d < l e f t t a r g e t s p e e d ){

l e f t S p e e d += SPEED STEP ;// d id we j u s t o v e r s hoo t the speed ?i f ( l e f t S p e e d > l e f t t a r g e t s p e e d )

l e f t S p e e d = l e f t t a r g e t s p e e d ;}e l s e{

l e f t S p e e d −= SPEED STEP ;// d id we j u s t unde r shoot the speed ?

i f ( l e f t S p e e d < l e f t t a r g e t s p e e d )l e f t S p e e d = l e f t t a r g e t s p e e d ;

}}

motor update . mtype = SERIAL WRITE ;motor update . s i z e = 13 ;motor update . po r t = 2 ;

motor update . s e r i a l s t r i n g [ 0 ] = 0x10 ; // packe theade r

motor update . s e r i a l s t r i n g [ 1 ] = 0x01 ;motor update . s e r i a l s t r i n g [ 2 ] = 0x20 ; //PWM add r e s smotor update . s e r i a l s t r i n g [ 3 ] = 0xF0 ; // Return

add r e s smotor update . s e r i a l s t r i n g [ 4 ] = 0x00 ; // F i l l w i th

checksum l a t e rmotor update . s e r i a l s t r i n g [ 5 ] = 0x01 ; // send ON

commandmotor update . s e r i a l s t r i n g [ 6 ] = 0x04 ; // # of by t e s

to f o l l o w i n the packe tmotor update . s e r i a l s t r i n g [ 7 ] = r i g h t D i r e c t i o n ;motor update . s e r i a l s t r i n g [ 8 ] = r i g h tSpe ed ;motor update . s e r i a l s t r i n g [ 9 ] = l e f t D i r e c t i o n ;motor update . s e r i a l s t r i n g [ 1 0 ] = l e f t S p e e d ;motor update . s e r i a l s t r i n g [ 1 1 ] = 0 x10 ; // c l o s i n g

packe tmotor update . s e r i a l s t r i n g [ 1 2 ] = 0 x04 ;

69

// c a l c u l a t e checksum//d5

motor update . s e r i a l s t r i n g [ 4 ] = motor update .s e r i a l s t r i n g [ 2 ] ˆ motor update . s e r i a l s t r i n g [ 3 ] ˆmotor update . s e r i a l s t r i n g [ 5 ] ˆ motor update .s e r i a l s t r i n g [ 6 ] ˆ motor update . s e r i a l s t r i n g [ 7 ] ˆmotor update . s e r i a l s t r i n g [ 8 ] ˆ motor update .s e r i a l s t r i n g [ 9 ] ˆ motor update . s e r i a l s t r i n g [ 1 0 ] ;

msgsnd ( s e r i a l q u e u e i d , ( s t ruc t msgbuf ∗ )&motor update, s i z eo f ( s e r i a lW r i t e m e s s a g e b u f f ) , IPC NOWAIT) ;

motor update . mtype = SERIAL WRITE ;motor update . s e r i a l s t r i n g = mak e s e r i a l p a c k e t ( ) ;motor update . s i z e = 13 ;motor update . po r t = 2 ;

msgsnd ( s e r i a l q u e u e i d , ( s t ruc t msgbuf ∗ )&motor update, s i z eo f ( s e r i a lW r i t e m e s s a g e b u f f ) , IPC NOWAIT) ;

}

char ∗ mak e s e r i a l p a c k e t ( ){

char ∗ s e r s t r i n g ;i n t i ;

s e r s t r i n g [ 0 ] = 0x10 ; // packe t heade rs e r s t r i n g [ 1 ] = 0x01 ;s e r s t r i n g [ 2 ] = 0x20 ; //PWM add r e s ss e r s t r i n g [ 3 ] = 0 x f0 ; // Return add r e s ss e r s t r i n g [ 4 ] = 0x00 ; // F i l l w i th checksum l a t e rs e r s t r i n g [ 5 ] = 0x01 ; // send ON commands e r s t r i n g [ 6 ] = 0x04 ; // # of by t e s to f o l l o w i n the

packe ts e r s t r i n g [ 7 ] = l e f t D i r e c t i o n ;s e r s t r i n g [ 8 ] = l e f t S p e e d ;s e r s t r i n g [ 9 ] = r i g h t D i r e c t i o n ;s e r s t r i n g [ 1 0 ] = r i g h tSp e ed ;s e r s t r i n g [ 1 1 ] = 0 x10 ; // c l o s i n g packe ts e r s t r i n g [ 1 2 ] = 0 x04 ;

// c a l c u l a t e checksum

s e r s t r i n g [ 4 ] = s e r s t r i n g [ 2 ] ˆ s e r s t r i n g [ 3 ] ˆs e r s t r i n g [ 5 ] ˆ s e r s t r i n g [ 6 ] ˆ s e r s t r i n g [ 7 ] ˆ

70

s e r s t r i n g [ 8 ] ˆ s e r s t r i n g [ 9 ] ˆ s e r s t r i n g [ 1 0 ] ;

return ( s e r s t r i n g ) ;

}

A.6.3 motorPIControl.h

#i fnde f MOTOR#def ine MOTOR 1

typedef enum { l =1, r } motor ;#end i f

#def ine UPDATE RATE 100

#def ine INTEGRAL GAIN 0 .10#def ine PROPORTIONAL GAIN 1 .0

void a l t e r p ow e r ( f l o a t e r r o r , motor motor Index ) ;f l o a t i n t e g r a t e ( f l o a t l e f tMo t o rV e l , f l o a t r i gh tMoto rVe l ,

f l o a t b i a s L e v e l ) ;void c o n t r o l mo t o r s p e e d ( ) ;f l o a t g e t mo t o r v e l ( motor motor Index ) ;void s e t mo t o r s p e ed ( f l o a t power , motor motor Index ) ;f l o a t c h e c k l i m i t ( f l o a t va l , f l o a t low , f l o a t h igh ) ;

A.6.4 motorPIControl.c

/∗∗ Motor PI c o n t r o l l oop . . .∗ adapted from code p r e s e n t e d i n mob i l e robot s ,

i n s p i r a t i o n to imp l ementa t i on ( Jones , Jo septh L . Et a l l)

∗∗ PI l oop i s op t im i z ed manua l l y∗/

#inc lude < s t d i o . h>#inc lude < s t d l i b . h>#inc lude <motorP ICont ro l . h>

f l o a t t a r g e t V e l C l i c k s = 0 . 0 ;f l o a t t a r g e t B i a s C l i c k s = 0 . 0 ;

71

f l o a t power [ 2 ]= { 0 . 0 , 0 . 0 } ;f l o a t i n t e g r a l = 0 . 0 ;

motor l e f t = ( motor ) ’ l ’ ;motor r i g h t = ( motor ) ’ r ’ ;

void a l t e r p ow e r ( f l o a t e r r o r , motor motor Index ) // fn{

power [ ( i n t ) motor Index ] = c h e c k l i m i t ( power [ ( i n t )motor Index ] + e r r o r , 0 . 0 , 1 0 0 . 0 ) ;

s e t mo t o r s p e ed ( power [ ( i n t ) motor Index ] , motor Index ) ;}

f l o a t i n t e g r a t e ( f l o a t l e f tMo t o rV e l , f l o a t r i gh tMoto rVe l ,f l o a t b i a s L e v e l ) // fn

{i n t e g r a l += l e f tMo t o rVe l + b i a s L e v e l −

r i g h tMoto rVe l ;return ( i n t e g r a l ) ;

}

void c o n t r o l mo t o r s p e e d ( ) // fn{

f l o a t l e f tMo to rVe l , r i gh tMoto rVe l , i n t e g r a l E r r o r ,l e f tMo t o rE r r o r , r i g h tMo t o rE r r o r ;

whi le ( 1 ){

l e f tMo t o rV e l = g e t mo t o r v e l ( l e f t ) ;r i g h tMoto rVe l = g e t mo t o r v e l ( r i g h t ) ;

i n t e g r a l E r r o r = INTEGRAL GAIN ∗ ( i n t e g r a t e (l e f tMo to rVe l , r i gh tMoto rVe l ,t a r g e t B i a s C l i c k s ) ) ;

l e f tMo t o r E r r o r = PROPORTIONAL GAIN ∗ (t a r g e t V e l C l i c k s − l e f tMo t o rV e l −i n t e g r a l E r r o r ) ;

r i g h tMo t o rE r r o r = PROPORTIONAL GAIN ∗ (t a r g e t V e l C l i c k s − r i g h tMoto rVe l +i n t e g r a l E r r o r ) ;

a l t e r p ow e r ( l e f tMo t o rE r r o r , l e f t ) ;a l t e r p ow e r ( r i gh tMoto rE r r o r , r i g h t ) ;

}}

72

f l o a t g e t mo t o r v e l ( motor motor Index ) // fn{

return ( 10 . 1001 ) ;}

void s e t mo t o r s p e ed ( f l o a t power , motor motor Index ) // fn{

}

f l o a t c h e c k l i m i t ( f l o a t va l , f l o a t low , f l o a t h igh ){

i f ( v a l < low )return ( low ) ;

i f ( v a l > h igh )return ( h igh ) ;

return ( v a l ) ;}

73

A.7 IR Sensor Process

A.7.1 irProcess.h


void run ( void ) ;


void i r u p d a t e t i m e r h a n d l e r ( i n t signum ) ;

f l o a t s c a l e I R ( i n t i n ) ;

A.7.2 irProcess.c

#inc lude #inc lude < s t d i o . h>#inc lude < un i s t d . h>#inc lude < f c n t l . h>#inc lude < e r r n o . h>#inc lude < t e rm i o s . h>#inc lude <commonlib . h>#inc lude < l i n u x /msg . h>#inc lude < l i n u x / i p c . h>#inc lude < s t r i n g . h>#inc lude < s y s / t ime . h>#inc lude < s i g n a l . h>#inc lude < sched . h>#inc lude < s y s / r e s o u r c e . h>

// communicat ions channe l v a l u e s , s e t g l o b a l l y i n i n i tk e y t i r q u e u e k e y ;i n t i r q u e u e i d ;

k e y t s e r i a l R e a d qu e u e k e y ;i n t s e r i a l R e a d q u e u e i d ;

// t ime r v a l u e ss t a t i c s t ruc t i t i m e r v a l t ime r ;s t ruc t t ime v a l t im e r v a l u e s ;

p i d t p i d ;


i n i t ( ) ;

74

run ( ) ;

shut down ( ) ;return (1 ) ;

}


p id = ge t p i d ( ) ;/∗

// s e t the s c h e d u l e r f o r t h i s p r o c e s s to FCFSschedu l e r pa r am . s c h e d p r i o r i t y = 0 ;s c h e d s e t s c h e d u l e r ( p id , SCHED RR,& schedu l e r pa r am ) ;

// ge t the s c h e d u l e r pa ramete r sb a s e p r i o r i t y = g e t p r i o r i t y (PRIO PROCESS , p i d ) ;m a x p r i o r i t y = s c h e d g e t p r i o r i t y m i n ( SCHED RR) ;f o r ( tmp = 0 ; tmp < 100 ; tmp++)

p r i n t f (” P r i o r i t y = %d \ n” , m a x p r i o r i t y ) ;∗/

s e t p r i o r i t y (SCHED OTHER, pid ,−10) ;



i r q u e u e i d = msgget ( i r q u e u e k e y , 0 7 7 7 ) ;i f ( i r q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to message queue :i r p r o c e s s \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 m i rP roc e s s a t t a c h i n g to i r message queueKEY : %d ID : %d \033 [0m \n” , i r q u e u e k e y ,

i r q u e u e i d ) ;

// open IPC comms channe l to s e r i a l p r o c e s s



75


f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to s e r i a l Readmessage queue : i r p r o c e s s \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 m i rP roc e s s a t t a c h i n g to s e r i a l r eadmessage queue KEY : %d ID : %d \033 [0m \n” ,s e r i a l R e a d qu e u e k e y , s e r i a l R e a d q u e u e i d ) ;

// s t a r t the update t ime r and r e g i s t e r the even t hand l e r// r e g i s t e r even t h and l e rs i g n a l (SIGALRM , ∗ i r u p d a t e t i m e r h a n d l e r ) ;// s e t t ime r to 100 ,000 uS = 100mS f o r updatet im e r v a l u e s . t v s e c = 0 ;t im e r v a l u e s . t v u s e c = 100000;t ime r . i t i n t e r v a l = t im e r v a l u e s ;t ime r . i t v a l u e = t im e r v a l u e s ;


}

void run ( void ){

i n t tmp ;k i l l m e s s a g e b u f f k i l l m e s s a g e ;


//we w i l l hang u n t i l l we ge t a k i l l messagetmp = msgrcv ( i r q u e u e i d , ( s t ruc t msgbuf ∗)&

k i l l m e s s a g e , s i z eo f ( k i l l m e s s a g e b u f f ) , KILL , 0 ) ;


p r i n t f ( ” K i l l i n g i r P roce s s . \ n” ) ;break ;

}

}

76

}


}

void i r u p d a t e t i m e r h a n d l e r ( i n t signum ){

r e a d r e q u e s t m e s s a g e b u f f req mq ;i r u p d a t e me s s a g e b u f f i r d a t a ;i r v a l u e m e s s a g e b u f f i r s c a l e ;

req mq . mtype=IR REQUEST ;

msgsnd ( s e r i a l R e a d q u e u e i d , ( s t ruc t msgbuf ∗ )&req mq ,s i z eo f ( r e a d r e q u e s t m e s s a g e b u f f ) , IPC NOWAIT) ;

//hang u n t i l l the s e r i a l r ead i s donemsgrcv ( s e r i a l R e a d q u e u e i d , ( s t ruc t msgbuf ∗)&i r d a t a ,

s i z eo f ( i r u p d a t e me s s a g e b u f f ) , IR RESPONSE , 0 ) ;

i r s c a l e . mtype=IR UPDATE ;

i r s c a l e . i r 0 = s c a l e I R ( i r d a t a . i r 0 ) ;i r s c a l e . i r 1 = s c a l e I R ( i r d a t a . i r 1 ) ;i r s c a l e . i r 2 = s c a l e I R ( i r d a t a . i r 2 ) ;i r s c a l e . i r 3 = s c a l e I R ( i r d a t a . i r 3 ) ;

msgsnd ( i r q u e u e i d , ( s t ruc t msgbuf ∗ )&i r s c a l e , s i z eo f (i r v a l u e m e s s a g e b u f f ) , IPC NOWAIT) ;

}

f l o a t s c a l e I R ( i n t i n ){

i n t i ;f l o a t y ;f l o a t x ;f l o a t n [5 ]={17 .723 , −117 .92 ,290 .43 , −326 .34 ,162 .52} ;

77

x = (5 . 0/255 . 0 ) ∗ ( ( f l o a t ) i n ) ;

y= x∗n [0]+n [ 1 ] ;f o r ( i = 2 ; i < 5 ; i++){

y = y ∗ x + n [ i ] ;}// i f the r e ad i n g i s g r e a t e r then 69 i t s c rap// so make i t b i g g e r , s t a b l e c rap .i f ( y > 69 .0)

y =100.0;return ( y ) ;

}

78

A.8 Sonar Sensor Process

A.8.1 sonarProcess.h


void run ( void ) ;


void p i n g s o n a r t im e r h a n d l e r ( i n t signum ) ;

A.8.2 sonarProcess.c

#inc lude < s ona rP r o c e s s . h>#inc lude < s t d i o . h>#inc lude < un i s t d . h>#inc lude < f c n t l . h>#inc lude < e r r n o . h>#inc lude < t e rm i o s . h>#inc lude <commonlib . h>#inc lude < l i n u x /msg . h>#inc lude < l i n u x / i p c . h>#inc lude < s t r i n g . h>#inc lude < s y s / t ime . h>#inc lude < s i g n a l . h>#inc lude < s y s / i o . h>#inc lude #inc lude < s t r i n g . h>#inc lude < sched . h>#inc lude < s y s / r e s o u r c e . h>#inc lude <math . h>

// communicat ions channe l v a l u e s , s e t g l o b a l l y i n i n i tk e y t s ona r queue k e y ;i n t s o n a r q u e u e i d ;

// t ime r v a l u e ss t a t i c s t ruc t i t i m e r v a l t ime r ;s t ruc t t ime v a l t im e r v a l u e s ;

// p r o c e s s PIDp i d t p i d ;

// s c e d u l e r g l o b a l v a r i a b l e si n t b a s e p r i o r i t y ;i n t max p r i o r i t y ;

// v a r i a b l e to t r a c k which sona r shou l d be p inged

79

boo l wh i ch sona r = TRUE;

i n t i = 0 ;


i n i t ( ) ;

run ( ) ;

shut down ( ) ;

return (1 ) ;}


i n t tmp ;s t ruc t sched param schedu l e r pa r am ;// get the programs PIDp id = ge t p i d ( ) ;

/∗// s e t the s c h e d u l e r f o r t h i s p r o c e s s to FCFSschedu l e r pa r am . s c h e d p r i o r i t y = 0 ;s c h e d s e t s c h e d u l e r ( p id , SCHED RR,& schedu l e r pa r am ) ;

// ge t the s c h e d u l e r pa ramete r sb a s e p r i o r i t y = g e t p r i o r i t y (PRIO PROCESS , p i d ) ;m a x p r i o r i t y = s c h e d g e t p r i o r i t y m i n ( SCHED RR) ;f o r ( tmp = 0 ; tmp < 100 ; tmp++)

p r i n t f (” P r i o r i t y = %d \ n” , m a x p r i o r i t y ) ;∗/

s e t p r i o r i t y (SCHED OTHER, pid ,−10) ;



s o n a r q u e u e i d = msgget ( sona r queue key , 0 7 7 7 ) ;i f ( s o n a r q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to message queue :sona r p r o c e s s \n” ) ;

e x i t (−1) ;}

80

p r i n t f ( ” \033[31 msonarProcess a t t a c h i n g to sona r messagequeue KEY : %d ID : %d \033 [0m \n” , s ona r queue k e y, s o n a r q u e u e i d ) ;

// r e q u e s t IO po r t range f o r sona r ( t h i s i s a hack , andshou ld r e a l l y be r e p l a c e d by a k e r n e l d r i v e r )

tmp = ioperm ( IO BASE ADD ,1 ,O RDWR) ;i f ( tmp < 0){

p r i n t f ( ” e r r o r open ing p a r a l l e l IO po r t f o r r ead /w r i t e : % s \n” , s t r e r r o r ( e r r n o ) ) ;

e x i t (−1) ;}// i n i t i l i z e IO po r toutb (0xDD, IO PORT0) ;

// s t a r t the update t ime r and r e g i s t e r the even t hand l e r// r e g i s t e r even t h and l e rs i g n a l (SIGALRM , ∗ p i n g s o n a r t im e r h a n d l e r ) ;// s e t t ime r to 100 ,000 uS = 100mS f o r updatet im e r v a l u e s . t v s e c = 0 ;t im e r v a l u e s . t v u s e c = 200000;t ime r . i t i n t e r v a l = t im e r v a l u e s ;t ime r . i t v a l u e = t im e r v a l u e s ;


}

void run ( void ){

i n t tmp ;k i l l m e s s a g e b u f f k i l l m e s s a g e ;


// u s l e e p (50000) ;

// have we r e c e i v e d a k i l l message ?// tmp = msgrcv ( s ona r qu eu e i d , ( s t r u c t msgbuf ∗)&

k i l l m e s s a g e , s i z e o f ( k i l l m e s s a g e b u f f ) , KILL , IPC NOWAIT) ;

81

tmp = msgrcv ( s ona r qu eu e i d , ( s t ruc t msgbuf ∗)&k i l l m e s s a g e , s i z eo f ( k i l l m e s s a g e b u f f ) , KILL , 0 ) ;


p r i n t f ( ” K i l l i n g sona r P roce s s . \ n” ) ;break ;

}

}

}


}

void p i n g s o n a r t im e r h a n d l e r ( i n t signum ){

s o n a r upda t e me s s a g e bu f f sona r mes sage ;sona r mes sage . mtype=SONAR UPDATE;

// t ime r v a r i a b l e ss t ruc t t ime v a l c u r r t im e ;unsigned long s t a r t t i m e ;unsigned long c u r r e n t t im e ;unsigned long r e t u r n t im e = 0 ;char p o r t v a l ;boo l b l ank = TRUE;

i n t i =0;

//TODO: add sona r p ing code he r e :

ge t t imeo fday (& cu r r t ime ,NULL) ;s t a r t t i m e = cu r r t im e . t v u s e c ;

i f ( wh i ch sona r ){

// wh i ch sona r = FALSE ;outb (0xDC , IO PORT0) ;

}e l s e{

82

wh i ch sona r = TRUE;outb (0xCD , IO PORT0) ;

}

whi le ( 1 ){

// 0 x22 r e p r e s e n t s mask f o r 0010 0010 w i l l p i c k outecho b i t s

p o r t v a l = inb ( IO PORT0) ;

g e t t imeo fday (& cu r r t ime ,NULL) ;

i f ( ( ! ( p o r t v a l & 0 x02 ) ) | ( ! ( p o r t v a l & 0 x20 ) ) ){

//we have r e c e i v e d a r e t u r nr e t u r n t im e = cu r r t im e . t v u s e c ;break ;

}

c u r r e n t t im e = cu r r t im e . t v u s e c ;// l ook out ove r 10 ’i f ( c u r r e n t t im e >= ( s t a r t t i m e + 10000) )

break ;

}

outb (0xDD, IO PORT0) ;

i f ( r e t u r n t im e != 0)// c a l c u l a t e the sona r range i n cmsona r mes sage . s o n a r r a ng e =(30 .5∗ . 9∗ ( ( r e t u r n t ime−

s t a r t t i m e ) /1000) ) ;

e l s esona r mes sage . s o n a r r a ng e =1000;

// end p ing c y c l e

msgsnd ( s ona r qu eu e i d , ( s t ruc t msgbuf ∗ )&sonar message, s i z eo f ( s o n a r upda t e me s s a g e bu f f ) , IPC NOWAIT) ;

s o n a r upda t e me s s a g e bu f f sona r mes sage ;sona r mes sage . mtype=SONAR UPDATE;

//TODO: add sona r p ing code he r e :

83

sona r mes sage . s o n a r r a ng e=i ++;

msgsnd ( s ona r qu eu e i d , ( s t ruc t msgbuf ∗ )&sonar message, s i z eo f ( s o n a r upda t e me s s a g e bu f f ) , IPC NOWAIT) ;

}

84

A.9 Serial Read Process

A.9.1 serialRead.h


void run ( void ) ;


i n t get quad ( long ∗ qu ad l e f t , long ∗ qu ad r i g h t ) ;

i n t g e t i r ( i n t ∗ i r 0 , i n t ∗ i r 1 , i n t ∗ i r 2 , i n t ∗ i r 3 ) ;

A.9.2 serialRead.c

#inc lude < s e r i a l R e a d . h>#inc lude < s e r i a l C o n t r o l . h>

#inc lude < s t d i o . h>#inc lude < un i s t d . h>#inc lude < f c n t l . h>#inc lude < e r r n o . h>#inc lude < t e rm i o s . h>#inc lude <commonlib . h>#inc lude < l i n u x /msg . h>#inc lude < l i n u x / i p c . h>#inc lude < s t r i n g . h>

// communicat ions channe l v a l u e s , s e t g l o b a l l y i n i n i tk e y t s e r i a l R e a d qu e u e k e y ;i n t s e r i a l R e a d q u e u e i d ;

i n t t t y S 3 f d ;


i n i t ( ) ;

run ( ) ;

shut down ( ) ;

return (1 ) ;}void i n i t ( void )

85

{





f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to message queue :s e r i a l r ead p r o c e s s \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 mse r i a lRead a t t a c h i n g to s e r i a l r eadmessage queue KEY : %d ID : %d \033 [0m \n” ,s e r i a l R e a d qu e u e k e y , s e r i a l R e a d q u e u e i d ) ;

t t y S 3 f d = open po r t ( 3 , O RDWR | O NOCTTY | O NDELAY );

i f ( t t y S 3 f d < 0 ){

f p r i n t f ( s t d e r r , ” E r r o r open ing po r t t t yS3 \n” ) ;

e x i t (−1) ;}

}

void run ( void ){

i n t tmp ;k i l l m e s s a g e b u f f k i l l m e s s a g e ;r e a d r e q u e s t m e s s a g e b u f f r e a d r e q ;i r u p d a t e me s s a g e b u f f i r v a l ;quad mes sage bu f f quad va l ;

// loop u n t i l l we f i n d a k i l l message i n the queue .

86

whi le ( 1 ){

u s l e e p (10000) ;

tmp = msgrcv ( s e r i a l R e a d q u e u e i d , ( s t ruc t msgbuf ∗)&read r eq , s i z eo f ( r e a d r e q u e s t m e s s a g e b u f f ) ,IR REQUEST , IPC NOWAIT) ;

i f ( tmp == s i z eo f ( r e a d r e q u e s t m e s s a g e b u f f ) ){

i r v a l . mtype=IR RESPONSE ;

g e t i r ( & i r v a l . i r 0 , & i r v a l . i r 1 , & i r v a l .i r 2 , & i r v a l . i r 3 ) ;

msgsnd ( s e r i a l R e a d q u e u e i d , ( s t ruc t msgbuf∗ )&i r v a l , s i z eo f ( i r u p d a t e me s s a g e b u f f

) , IPC NOWAIT) ;

}

tmp = msgrcv ( s e r i a l R e a d q u e u e i d , ( s t ruc t msgbuf ∗)&read r eq , s i z eo f ( r e a d r e q u e s t m e s s a g e b u f f ) ,QUAD REQUEST, IPC NOWAIT) ;

i f ( tmp == s i z eo f ( r e a d r e q u e s t m e s s a g e b u f f ) ){//TODO: d e a l w i th quad r e q u e s t message .

quad va l . mtype=QUAD RESPONSE;}

// have we r e c e i v e d a k i l l message ?tmp = msgrcv ( s e r i a l R e a d q u e u e i d , ( s t ruc t msgbuf ∗)&



p r i n t f ( ” K i l l i n g s e r i a l Read Proce s s . \ n” ) ;break ;

}

}

}

87


// c l o s e the open s e r i a l po r tc l o s e p o r t ( t t y S 3 f d ) ;

}

i n t g e t i r ( i n t ∗ i r 0 , i n t ∗ i r 1 , i n t ∗ i r 2 , i n t ∗ i r 3 ){

i n t s i z e o f p a c k e t ;char r e t u r n p a c k e t [ 6 ] ;char command packet [ 9 ] ;

command packet [ 0 ] = 0x10 ; // packe t heade rcommand packet [ 1 ] = 0x01 ;command packet [ 2 ] = 0x30 ; // IR / a−>d add r e s scommand packet [ 3 ] = 0xF0 ; // Return add r e s scommand packet [ 4 ] = 0x00 ; // F i l l w i th checksum l a t e rcommand packet [ 5 ] = 0x01 ; // send ON commandcommand packet [ 6 ] = 0x00 ; // # of by t e s to f o l l o w i n

the packe tcommand packet [ 7 ] = 0 x10 ; // c l o s i n g packe tcommand packet [ 8 ] = 0 x04 ;command packet [ 4 ] = command packet [ 2 ] ˆ command packet

[ 3 ] ˆ command packet [ 5 ] ˆ command packet [ 6 ] ;

s i z e o f p a c k e t=s e r i a l r e a d ( command packet , 9 , t t yS3 fd ,r e t u r n p a c k e t ) ;

∗ i r 0 = ( i n t ) r e t u r n p a c k e t [ 0 ] ;∗ i r 1 = ( i n t ) r e t u r n p a c k e t [ 1 ] ;∗ i r 2 = ( i n t ) r e t u r n p a c k e t [ 2 ] ;∗ i r 3 = ( i n t ) r e t u r n p a c k e t [ 3 ] ;

return (1 ) ;

}

i n t get quad ( long ∗ qu ad l e f t , long ∗ qu ad r i g h t ){

i n t s i z e o f p a c k e t ;char r e t u r n p a c k e t [ 5 ] ;

88

char command packet [ 9 ] ;

command packet [ 0 ] = 0x10 ; // packe t heade rcommand packet [ 1 ] = 0x01 ;command packet [ 2 ] = 0x30 ; // IR / a−>d add r e s scommand packet [ 3 ] = 0xF0 ; // Return add r e s scommand packet [ 4 ] = 0x00 ; // F i l l w i th checksum l a t e rcommand packet [ 5 ] = 0x00 ; // send ON commandcommand packet [ 6 ] = 0x00 ; // # of by t e s to f o l l o w i n

the packe tcommand packet [ 7 ] = 0 x10 ; // c l o s i n g packe tcommand packet [ 8 ] = 0 x04 ;command packet [ 4 ] = command packet [ 2 ] ˆ command packet

[ 3 ] ˆ command packet [ 5 ] ˆ command packet [ 6 ] ;

s i z e o f p a c k e t=s e r i a l r e a d ( command packet , 9 , t t yS3 fd ,r e t u r n p a c k e t ) ;

∗ q u a d l e f t =( long ) ( ( ( long ) r e t u r n p a c k e t [ 0 ] ) ∗0xFF+((long ) r e t u r n p a c k e t [ 1 ] ) ) ;

∗ quad r i g h t =( long ) ( ( ( long ) r e t u r n p a c k e t [ 2 ] ) ∗0xFF+((long ) r e t u r n p a c k e t [ 3 ] ) ) ;

return (1 ) ;

}

89

A.10 Serial Write Process

A.10.1 serialWrite.h


void run ( void ) ;


A.10.2 serialWrite.c

#inc lude < s e r i a l W r i t e . h>#inc lude < s e r i a l C o n t r o l . h>

#inc lude < s t d i o . h>#inc lude < un i s t d . h>#inc lude < f c n t l . h>#inc lude < e r r n o . h>#inc lude < t e rm i o s . h>#inc lude <commonlib . h>#inc lude < l i n u x /msg . h>#inc lude < l i n u x / i p c . h>#inc lude < s t r i n g . h>

// communicat ions channe l v a l u e s , s e t g l o b a l l y i n i n i tk e y t s e r i a lW r i t e q u e u e k e y ;i n t s e r i a l W r i t e q u e u e i d ;

i n t t t y S 2 f d ;


i n i t ( ) ;

run ( ) ;

shut down ( ) ;

return (1 ) ;}


90


s e r i a lW r i t e q u e u e k e y = f t o k ( ”/mqkey/ se r i a lWr i t eMQ ” , ’ a’ ) ;

s e r i a l W r i t e q u e u e i d = msgget ( s e r i a lW r i t e q u e u e k e y, 0 7 7 7 ) ;

i f ( s e r i a l W r i t e q u e u e i d < 0){

f p r i n t f ( s t d e r r , ” E r r o r a t t a c h i n g to message queue :s e r i a l w r i t e p r o c e s s \n” ) ;

e x i t (−1) ;}

p r i n t f ( ” \033[31 ms e r i a lW r i t e a t t a c h i n g to s e r i a l w r i t emessage queue KEY : %d ID : %d \033 [0m \n” ,s e r i a lW r i t e q u e u e k e y , s e r i a l W r i t e q u e u e i d ) ;

t t y S 2 f d = open po r t ( 2 , O RDWR | O NOCTTY | O NDELAY) ;

i f ( t t y S 2 f d < 0 ){

f p r i n t f ( s t d e r r , ” E r r o r open ing po r t t t yS2 \n” ) ;

e x i t (−1) ;}

}

void run ( void ){

i n t tmp ;k i l l m e s s a g e b u f f k i l l m e s s a g e ;s e r i a lW r i t e m e s s a g e b u f f out message ;


u s l e e p (10000) ;

91

tmp = msgrcv ( s e r i a lW r i t e q u e u e i d , ( s t ruc t msgbuf ∗)&out message , s i z eo f ( s e r i a lW r i t e m e s s a g e b u f f ) ,SERIAL WRITE , IPC NOWAIT) ;

i f ( tmp == s i z eo f ( s e r i a lW r i t e m e s s a g e b u f f ) ){

//we have r e c e i v e d a b l o ck o f s e r i a l data tow r i t e to the po r t

//TODO: add the code to hand l e the s e r i a lpacke t

// p r i n t f ( ” w r i t i n g to s e r i a l po r t %s \ n” ,out message . s e r i a l s t r i n g ) ;

tmp = s e r i a l w r i t e ( out message . s e r i a l s t r i n g ,t t y S 2 f d , out message . s i z e ) ;

i f ( tmp < 0){// f p r i n t f ( s t d e r r , ”ERROR WRITING TO / dev/

t tyS2 , a b o r t i n g send attempt : % s \ n” ,s t r e r r o r ( e r r n o ) ) ;

}

}

// have we r e c e i v e d a k i l l message ?tmp = msgrcv ( s e r i a lW r i t e q u e u e i d , ( s t ruc t msgbuf ∗)&


i f ( tmp == s i z eo f ( k i l l m e s s a g e b u f f ) ){

break ;}

}

}


p r i n t f ( ” K i l l i n g s e r i a l W r i t e P roce s s . \ n” ) ;t c f l u s h ( t t yS2 fd , TCIFLUSH) ;c l o s e p o r t ( t t y S 2 f d ) ;

}

92

A.11 Linux version of DOS PC.H

A.11.1 pc.h

void i n i t k e y b o a r d ( ) ;void c l o s e k e y b o a r d ( ) ;i n t kbh i t ( ) ;i n t r eadch ( ) ;

A.11.2 pc.c

#inc lude < s t d i o . h>#inc lude < t e rm i o s . h>#inc lude < term . h>#inc lude < c u r s e s . h>#inc lude < un i s t d . h>

s t a t i c s t ruc t t e rm i o s i n i t i a l s e t t i n g s , n ew s e t t i n g s ;s t a t i c i n t p e e k c h a r a c t e r = −1;void i n i t k e y b o a r d ( ){

t c g e t a t t r (0 ,& i n i t i a l s e t t i n g s ) ;n ew s e t t i n g s = i n i t i a l s e t t i n g s ;n ew s e t t i n g s . c l f l a g &= ˜ICANON;n ew s e t t i n g s . c l f l a g &= ˜ECHO;n ew s e t t i n g s . c l f l a g &= ˜ IS IG ;n ew s e t t i n g s . c c c [VMIN] = 1 ;n ew s e t t i n g s . c c c [VTIME ] = 0 ;t c s e t a t t r ( 0 , TCSANOW, & new s e t t i n g s ) ;

}

void c l o s e k e y b o a r d ( ){

t c s e t a t t r ( 0 , TCSANOW, & i n i t i a l s e t t i n g s ) ;}

i n t kbh i t ( ){

char ch ;i n t nread ;

i f ( p e e k c h a r a c t e r != −1)return 1 ;

n ew s e t t i n g s . c c c [VMIN]=0;t c s e t a t t r ( 0 , TCSANOW, & new s e t t i n g s ) ;n read = read (0 ,&ch , 1 ) ;n ew s e t t i n g s . c c c [VMIN]=1;t c s e t a t t r ( 0 , TCSANOW, & new s e t t i n g s ) ;

93

i f ( nread == 1) {p e e k c h a r a c t e r = ch ;return 1 ;

}return 0 ;

}

i n t r eadch ( ){

char ch ;

i f ( p e e k c h a r a c t e r != −1) {ch = pe e k c h a r a c t e r ;p e e k c h a r a c t e r = −1;return ch ;

}r ead (0 ,&ch , 1 ) ;return ch ;

}

94

B Satellite processor code

B.1 PIC Common Serial Module

B.1.1 aserial.h

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ F i l e : a s e r i a l . h∗ Notes : heade r f i l e f o r a synch ronous s e r i a l

comminucat ions∗ used from Co inca rd work e x p e r i e n c e term∗∗ F i r s t Created : 12/06/00∗ Las t Mod i f i ed : 01/03/01∗ Ve r s i on : 1 . 0∗ Author : JRJacox∗∗ Pla t fo rm : PIC 16 f874 @ 20 Mhz∗ : h i−t e ch comp i l e r V7 . 8 6 P2 wi th MPLab

V5 .20∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

#def ine Fosc 20000000

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Symbol i c Cons tant s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/#def ine ASe r i a l R xBu f f e r S i z e 12#def ine s e r i a l B a n k bank1

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Recept i on s t a t u s b i t i n d i c a t o r s .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/#def ine ASer i a lRxEr ro rS ta tu sOK 0x00#def ine ASe r i a lR xE r r o rS t a t u sOve r r un 0x01#def ine ASe r i a lRxE r r o rS t a t u sF r am ing 0x02#def ine ASe r i a l R xE r r o r S t a t u sBu f f e rOv e r f l ow 0x03#def ine ASe r i a l R xE r r o r S t a t u sUnd e f i n e dC t r l S e q 0 x04/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Con t r o l SequencesEach c o n t r o l s equence c o n s i s t s o f the C t r l S e q I n i t by te

f o l l ow e d by a

95

Ct r l S eqF l a g byte . Each data t r a n s f e r i s begun byp r e c e d i n g i t w i th one

o f s e v e r a l S t a r t c o n t r o l sequences , and each datat r a n s f e r i s ended by

the End c o n t r o l s equence .

A byte o f the same va l u e as C t r l S e q I n i t i n the datat r a n s f e r i s

kept by p r e c e d i n g i t w i th C t r l S e q I n i t .Th i s i s a byte−s t u f f i n g p rocedu r e .

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/

#def ine A S e r i a l C t r l S e q I n i t 0 x10#def ine AS e r i a l C t r l S e q F l a g S t a r t T r a n s f e r 0 x02#def ine ASe r i a l C t r l S e qF l a gS t a r t P a c k e t 0x01#def ine ASe r i a l C t r l S e qF l a gS t a r t I 2CWr i t e 0 x11#def ine ASe r i a l C t r l S e qF l a gS t a r t I 2CRead 0x05#def ine ASe r i a lC t r l S eqF l a gEnd 0x04

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Va r i a b l e s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Func t i on s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/extern void A S e r i a l I n i t ( unsigned long baudRate ) ;extern void ASer i a lRx ISR ( void ) ;extern void ASe r i a l Enab l eRe c ep t i o n ( unsigned char

r x I s E n a b l e d ) ;extern unsigned char AS e r i a l T r a n s f e r I s R e c e i v e d ( unsigned

char ∗ s t a r t F l a g ,unsigned char ∗

dataCount ,unsigned char ∗r x E r r o r S t a t u s ) ;

extern void ASer i a lTx ISR ( void ) ;extern void ASe r i a l Enab l eT r an sm i s s i o n ( unsigned char

t x I sEnab l e d ,unsigned char

s t a r t F l a g ,unsigned char

96

data [ ] ,unsigned char

da t aS i z e ) ;extern unsigned char AS e r i a l T r a n s f e r I s S e n t ( unsigned char ∗

dataCount ) ;

B.1.2 aserial.c

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ F i l e : a s e r i a l . c∗ Notes : s ou r c e f i l e f o r a synch ronous s e r i a l

comminucat ions∗∗∗ F i r s t Created : 30/05/03∗ Las t Mod i f i ed : −−/−−/−−∗ Ve r s i on : 1 . 0∗ Author : JRJacox∗∗ Pla t fo rm : PIC 16 f877 @ 20 Mhz∗ : h i−t e ch comp i l e r V8 . 0 0 PL2 wi th MPLab

V5 .62∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

#inc lude ” a s e r i a l . h”#inc lude 

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Va r i a b l e s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗//∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Recept i on b u f f e r .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/s e r i a l B a n k char ASe r i a l R xBu f f e r [ AS e r i a l R xBu f f e r S i z e ] ;s t a t i c s e r i a l B a n k unsigned char RxDataCount ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Recept i on e r r o r s t a t u s .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/s t a t i c s e r i a l B a n k unsigned char RxEr r o rS t a tu s ;/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Recept i on c o n t r o l s equence s t a t e .

97

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/

s t a t i c s e r i a l B a n k unsigned char RxCont ro lSequence I sBegun ;/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

S t a r t i n g c o n t r o l s equence f l a g .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/s t a t i c s e r i a l B a n k unsigned char RxCt r l S eqF l a gS t a r t ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Func t i on s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗//∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Set the PIC USART module f o r a synch ronous s e r i a l s e r v i c e atthe g i v en

baud r a t e .∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void A S e r i a l I n i t ( unsigned long baudRate ){/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Make su r e no r e c e p t i o n s or t r a n sm i s s i o n s take p l a c e j u s ty e t .

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/TXIE = 0 ;RCIE = 0 ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

TXSTA:CSRC( x ) , TX89 (0) , TXEN(1) , SYNC(0) , −(u ) , BRGH(1) ,

TRMT( r ) , TXD8( x )RCSTA:

SPEN(1) , RC89 (0 ) , SREN( x ) , CREN(1) , −(u ) , FERR( r ) ,OERR( r ) , RCD8( r )

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/TXSTA = 0 x24 ;RCSTA = 0 x90 ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

98

Re f e r to the PIC Data Sheet f o r the baud r a t ec a l c u l a t i o n fo rmu la .

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/SPBRG = ( unsigned char ) ( ( ( unsigned long ) Fosc /

baudRate ) / 1 6 ) − 1;}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

The r e c e p t i o n i n t e r r u p t s e r v i c e r o u t i n e .∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void ASer i a lRx ISR ( void ){

unsigned char r xByte ;unsigned char ha rdwa r eDe t e c t edE r ro r ;unsigned char r xBy t e I sT r an s f e rDa t a ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Find any PIC−de t e c t e d e r r o r s .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/ha rdwa r eDe t e c t edE r ro r = 0 ;i f (OERR == 1)

ha rdwa r eDe t e c t edE r ro r |= ASe r i a lR xE r r o rS t a t u sOve r r un ;i f ( FERR == 1)

ha rdwa r eDe t e c t edE r ro r |= ASe r i a lRxE r r o rS t a t u sF r am ing ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Ex t r a c t the byte from the PIC r e c e p t i o n bu f f e r , andc l e a r any f r am ing

e r r o r .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/r xByte = RCREG;

i f ( ha rdwa r eDe t e c t edE r ro r == ASe r i a lR xE r r o r S t a t u sOve r r un)

{/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

This r e s e t o f r e c e p t i o n l o g i c c l e a r s the ove r r une r r o r .

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/

CREN = 0;

99

CREN = 1;}

RxEr r o rS t a tu s |= hardwa r eDe t e c t edE r ro r ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Detect and hand l e a c o n t r o l s equence .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/r xBy t e I sT r an s f e rDa t a = 0 ;i f ( ! RxCont ro lSequence I sBegun )

i f ( r xByte != A S e r i a l C t r l S e q I n i t )r xBy t e I sT r an s f e rDa t a = 1 ;

e l s eRxCont ro lSequence I sBegun = 1 ;

e l s e{

switch ( rxByte ){

case ASe r i a l C t r l S e qF l a gS t a r t P a c k e t :/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

The data t r a n s f e r has begun .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/RxCt r l S eqF l a gS t a r t = rxByte ;RxE r r o rS t a tu s = ASer i a lRxEr ro rS ta tu sOK ;RxDataCount = 0 ;break ;

case A S e r i a l C t r l S e q I n i t :/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

The data byte to be t r a n s f e r r e d i s the sameva l u e as the

c t r l . seq . i n i t i a t o r .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/r xBy t e I sT r an s f e rDa t a = 1 ;break ;

case ASe r i a lC t r l S eqF l a gEnd :/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

The data t r a n s f e r has ended .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/RCIE = 0 ;

100

break ;

de fau l t :RxE r r o rS t a tu s |=

ASe r i a l R xE r r o r S t a t u sUnd e f i n e dC t r l S e q ;break ;

}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

The c o n t r o l s equence has ended .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/RxCont ro lSequence I sBegun = 0 ;

}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Handle the r e c e i v e d data byte .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/i f ( r xBy t e I sT r an s f e rDa t a ){

i f ( RxDataCount < ASe r i a l R xBu f f e r S i z e )ASe r i a l R xBu f f e r [ RxDataCount ] = rxByte ;

e l s eRxEr r o rS t a tu s |=

ASe r i a l R xE r r o r S t a t u sBu f f e rOv e r f l ow ;RxDataCount++;

}}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Set the e n a b l i n g o f the PIC to r e c e i v e asynch ronous s e r i a ldata t r a n s f e r s .

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

void ASe r i a l Enab l eRe c ep t i o n ( unsigned char r x I s E n a b l e d ){

unsigned char temp ;/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

D i s ab l e i n t e r r u p t s .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/RCIE = 0 ;

i f ( r x I s E n a b l e d )

101

{/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

I n i t i a l i z e the data t r a n s f e r c o n t r o l s .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/RxDataCount = 0 ;RxCont ro lSequence I sBegun = 0 ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

C l e a r the doub l e b u f f e r and any f ram ing e r r o r .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/temp = RCREG;temp = RCREG;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

This r e s e t o f r e c e p t i o n l o g i c c l e a r s any ove r r une r r o r .

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/

CREN = 0;CREN = 1;

RCIE = 1 ;}

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Return a boo l ean i n d i c a t i o n o f the comp l e t i on o f anasynch ronous s e r i a l

data r e c e p t i o n . Also , r e t u r n the f l a g byte o f the l a s ts t a r t i n g c o n t r o l

sequence , the c u r r e n t count o f r e c e i v e d data bytes , and thec u r r e n t

r e c e p t i o n e r r o r s t a t u s .∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/unsigned char AS e r i a l T r a n s f e r I s R e c e i v e d ( unsigned char ∗

s t a r t F l a g ,unsigned char ∗ dataCount , unsigned char ∗

r x E r r o r S t a t u s ){

unsigned char r e tV a l ;

r e tV a l = ( unsigned char ) ! RCIE ;

102

∗ s t a r t F l a g = RxCt r l S eqF l a gS t a r t ;∗ r x E r r o r S t a t u s = RxE r r o rS t a tu s ;∗dataCount = RxDataCount ;

return r e tV a l ;}

103

B.2 PIC PWM ISR

B.2.1 aserial.h

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ F i l e : i s r . h∗ Notes : heade r f i l e f o r low l e v e l I2C MSSP c o n t r o l s∗∗∗ F i r s t Created : 30/05/03∗ Las t Mod i f i ed : −−/−−/−−∗ Ve r s i on : 1 . 0∗ Author : JRJacox∗∗ Pla t fo rm : PIC 16 f877 @ 20 Mhz∗ : h i−t e ch comp i l e r V8 . 0 0 P2 wi th MPLab

V5 .62∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Func t i on s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/extern void I S RD i s a b l e G l o b a l I n t e r r u p t s ( void ) ;extern void I SREn ab l eG l o b a l I n t e r r u p t s ( void ) ;extern void I S R D i s a b l e P e r i p h e r a l I n t e r r u p t s ( void ) ;extern void I S RE n a b l e P e r i p h e r a l I n t e r r u p t s ( void ) ;

B.2.2 aserial.c

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ F i l e : i s r . c∗ Notes : s ou r c e f i l e f o r low l e v e l I2C MSSP c o n t r o l s∗∗∗ F i r s t Created : 30/05/03∗ Las t Mod i f i ed : −−/−−/−−∗ Ve r s i on : 1 . 0∗ Author : JRJacox∗∗ Pla t fo rm : PIC 16 f877 @ 20 Mhz∗ : h i−t e ch comp i l e r V8 . 0 0 P2 wi th MPLab

V5 .62∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

#inc lude ” i s r . h”#inc lude ” a s e r i a l . h”#inc lude 

104

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Func t i on s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗//∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Ensure tha t a l l PIC i n t e r r u p t s a r e d i s a b l e d .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I S RD i s a b l e G l o b a l I n t e r r u p t s ( void ){

doGIE = 0 ;

whi le ( GIE == 1) ;}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Al low a l l PIC i n t e r r u p t s to be enab l ed .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I SREn ab l eG l o b a l I n t e r r u p t s ( void ){

GIE = 1 ;}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Ensure tha t P e r i p h e r a l i n t e r r u p t s a r e d i s a b l e d .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I S R D i s a b l e P e r i p h e r a l I n t e r r u p t s ( void ){

doPEIE = 0 ;

whi le ( PEIE == 1) ;}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Al low P e r i p h e r a l i n t e r r u p t s to be enab l ed .

105

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/

void I S RE n a b l e P e r i p h e r a l I n t e r r u p t s ( void ){

PEIE = 1 ;}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

ISP∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void i n t e r r u p t ISP ( void ){

i f ( RCIE==1 && RCIF==1){

ASer i a lRx ISR ( ) ;}

}

106

B.3 PIC PWM

B.3.1 aserial.h

// pwm. h#inc lude ” j j t y p e . h”//#i n c l u d e ” a s e r i a l . h”

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Va r i a b l e s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/extern bank1 BYTE ASe r i a l R xBu f f e r [ ] ; // I /P b u f f e r f o r

s e r i a l comms

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/char pwmStatus ;// b7 − Thermal Senso r 1// b6 − Thermal Senso r 0// b5// b4// b3// b2// b1// b0

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/// S e r i a l Data I n f o rma t i o n//BYTE rxDataS i z e ;char t xRep l yAdd r e s s ;char rxCommand ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Cons tant s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/#def ine PWMRS232ADDRESS 0x20

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Port D e f i n i t i o n s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/// Port B

107

// B0 − Brake 0 O/P// B1 − D i r e c t i o n 0 O/P// B2 − TFL0 I /P// B3 −// B4 − Brake 1 O/P// B5 − D i r e c t i o n 1 O/P// B6 − TFL1 I /P// B7 −#def ine BRAKE0 RB0#def ine DIR0 RB5 // RB1#def ine BRAKE1 RB4#def ine DIR1 RB1 // RB5

#def ine THERMALMASK0 0x04#def ine THERMALMASK1 0x40

#def ine PWMPORT PORTB#def ine PWMTRIS TRISB

#def ine PWMDEFAULTTRIS 0xCC#def ine PWMDEFAULTDATA 0x00

// Port C// C0 −// C1 − PWM0 O/P// C2 − PWM1 O/P// C3 −// C4 −// C5 −// C6 −// C7 −

#def ine PORTCDEFAULTDATA 0x00#def ine PORTCDEFAULTTRIS 0xC0

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Macro D e f i n i t i o n s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/#def ine BITSET( l v a l , b i t ) l v a l |=(1<<( b i t ) )#def ine BITNOT( l v a l , b i t ) l v a l ˆ=(1<<( b i t ) )#def ine BITCLR( l v a l , b i t ) l v a l &=˜(1<<( b i t ) )#def ine BITTST( var , b i t ) ( ( va r )&(1<<( b i t ) ) )

#def ine NULL 0x00

108

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on P ro to t ype s − p a r t i a l l i s t as needed to comp i l e∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void RS232 SendByte ( char ) ;

B.3.2 aserial.c

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ F i l e : pwm. c∗ Notes : s ou r c e f i l e f o r PWM motor c o n t r o l l e r∗∗∗ F i r s t Created : 30/05/03∗ Las t Mod i f i ed : −−/−−/−−∗ Ve r s i on : 1 . 0∗ Author : JRJacox∗∗ Pla t fo rm : PIC 16 f877 @ 20 Mhz∗ : h i−t e ch comp i l e r V8 . 0 0 PL2 wi th MPLab

V5 .62∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

#inc lude #inc lude ”pwm. h”#inc lude ” i s r . h”#inc lude ” a s e r i a l . h”

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗∗ S e r i a l C o m m a n d s f r o m H o s t∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : pwmOff ( )Notes : K i l l s pwm to both motors ; does not d i s a b l e PWM

module , j u s t s e t s 0 duty∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/

void pwmOff ( void )

109

{CCP1CON = 0 x0 f ;CCPR1L = 0 x00 ;

CCP2CON = 0 x0 f ;CCPR2L = 0 x00 ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : pwmOn( d i r 1 , speed1 , d i r 2 , speed2 )Notes : Se t s d i r e c t i o n and duty f o r each motor

− assuming 8− b i t v a l u e f o r duty andd ropp ing l owe r 2 b i t s . Can modi fy i f

g r e a t e r r e s o l u t i o n i s r e q u i r e d .∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/

void pwmOn( i n t d i r 1 , i n t speed1 , i n t d i r 2 , i n t speed2 ){

DIR0 = ! d i r 1 ; // R ight Motor i smounted backwards and has n e g a t i v e l o g i c

CCP2CON = 0 x0 f ; // PWM 0 and PWM 1are r e v e r s e d on PCB by a c c i d e n t

CCPR2L = speed1 ;

DIR1 = d i r 2 ;CCP1CON = 0 x0 f ;CCPR1L = speed2 ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : pwmReadStatus ( )Notes : Sends motor s t a t u s to hos t − w i l l wrap wi th message

heade r i f need be∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/

void pwmReadStatus ( void ){

i f (PWMPORT & THERMALMASK1)pwmStatus |= 0 x80 ;

i f (PWMPORT & THERMALMASK0)

110

pwmStatus |= 0 x40 ;

RS232 SendByte ( pwmStatus ) ; // s t a t u sRS232 SendByte (0 x0d ) ; // CRpwmStatus = 0 x00 ; // c l e a r

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : pwmEBrake ( )Notes : Emergency Brake − k i l l s both motors i n s t a n t l y∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/

void pwmEBrake ( i n t s t ){

i f ( s t ) // brake on{

BRAKE0 = 1 ;BRAKE1 = 1 ;pwmOff ( ) ;

}e l s e // brake o f f{

BRAKE0 = 0 ;BRAKE1 = 0 ;

}}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗∗ S e r i a l P r o t o c o l C o n t r o l l e r∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Tran sm i tS e r i a lDa t a−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗//∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d XmitWait ( vo i d ) ;Notes : d e l a y r o u t i n e f o r s e r i a l t r a n sm i s s i o n . Use between

s end i ng c h a r a c t e r s

111

on s e r i a l po r t .∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void XmitWait ( void ){

whi le ( ! TXIF ) // wa i t f o r empty xmit b u f f e r{}

}/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d RS232 SendByte ( cha r ) ;Notes : s ends a byte to the s e r i a l po r t .∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void RS232 SendByte ( char data ){/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Send s i n g l e −byte to s e r i a l po r t−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/TXIE = 0 ;TXEN = 1;

TXREG = data ;

whi le ( ! BITTST(TXSTA, 1 ) ){

}

}/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d RS232ReceiveMessage ( vo i d )checks f o r incoming message packe t

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

BYTE RS232ReceiveMessage ( void ){BYTE r xE r r o r S t a t u s ;BYTE s t a r t F l a g ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Check f o r i n v a l i d s i t u a t i o n s

112

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/i f ( ! A S e r i a l T r a n s f e r I s R e c e i v e d (& s t a r t F l a g , NULL, &

r xE r r o r S t a t u s ) ){

return 0 ;}

i f ( s t a r t F l a g != ASe r i a l C t r l S e qF l a gS t a r t P a c k e t ){

ASe r i a l Enab l eRe c ep t i o n (1 ) ;return 0 ;

}

i f ( r x E r r o r S t a t u s != ASer i a lRxEr ro rS ta tu sOK ){


}

return 1 ;}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d RS232Val idateMessage ( vo i d )v a l i d a t e s incoming message packe t

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

BYTE RS232Val idateMessage ( void ){

i n t i ;i n t ck=0;

i f ( ASe r i a l R xBu f f e r [ 0 ] ! = PWMRS232ADDRESS)// check f o r matching add r e s s

return ( 0 ) ;

ck = ASe r i a l R xBu f f e r [ 0 ] ˆ ASe r i a l R xBu f f e r [ 1 ] ;// check checksum

f o r ( i =0; i < ASe r i a l R xBu f f e r [ 4 ] + 2 ; i++){

ck = ck ˆ ASe r i a l R xBu f f e r [3+ i ] ;}i f ( ck != ASe r i a l R xBu f f e r [ 2 ] )

return ( 0 ) ;

t xRep l yAdd r e s s = ASe r i a l R xBu f f e r [ 1 ] ;// v a l i d packe t − s t r i p i n f o

113

rxCommand = ASe r i a l R xBu f f e r [ 3 ] ;return ( 1 ) ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d i n i t s y s t em ( vo i d )i n i t system

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

void I n i t S y s t em ( void ){

PWMPORT = PWMDEFAULTDATA;// c l e a r a l l

p o r t b output r eg .PWMTRIS = PWMDEFAULTTRIS;

// se tupd i r e c t i o n and brake p i n s

RBPU = 1;

// weak pu l l−up f o r the rma l s e n s o r

PORTC = PORTCDEFAULTDATA;TRISC = PORTCDEFAULTTRIS ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−I n i t a l l PWM paramete r s−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−∗/

T2CON = 0 x00 ; // PWM Pr e s c a l e = 0PR2 = 0xFF ; // PWM Per i od

// PWM Freq = 1 / [ ( PR2+1)∗4∗Tosc∗ P r e s c a l e] = 19 . 5 3 kHz

CCP1CON = 0 x0 f ; // Set 0 duty− ch0CCPR1L = 0 x00 ; // Set 0 duty− ch0CCP2CON = 0 x0 f ;CCPR2L = 0 x00 ;T2CON |= 0 x04 ; // pwm on

// A S e r i a l I n i t (57600) ;// i n i t s e r i a l comms

114

A S e r i a l I n i t (19200) ;// use f o r s e r i a l t e s t e r

I SREn ab l eG l o b a l I n t e r r u p t s ( ) ;I SRE n a b l e P e r i p h e r a l I n t e r r u p t s ( ) ;

pwmStatus = 0 ; // c l e a r temp s e n s o r r e a d i n g

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗∗∗∗∗ M A I N∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

void main ( ){

I n i t S y s t em ( ) ;

// RS232 SendByte (0 x01 ) ; // TEST − send 0 x01on boot

ASe r i a l Enab l eRe c ep t i o n (1 ) ;pwmOff ( ) ;

whi le ( 1 ){

CLRWDT() ;

i f ( RS232ReceiveMessage ( ) )// f u l l packe t has been r e c e i v e d

{

i f ( RS232Val idateMessage ( ) )// packe t has no e r r o r s

an i s add r e s s ed f o r t h i s module{

switch ( rxCommand ){

case ( 0 x00 ) :{

pwmOff ( ) ;break ;

}case ( 0 x01 ) :

115

{pwmOn( ASe r i a l R xBu f f e r

[ 5 ] , AS e r i a l R xBu f f e r[ 6 ] , AS e r i a l R xBu f f e r[ 7 ] ,

AS e r i a l R xBu f f e r[ 8 ] );

break ;}case ( 0 x02 ) :{

pwmReadStatus( ) ;


pwmEBrake ( ASe r i a l R xBu f f e r[ 5 ] ) ;

break ;}

de fau l t :break ;

} // sw} // i f v a l i d message

ASe r i a l Enab l eRe c ep t i o n (1 ) ;// e n ab l i n g r e s e t s

b u f f e r s} // i f

} // wh i l e

} // main

116

B.4 PIC A/D ISR

B.4.1 aserial.h

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ F i l e : i s r . h∗ Notes : heade r f i l e f o r low l e v e l I2C MSSP c o n t r o l s∗∗∗ F i r s t Created : 30/05/03∗ Las t Mod i f i ed : −−/−−/−−∗ Ve r s i on : 1 . 0∗ Author : JRJacox∗∗ Pla t fo rm : PIC 16 f877 @ 20 Mhz∗ : h i−t e ch comp i l e r V8 . 0 0 P2 wi th MPLab

V5 .62∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Func t i on s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/extern void I S RD i s a b l e G l o b a l I n t e r r u p t s ( void ) ;extern void I SREn ab l eG l o b a l I n t e r r u p t s ( void ) ;extern void I S R D i s a b l e P e r i p h e r a l I n t e r r u p t s ( void ) ;extern void I S RE n a b l e P e r i p h e r a l I n t e r r u p t s ( void ) ;extern void I SRD i s a b l eC a p t u r e I n t e r r u p t s ( void ) ;extern void I SREnab l eCap t u r e I n t e r r u p t s ( void ) ;

B.4.2 aserial.c

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ F i l e : i s r . c∗ Notes : s ou r c e f i l e f o r low l e v e l I2C MSSP c o n t r o l s∗∗∗ F i r s t Created : 30/05/03∗ Las t Mod i f i ed : −−/−−/−−∗ Ve r s i on : 1 . 0∗ Author : JRJacox∗∗ Pla t fo rm : PIC 16 f877 @ 20 Mhz∗ : h i−t e ch comp i l e r V8 . 0 0 P2 wi th MPLab

V5 .62∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

117

#inc lude ” i s r . h”#i n c l u d e ” a s e r i a l . h”#inc lude ”quad . h”#inc lude 

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Func t i on s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗//∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Ensure tha t a l l PIC i n t e r r u p t s a r e d i s a b l e d .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I S RD i s a b l e G l o b a l I n t e r r u p t s ( void ){

doGIE = 0 ;

whi le ( GIE == 1) ;}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Al low a l l PIC i n t e r r u p t s to be enab l ed .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I SREn ab l eG l o b a l I n t e r r u p t s ( void ){

GIE = 1 ;}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Ensure tha t P e r i p h e r a l i n t e r r u p t s a r e d i s a b l e d .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I S R D i s a b l e P e r i p h e r a l I n t e r r u p t s ( void ){

doPEIE = 0 ;

whi le ( PEIE == 1) ;}

118

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Al low P e r i p h e r a l i n t e r r u p t s to be enab l ed .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I S RE n a b l e P e r i p h e r a l I n t e r r u p t s ( void ){

PEIE = 1 ;}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Ensure tha t Capture i n t e r r u p t s a r e d i s a b l e d .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I SRD i s a b l eC a p t u r e I n t e r r u p t s ( void ){

doCCP1IE = 0 ;

whi le ( CCP1IE == 1) ;

doCCP2IE = 0 ;

whi le ( CCP2IE == 1) ;}

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Al low Capture i n t e r r u p t s to be enab l ed .−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/void I SREnab l eCap t u r e I n t e r r u p t s ( void ){

CCP1IF = 0 ;CCP1IE = 1 ;CCP2IF = 0 ;CCP2IE = 1 ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

ISP∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void i n t e r r u p t ISP ( void ){

119

i f ( RCIE==1 && RCIF==1){

ASer i a lRx ISR ( ) ;}i f ( CCP1IE==1 && CCP1IF==1){

CCPISR ( ) ;}

}

120

B.5 PIC A/D

B.5.1 aserial.h

// quad . h

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Va r i a b l e s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/extern bank1 char ASe r i a l R xBu f f e r [ ] ; // I /P b u f f e r f o r

s e r i a l commsextern unsigned i n t counte r1a ;extern unsigned i n t counte r1b ;extern unsigned i n t counte r2a ;extern unsigned i n t counte r2b ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Cons tant s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/#def ine QUADRS232ADDRESS 0x30

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Port D e f i n i t i o n s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/// Port A// A0 − AD0 I /P// A1 − AD1 I /P// A2 − AD2 I /P// A3 − AD3 I /P// A4 −// A5 − AD4 I /P

#def ine PORTADEFAULTTRIS 0xFF#def ine PORTADEFAULTDATA 0x00

// Port B// B0 −// B1 − Quad1b I /P// B2 − Quad2b I /P

121

// B3 −// B4 −// B5 −// B6 −// B7 −

#def ine PORTBDEFAULTTRIS 0xFF#def ine PORTBDEFAULTDATA 0x00

// Port C// C0 −// C1 − CCP2 − Quad2a I /P// C2 − CCP1 − Quad1a I /P// C3 −// C4 −// C5 −// C6 − Tx// C7 − Rx

#def ine PORTCDEFAULTTRIS 0xFF#def ine PORTCDEFAULTDATA 0x00

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Macro D e f i n i t i o n s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/#def ine BITSET( l v a l , b i t ) l v a l |=(1<<( b i t ) )#def ine BITNOT( l v a l , b i t ) l v a l ˆ=(1<<( b i t ) )#def ine BITCLR( l v a l , b i t ) l v a l &=˜(1<<( b i t ) )#def ine BITTST( var , b i t ) ( ( va r )&(1<<( b i t ) ) )

#def ine NULL 0x00

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on P ro to t ype s − p a r t i a l l i s t as needed to comp i l e∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void RS232 SendByte ( char ) ;void CCPISR( void ) ;

B.5.2 aserial.c

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ F i l e : quad . c∗ Notes : s ou r c e f i l e f o r quad ra tu r e coun t e r and ana logue∗

122

∗∗ F i r s t Created : 12/06/03∗ Las t Mod i f i ed : −−/−−/−−∗ Ve r s i on : 1 . 0∗ Author : JRJacox∗∗ Pla t fo rm : PIC 16 f876 @ 20 Mhz∗ : h i−t e ch comp i l e r V8 . 0 0 PL2 wi th MPLab

V5 .62∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

#inc lude #inc lude ”quad . h”#inc lude ” i s r . h”#inc lude ” a s e r i a l . h”

//#d e f i n e FINAL 1 // uncomment f o r p r oduc t i o n code(57 kbps s e r i a l )

// comment f o r demons t r a t i on code(19 kbps s e r i a l )

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

G loba l V a r i a b l e s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/char t xRep l yAdd r e s s ;char rxCommand ;char a d c t r l ;

char adData [ 5 ] ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗ S e r i a l C o m m a n d s f r o m H o s t∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on :Notes :

123

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

void GetQuadStatus ( void ){

unsigned i n t c1a ;unsigned i n t c1b ;unsigned i n t c2a ;unsigned i n t c2b ;unsigned long ac1temp ;unsigned long ac2temp ;

c1a = counte r1a ;c1b = counte r1b ;c2a = counte r2a ;c2b = counte r2b ;

i f ( c1a >= c1b ){

ac1temp = ( unsigned long ) c1a − ( unsignedlong ) c1b ;

}e l s e{

ac1temp = ( unsigned long ) c1a − ( unsignedlong ) c1b + 0 x100 ;

}

i f ( c2a >= c2b ){

ac2temp = ( unsigned long ) c2a − ( unsignedlong ) c2b ;

}e l s e{

ac2temp = ( unsigned long ) c2a − ( unsignedlong ) c2b + 0 x100 ;

}

RS232 SendByte ( ac1temp>>8) ;RS232 SendByte ( ac1temp ) ;RS232 SendByte ( ac2temp>>8) ;RS232 SendByte ( ac2temp ) ;RS232 SendByte (13) ; // CR

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

124

Funct i on :Notes :∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void GetADStatus ( i n t channe l ){

char i ;

f o r ( i =0; i < 5 ; i++){

RS232 SendByte ( adData [ i ] ) ;}RS232 SendByte (13) ; // CR

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on :Notes :∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void GetADValues ( void ){

adData [ ( a d c t r l&0x07 ) ] = (ADRESH) ; // d ropp ing l owe r2 b i t s f o r now

// inc r ement channe l coun t e ri f ( a d c t r l == 0x04 ){

a d c t r l = 0 ;}e l s e{

a d c t r l ++;}

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗∗ S e r i a l P r o t o c o l C o n t r o l l e r∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

125

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Tran sm i tS e r i a lDa t a−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗//∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d XmitWait ( vo i d ) ;Notes : d e l a y r o u t i n e f o r s e r i a l t r a n sm i s s i o n . Use between

s end i ng cha r son s e r i a l po r t .

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

void XmitWait ( void ){

whi le ( ! TXIF ) // wa i t f o r empty xmit b u f f e r{}

}/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d RS232 SendByte ( cha r ) ;Notes : s ends a byte to the s e r i a l po r t .∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

∗/void RS232 SendByte ( char data ){/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Send s i n g l e −byte to s e r i a l po r t−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/TXIE = 0 ;TXEN = 1;

TXREG = data ;

whi le ( ! BITTST(TXSTA, 1 ) ){

}

}/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d RS232ReceiveMessage ( vo i d )checks f o r incoming message packe t

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

126

unsigned char RS232ReceiveMessage ( void ){unsigned char r x E r r o r S t a t u s ;unsigned char s t a r t F l a g ;

/∗−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

Check f o r i n v a l i d s i t u a t i o n s−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

∗/i f ( ! A S e r i a l T r a n s f e r I s R e c e i v e d (& s t a r t F l a g , NULL, &

r xE r r o r S t a t u s ) ){

return 0 ;}

i f ( s t a r t F l a g != ASe r i a l C t r l S e qF l a gS t a r t P a c k e t ){


}

i f ( r x E r r o r S t a t u s != ASer i a lRxEr ro rS ta tu sOK ){


}

return 1 ;}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d RS232Val idateMessage ( vo i d )v a l i d a t e s incoming message packe t

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

unsigned char RS232Val idateMessage ( void ){

i n t i ;i n t ck=0;

i f ( ASe r i a l R xBu f f e r [ 0 ] ! = QUADRS232ADDRESS) //check f o r match addr

return ( 0 ) ;

ck = ASe r i a l R xBu f f e r [ 0 ] ˆ ASe r i a l R xBu f f e r [ 1 ] ; //check checksum

127

f o r ( i =0; i < ASe r i a l R xBu f f e r [ 4 ] + 2 ; i++){

ck = ck ˆ ASe r i a l R xBu f f e r [3+ i ] ;}i f ( ck != ASe r i a l R xBu f f e r [ 2 ] )

return ( 0 ) ;

t xRep l yAdd r e s s = ASe r i a l R xBu f f e r [ 1 ] ; // v a l i dpacke t − s t r i p i n f o

rxCommand = ASe r i a l R xBu f f e r [ 3 ] ;return ( 1 ) ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

Funct i on : vo i d i n i t s y s t em ( vo i d )i n i t system

∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

void I n i t S y s t em ( void ){

RBPU = 1;

PORTA = PORTBDEFAULTTRIS ; // a l l IPf o r A/D

PORTB = PORTBDEFAULTDATA; //seconda ry Quadrature IP

PORTB = PORTBDEFAULTTRIS ;PORTC = PORTCDEFAULTDATA; // p r imary

Quadrature IPPORTC = PORTCDEFAULTTRIS ;

CCP1CON = 0 x05 ; // cap tu r e e v e r yPGT ( no s c a l i n g )

ADCON1 = 0 x00 ; // a l l ana log IP ( Vre f+ =Vcc , Vref − = Vss )

#i f d e f FINALA S e r i a l I n i t (57600) ; // i n i t s e r i a l

comms#el se

A S e r i a l I n i t (19200) ; // use f o r s e r i a lt e s t e r

#end i f

128

I SREn ab l eG l o b a l I n t e r r u p t s ( ) ;I SRE n a b l e P e r i p h e r a l I n t e r r u p t s ( ) ;I SREnab l eCap t u r e I n t e r r u p t s ( ) ;

a d c t r l = 0 x81 ; // s e t AD c o n t r o l l e r tos t a r t a t ch 1

// c l e a r c ap tu r e c oun t e r scounte r1a = 0 ;counte r1b = 0 ;counte r2a = 0 ;counte r2b = 0 ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗∗∗∗∗ M A I N∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

void main ( ){

char adde l a y = 0 ;

I n i t S y s t em ( ) ;

// RS232 SendByte (0 x02 ) ; // TEST − send byteon boot

ASe r i a l Enab l eRe c ep t i o n (1 ) ;T1CON = 0 x01 ; // tu rn t ime r 1 on

f o r c ap tu r e

whi le ( 1 ){

CLRWDT() ;

i f ( a d c t r l & 0 x20 ) // wa i t i n g to s t a r tnew a c q u i s i t i o n

{i f ( adde l a y ++ > 50) // too long

/ s h o r t d e l a y ?{

129

adde l a y = 0 ;a d c t r l &= 0 x0 f ; // c l e a r

s t a r t u p f l a gADCON0 = (0 x81 | ( a d c t r l

<< 3) ) ;// tu rn on AD on p a r t i c u l a r

channe la d c t r l |= 0 x40 ;// s e t a c q u i i t i o n d e l a y

f l a g}

}

i f ( a d c t r l & 0 x40 ) //wa i t i n g f o r a c q u i s i t i o n to f i n i s h

{i f ( adde l a y ++ > 25) //

too long / s h o r t d e l a y ?{

adde l a y = 0 ; //r e s e t d e l a y coun t e r f o rnex t t ime

a d c t r l &= 0 x0 f ; //c l e a r acq d e l a y f l a g

ADCON0 |= 0 x04 ; //s t a r t o c n v e r s i o n

a d c t r l |= 0 x80 ; //s e t f l a g to wa i t f o rr e s u l t

}}

i f ( (ADCON0 & 0 x04 ) == 0 && ( a d c t r l & 0 x80 )) // c on v e r s i o n not i n p r o g r e s s and we ’ r ewa i t i n g

{a d c t r l &= 0 x0 f ;

// c l e a r wa i t f o r r e s u l t f l a gGetADValues ( ) ;

// get AD va l u e sa d c t r l |= 0 x20 ;

// s e t new a c q u i s i t i o n f l a g}

130

i f ( RS232ReceiveMessage ( ) )// f u l l packe t has been r e c e i v e d

{i f ( RS232Val idateMessage ( ) )

// packe t has no e r r o r san i s add r e s s ed f o r t h i s module

{switch ( rxCommand ){

case ( 0 x00 ) :{

GetQuadStatus( ) ;


GetADStatus (ASe r i a l R xBu f f e r[ 5 ] ) ;

break ;}

de fau l t :break ;

} // sw} // i f v a l i d message

ASe r i a l Enab l eRe c ep t i o n (1 ) ;// e n ab l i n g r e s e t s

b u f f e r s} // i f

} // wh i l e

} // main

131

B.6 PIC Capture

B.6.1 aserial.h

extern void CCPISR( void ) ;

B.6.2 aserial.c

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

/∗ ∗∗∗∗ C C P MODULE∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/

#inc lude 

unsigned i n t counte r1a ;unsigned i n t counte r1b ;unsigned i n t counte r2a ;unsigned i n t counte r2b ;

void CCPISR( void ){

i f ( CCP1IF ){

CCP1IF = 0 ; // c l e a r i n t f l a g

counte r1b = counte r1a ;counte r1a = ((CCPR1H << 8) | CCPR1L) ;

}

i f ( CCP2IF ){

CCP2IF = 0 ; // c l e a r i n t f l a g

counte r2b = counte r2a ;counte r2a = ((CCPR2H << 8) | CCPR2L) ;

}}

132

C Schematics

133

C.1 PC Power Supply Schematics

Not included with online version of documentation, please contact the authors forschematics.

134

C.2 Sonar Power Supply Schematics


135

C.3 Sonar Daughter Board Schematics


136

C.4 Satellite processor Schematics


137

C.5 Motor Power Amplifier Schematics


138

D Mechanical Drawings

139

D.1 Overall Platform drawings

140

D.2 Chassis bottom drawings

141

D.3 Motor busing drawings

142

D.4 Motor mount drawings

143

D.5 Octagon edge drawings

144

D.6 Top octagon drawings

145

E progress report 1

146

E.1 Purpose

The intent of this project is to develop a low cost robotics research platform suitablefor future research into autonomous control algorithms. This initial development willalso include a basic control algorithm to allow the platform to traverse the engineeringbuilding in a random fashion, avoiding obstacles as it goes.The platform itself will be designed in such a way as to make later modifications oradditions simpler. This means modularity needs to be the primary concern in themechanical, electrical, and software designs.This platform needs to be developed in a low cost way as it is being funded by thedevelopment team. This means that custom hardware will be used wherever possibleto attempt to keep the total project cost down.

147

E.2 Progress

E.2.1 CPU Board

The CPU board acts as the main control unit for the robotic platform and will beresponsible for control of all operational facets of the platform, as well as logging testdata. Both team members will be working on the remaining tasks relating to the CPUboard and its mounting.

E.2.1.1 Operating System

The operating system that has been installed on the PC/104 board is a custom compiledversion of Linux. This custom install is based on the Gentoo Linux distribution, whichis a system that allows for complete recompiling of all kernel components. This abilityto recompile all components allows for complete optimization for the target platform.This optimization is very important in a real time system such as this platform, as itallows the control system to respond to stimuli with minimal lag time.The Linux kernel being employed in this system is the 2.4.20 base kernel, with no patchset installed. This kernel was chosen as it is the first kernel branch to incorporate apreemptive scheduler, this will help allow the control algorithm to respond more quicklyto external stimuli.The kernel and all related tools have been installed and are operational on the PC/104board. There were some minor configuration issues with the onboard network card,but these have all been worked out, development of the software components can nowbegin.

E.2.1.2 Hardware

The PC/104 board and attached hard drive have been mounted in a large aluminiumcase that will allow for :

• proper air flow.

• good electrical noise isolation.

• adequate protection of the electrical components.

• sufficient room for future expansion.

Figure 13 shows the processor board and hard drive mounted in the case.There are still several tasks that need to be completed regarding the cpu board and itshousing. These tasks include :

• mount CPU cooling fan.

• machine mounting holes for serial and parallel ports.

• mount power connector.

• possibly mount power supply inside the case.

These tasks will be completed after the chassis has been completely assembled or, de-pending on availability of the machine shop, other methods for completing this assemblywill be found.

148

Figure 13: CPU and hard drive mounted in case.

E.2.1.3 Electrical

There are still several tasks that need to be considered regarding the electrical needsof the CPU board. A suitable power supply needs to be purchased or designed. Thispower supply needs to supply a current of 4 amps at a voltage of +5V (±5%). Thisrequirement on the power supply excludes using a linear supply as the power dissipationwould be too great:Assuming a worst case battery voltage of +14V.

P = IV

P = 4A · (14V − 5V )P = 36W

We would need to dissipate 36W of power in the liner regulator, so for this application ahigh efficiency switching regulator. National semiconductors produces a line of “simpleswitcher” regulators that require few external components to design such a powersupply. The task of designing this power supply will occur after the platform has beenassembled and tested, as code development can begin without it by simply using astandard PC power supply.

E.2.2 Sensor Network

Jordan will be responsible for implementing the sensor interface, and this will occurafter the platform has been constructed.

E.2.2.1 Sonar sensors

The sonar sensors that will be used on the platform are polaroid ranging type sonarsensors. These sensing modules consists of an instrument grade sonar transducer and a

149

processor board and are able to determine the approximate distance to an object withina 20o arc extending from the center of the transducer at a range between 6 inches and35 feet. These sonar sensors will be mounted in the front of the unit in such a wayas to slightly overlap the main lobes of sensitivity, this should result in sonar coverageextending out in a 40o arc from the front of the platform. Care will be needed toassure that a late return from one modules transmit cycle will not look like a targetto the second module. These modules have already been purchased and are ready formounting.The sonar modules require a mounting to be constructed that will rigidly hold thetransducers in the required position as well as to house the receiver modules and relatedpower supplies.Software drivers will be needed to allow for easy device independent interfacing to thesonar modules. The PC/104 board is equipped with 48 digital IO lines that will makethe interfacing task fairly trivial. The drivers will be implemented as kernel modules toallow for more direct hardware control.

E.2.2.2 Infrared sensors

The infrared ranging modules that are being considered for this platform are SharpGP2D02 type sensors. These sensors are able to detect an object in the range of 10cmto approximately 80cm.These modules will be mounted on the periphery of the sonar modules to detect ob-stacles coming up at an angle to the platform. As well a sensor will be mounted oneach side of the platform to allow for wall following, and hallway traversal, this will bepossible by keeping the distance signal on each side of the platform the same.Finally drivers need to be written to control these modules, these drivers will be writtenin a similar way to the sonar drivers.

E.2.3 Mechanical Design

The better part of the design work for the platform has been completed, and assemblyhas slowly begun. Figure 14 shows one of the two assembled octagonal halves ofthe chassis. Please refer to Appendix A for related mechanical drawings. Both teammembers will work on completing the required mechanical tasks.The chassis is assembled from 3/8” 1 1

2” × 1 12” aluminum L extrusions. This creates

an extremely ridged base to mount the drive train and related components to withoutneeding to worry about possible damage to the platform in the case of an accidentduring development.

The following is a list of mechanical tasks that require completion:

• fabricate motor mounts.

• fabricate bushings for mounting sprockets to motor shafts.

• mount drive train to chassis.

• create battery mount.

• assemble top and bottom halves of chassis.

• mount remaining electrical components.

150

Figure 14: Bottom half of chassis.

The main outstanding issue with the mechanical design is gaining access to a machineshop to complete fabrication of required parts. Unfortunately the mechanical engineer-ing department shops are unavailable for use, an attempt is being made to gain accessto the machine shops in the physics department.

E.2.4 Motor Controller

E.2.4.1 Control Module

In order to ensure the completion of the project within the allotted time, certain controlmodules will be implemented separately, off the PC-104 board. This will speed updevelopment by easier code writing and testing. This also means more CPU power forthe AI architecture. The PWM module to control the motors is the first such modulethat will be designed. The following tasks need to be completed:

• Create high-level serial communication protocol between PC-104 and externalmodules

• Design PCB for use with PIC microcontroller and motor H-bridges (may useexisting board)

• Write PIC code serial routines

• Write PIC code PWM routines

• Write PIC code for A/D conversion of motor current sensors (optional)

151

Comm ParametersParameter Value

Protocol RS-232 (maybe 422/485)Speed 57600 bpsData bits 8Parity bit NoneStop bit 1

Packet Format:0x10 0x01 Rx ADDR Tx ADDR CHK CMD Size Data 0x10 0x04

• All control sequences start with 0x10

– 0x01 ⇒ start of message

– 0x04 ⇒ end of message

– 0x10 ⇒ byte stuffing for data value of 0x10

– Byte stuffing will be used when 0x10 appears in other non-command data.Ie: data = 0x40 0x55 0x10 0x10 0x36 0x43 (repeated 0x10 not includedin Size or CHK parameters)

• Rx ADDR = receiving address

• Tx ADDR = transmitting address

• CHK = checksum = XOR of all message bytesEXCLUDING:

– control sequences (ie: 0x10 0x01)

– CHK itself

– second byte-stuffed 0x10

• CMD = command (device dependant)

• Size = # of data bytes

• Data = message data

Device ListAddress Device0x20 PWM Controller0x30 Sonar Sensors

Device: 0x20 - PWM ControllerCommand Description Data0x00 Disable none0x01 Enable none0x02 Set On Time 2 bytes : time in us0x03 Set Off Time 2 bytes : time in us0x04 Read Status Flags none0x05 Dynamic Brake On none0x06 Dynamic Brake Off none0x07 Dynamic Brake Pulse 1 byte : on-time in ms

152

Figure 15: Power amplifier board.

E.2.4.2 Power Amplifier

The power amplifiers being employed are high current, high voltage H-bridge type motorcontrollers. These modules are based around the National Semiconductors LMD18200H-Bridge modules. These driver boards were designed , assembled and tested as partof a previous project but will work quite well for this project. These boards are capableof driving two motors in bidirectional mode at a current of up to 6 amps continuous.A dynamic break is available for emergency situations. This will allow the motors tobe stopped within a fraction of a revolution at the risk of damaging the drive train bystripping sprockets or bending the drive shaft. Figure 15 shows the assembled poweramplifier board. The previously described control module will generate all the requiredsignals to run these amplifier boards.

E.2.5 Control algorithms

The control system will be based around a simple subsumption architecture. This willallow for quick development and easy modular testing of the cognitive system. Figure16 shows a rough outline of how the Augmented Finite State Machines (AFSMs) maybe implemented. The actual implementation will become more concrete as the projectprogresses.

153

Figure 16: Outline of subsumption architecture.

E.3 Timeline

Please see the following page for a Gant chart indicating the approximated time it willtake to complete the remaining tasks.

154

F progress report 2

156

F.1 Purpose

The intent of this project is to develop a low cost robotics research platform suitablefor future research into autonomous control algorithms. This initial development willalso include a basic control algorithm to allow the platform to traverse the engineeringbuilding in a random fashion, avoiding obstacles as it goes.The platform itself will be designed in such a way as to make later modifications oradditions simpler. This means modularity needs to be the primary concern in the me-chanical, electrical, and software designs.This platform needs to be developed in a low cost way as it is being funded by thedevelopment team. This means that custom hardware will be used wherever possibleto attempt to keep the total project cost down.

The goal of this project has not changed since the last status report, we are stillon target to complete all operations as initially laid out.

157

F.2 Progress

F.2.1 CPU Board

No further progress was required for the CPU board since the previous report.

F.2.1.1 Hardware

There are still several tasks that need to be completed regarding the cpu board and itshousing. These tasks include :

• mount CPU cooling fan.

• machine mounting holes for serial and parallel ports.

• mount power connector.

• possibly mount power supply inside the case.

No further progress has been made on these tasks since the last report as they areprimarily cosmetic tasks and will not affect the proper operation of the platform.These tasks will be completed after the chassis has been completely assembled or, de-pending on availability of the machine shop, other methods for completing this assemblywill be found.

F.2.1.2 Electrical

A simple 3A hybrid switching power supply has been selected for use in this project,currently the supply is on order and should arrive at any time. A PCB has beendesigned to make connection to the hybrid module easier, and this board will be etchedand assembled during reading break

F.2.2 Sensor Network

Jordan will be responsible for implementing the sensor interface, and this will occurafter the platform has been constructed.The Linux device drivers responsible for interfacing to all the sensors is under way andshould be completed and tested by the end of reading break. Once the main driver setis complete it will be a simple task to integrate the sensors into the platform.

F.2.2.1 Sonar sensors

The power supply for the sonar sensors has been designed, along with the accompanyingprinted circuit board, this will be etched and assembled at the same time as the powersupply board for the CPU.

F.2.2.2 Infrared sensors

The ordering of the Infrared sensors is pending waiting for success in implementing thesonar modules, and can be fairly quickly ordered and attached once the main sensorcode is completed.

158

F.2.3 Mechanical Design

The mechanical design is complete. The fabrication and assembly of the platform itselfis almost complete, and should be done by Friday June 27. At this point the platformshould be able to move under its own power, and soon after, under control of the CPU.

F.2.4 Motor Controller

F.2.4.1 Control Module

The PIC microcontroller code has been written for the PWM module. The serial in-terface has been tested on a PC allowing pre-testing of the module before the finalPC-104 platform is ready. The command structure has been refined making it simplerbut still retaining full functionality.The code has been designed in such a way that new modules, such as the IR proximitysensors, can be created in very short order (under a day).For the time being, the PWM module will be constructed on a MicrochipTM demon-stration board with on-board oscillator and RS-232 transceiver. Should there be a needor desire to build a separate PCB for this module, it will take under a day to design,layout, and create.

F.2.4.2 Power Amplifier

The power amplifier module is completed and is awaiting integration into the platformonce the mechanical fabrication is complete

F.2.5 Control algorithms

No further progress has been made on the Control algorithms.

159

F.3 Changes since last report :

Since the last report the only major change made to the system is the inclusion of awireless access point to allow for SSH access to the onboard PC/104 board while thesystem is in operation. This will allow for collecting real time telemetry data, as wellas controlling the startup and shutdown of the system.Additionally we are working on integrating some safety systems into the platform asthe final weight of the platform is going to be higher then expected. This means itcould do severe damage to objects if it collides with them at high speed. This safetywill most likely take the form of a rapid software shutdown that can be activated overthe wireless link. As well a onboard manual safety interlock to allow for rapid shutdownof the system in the case of a RF dropout.

F.4 Timeline

Please see the following page for a Gant chart indicating the approximated time it willtake to complete the remaining tasks.Based on our initial time estimates, we are ahead in the fabrication process and low levelhardware integration phases, and slightly behind in implementing the control algorithm.This is due in part to spending more time then expected in fabricating the chassis.

160

Documents

CENG 499 Final Report Autonomous Robotics Projectelec499/2003a/group12/docs/final.pdf · signing a robotics platform to meet the requirements we laid out during the inception phase