labyrinth - github pages · labyrinth initial design figure 2 setup of labyrinth the maze will be...

NORTHWESTERN UNIVERSITY

ME 433 ADVANCED MECHATRONICS

Labyrinth Final Report

Yoke Peng Leong

6/7/2011

Table of Contents

Introduction ......................................................................................................................... 2

Labyrinth ............................................................................................................................. 3

Maze ................................................................................................................................... 5

Aluminum Sheet.................................................................................................................. 8

Printed Circuit Board (PCB) ................................................................................................ 9

Sensors .............................................................................................................................10

Actuators............................................................................................................................12

Controllers .........................................................................................................................13

Results & Discussions........................................................................................................16

Future Work .......................................................................................................................17

Conclusion .........................................................................................................................17

Appendix A - Mechanical Design and Drawings .................................................................18

Appendix B - Sensors Design and Information ...................................................................20

Appendix C - Actuator Information .....................................................................................24

Appendix D - Controller Program .......................................................................................25

Introduction

The aim of our project is to create a robotic labyrinth solver using the PIC 32 (Figure 1). Initially, our idea

was to buy a labyrinth, attach servo motors to it, and track the ball using a camera. However, since we

had to use the PIC 32, we changed our control method by using IR sensing instead. We will approach our

problem by isolating one of the motors first for one axis rotational control and then continuing with the

two axis controller once the previous method succeeds. In our report, we will begin by describing the

overall design process and conclude by discussing our results.

Part list:

Labyrinth (replace the top maze board with maze made of acrylic)

Sensors - IR sensors

Actuators - RC Servo

Controller - PIC32

Figure 1 Labyrinth Final Design

Labyrinth

Initial Design

Figure 2 Setup of Labyrinth

The maze will be setup as in Figure 2. The maze board will be made of a material transparent to IR,

possibly acrylic. The IR emitters will be on top of the maze board, emitting IR to receivers underneath

the board. The distance of the emitters from the board depends on the material used to

suspend/support them. If the material used is opaque, they will have to be placed at a certain distance

(approximately 20cm) above the maze board to allow the user (us) to actually see the board. If the

material is transparent (e.g., wires, transparent acrylic, or the like) we can place the emitters close to

the board since it will not obstruct our view.

Final Design

Figure 3 Setup of Labyrinth (Revised Version)

The final maze is setup as in Figure 3. The maze board is made of acrylic with a layer of blue

semitransparent adhesive. The board is however transparent to IR. The IR emitter is a table lamp 60W

white light bulb which is placed on top of the maze board, emitting IR to receivers underneath the board.

The distance of the emitters from the board is approximately 1ft.

The maze board consists of 3 layers – the maze, aluminum sheet and PCB (Figure 4). Each layer will be

explained in the following sections.

Figure 4 Three layers of maze board

Maze

At the very top of the labyrinth is the maze (Figure 4).

Figure 5 Maze on top of Labyrinth

Initial Design

Figure 6 Maze which requires 1-axis control

Figure 5 depicts our first prototype maze. It only needs one axis of rotation for control (e.g. roll). The

other axis of rotation of the board (e.g. pitch) will be fixed at a certain angle (to be determined) to allow

the ball to reach the end of the maze. There will be a pair of sensors at each of the openings of the maze

and a sensor at each ends of the vertical path of the maze. When the ball is on top of the sensor, it will

obstruct the IR emitter and the receiver will not receive any IR. Our concept is to execute control by

moving the RC servo appropriately whenever the ball is on top of a sensor. By doing this control

repeatedly, we are hopeful that we will be able to guide the ball to the end of the maze.

Figure 7 Maze which requires 2-axis control

For our final product, we want to be able to execute 2-axis control (pitch and roll). The maze (Figure 7)

will be more complex but the sensors remain at the same locations. We will also have to add sensors at

each ends of the horizontal path of the maze. Using the same concept as previously stated, and if

everything goes as planned, the ball should be able to reach the end zone.

Final Design

Here are final designs for both mazes (Figure 8 and Figure 9). Refer to Appendix A for detailed drawing

with dimensions.

Figure 8 Maze which requires 1-axis control (Revised Version)

Figure 9 Maze which requires 2-axis control (Revised Version)

Aluminum Sheet

Underneath the maze is a layer of aluminum sheet as shown in Figure 10. This aluminum sheet has two

functions. One, it provides magnetic damping to the magnetic ball we are using to slow down its’ motion.

Two, it helps to prevent IR sensor underneath it from sensing IR from the side of the sensor head.

Figure 10 Aluminum sheet with holes

Holes are drilled at locations where sensors are located at. The sensor holes are ¼ in diameter and the

additional screw/spacer holes are ⅛ in diameter, and the centers are 0.875 in apart.

Printed Circuit Board (PCB)

Under the aluminum sheet is PCB with sensors as shown in Figure 11 and Figure 12. Each sensor is

distributed in a square grid of size 0.875 in by 0.875 in. There are 49 available spots for a 7 by 7 array of

sensors in total, but only 33 sensors are soldered on and used because of mazes we had. Theoretically,

the PCB will be suitable for all square maze of size 6.125 in by 6.125 in with a resolution of 0.875 in by

0.875 in. Refer to next section for more details on sensors design.

Figure 11 PCB for sensors (Front)

Figure 12 PCB for sensors (Back)

Sensors

Figure 13 Sensor Unit

IR sensor is used to detect IR from a table lamp (IR source) placed on top of the labyrinth board.

We did a simple experiment to find out the output voltage of IR sensor with varying distances from the

table lamp.

Distance (m) Open (HI) (V) Under shadow (LO) (V)

0.050 2.90 0.40

0.150 2.85 0.35

0.300 2.00 0.20

Table 1 Voltage swing of the sensors with varying source distance

Based on the simple experiment, 60W white table lamp is a good source of IR for the IR sensor.

Figure 14 Sensors location and label

Calibration of sensor with PIC32:

Resistor V_high V_low V_swing

1k ohm 1.6V 0.2V 1.4V

3.3k ohm 3.12V 0.7V 2.42V

10k ohm 3.17V 1.44V 1.73V

Table 2 Photo-transistors Voltage Reading from a 60W bulb at 1ft

We also varied the resistance of the sensor circuit to give us different gains. We tried to find a resistor

that will give us a voltage swing large enough for digital input/output (I/O) implementation. Hence, we

chose 3.3k ohm resistor for our photo-transistor because it gives the optimal voltage swing for PIC32

digital I/O pins.

Refer to Appendix B for datasheet link, detailed drawing and full schematic for sensors PCB.

Actuators

Figure 15 Servo motor attached to control knob

To control the board rotation, two servo motors are attached to each of control knob (see Figure 15)

respectively. The actuators used are the TOWERPRO SG-5010 servo motors. By just considering the mass

of the ball and the furthest distance of it from the axis of rotation, we need a maximum torque of 0.544

kg cm. At 6V, this servo generates 6.5 kg cm of torque, which is well enough to tilt the maze. It gives us

enough leeway to take into account the friction from the rotation of the maze. More information on the

servo motor can be found in Appendix C.

The PWM value for the servo can be calculated with the equation:

We centered our maze at 90 degrees (PWM of 1800) which means a tilt of 10 degrees will be equal to

100 degrees. The table below represents the PIC values for the PWM and the output angle from the

servo.

Calibration of motor with PIC32:

Input Angle (degrees) Servo Motor Input Value Output Angle (degrees)

5 1883 4.9

10 1967 9.8

-5 1717 5.1

-10 1633 9.9

Table 3 PIC and servo angle calibration

We see that the output angle from the servo is consistent with the input angles from the PIC.

Controllers

The controller used in this project is the NU32v2 which uses a PIC32MX795F512L as the microprocessor

(refer to as PIC32 in this report).

Figure 16 PIC32 with circuits for sensors

1-Axis Control

Figure 17 Maze using 1-axis control with sensors labeled

y

x

17

19

18 20

For the one-axis maze, the maze will always be tilted towards one side along the y-axis and the other

motor will be controlling the ball. We placed sensors at the ends of each column and at the openings in

between the columns. We figured that these are the so called strategic locations of the maze since the

ball will most probably get stuck at the end of the columns. The sensors at the openings between the

columns are essential since we need to detect whether the ball has crossed from one column to another.

Our controller algorithm is as follows:

If the ball is at locations 1, 5, 8, 12, 13, or 19, the board will tilt counterclockwise along the x-axis.

If the ball is at locations 3, 7, 10, 11, 15, 17, or 18, the board will tilt clockwise along the x-axis.

Finally, if the ball is at locations 6, 9, and 14, the board will not be tilted.

The controller can be implemented by nesting if-else statements in the program. When the ball reaches

a position and covers the IR sensor, we can determine where the ball should go by changing the tilt of

the maze.

2-Axis Control

Figure 18 Maze using 2-axis control with sensors labeled

Unlike the 1-axis controller, the 2-axis controller has two active motors. Furthermore, some of the

sensors in the 2-axis controller must produce two behaviors (tilt in the x-axis and y-axis) for the ball to

successfully reach the end zone. However, the overall concept of the controller does not change much.

For the ends of every rows and columns, there will be a sensor. Some of the rows and columns share the

same end, thus forcing the shared sensor to produce two different rotations. There are also sensors at

the openings of every row and columns just as the 1-axis controller.

The code for the 2-axis controller also consists of nested if-else statements with the addition of the

rotation along the y-axis. We used trial and error to obtain the angle of tilt of the maze. The angle is

chosen to be small to prevent the strings from slipping. More information about the code can be found

in Appendix D.

An interesting feature to our controller is that the maze will run infinitely back and forth as long as the

ball manages to reach the end zones every time. In addition, we programmed the controller to wiggle

the maze if it remains stationary for some amount of time. This can be detected by the sensors not

changing their values for a period of time. The jerk introduced will prevent the ball from being stuck at a

specific location in the maze.

Results & Discussions

Our design turns out to be pretty successful. It was able to complete the maze to and fro with both the

1-axis and 2-axis controller. The wiggle/jerk feature in our program also works well, forcing the ball to

move and not being stuck in the maze. The duration for the completion of the maze is typically around

30 seconds but it does vary and it might take as long as two minutes. The ball does overshoot and

undershoot through openings but the controller is able to compensate for the error and direct the ball

back to its intended path.

Maze Modification for Better Sensing

After testing the labyrinth, we found out that the IR sensor does not sense the ball consistently because

the columns are too wide. Hence, we added another layer of material to the maze wall to increase its

thickness so that the IR sensor will be able to sense the ball more consistently.

Removal of Magnetic Damping Feature

We have decided to not use the magnetic damping because with damping the tilt angle has to be larger

for the magnetic ball to overcome its initial static friction and start moving. However, with a large tilt

angle, slipping will occur more frequently on the surface between the string and the controlling rod.

Thus, we have made the tilt angle to be very small so that the normal ball will not move so quickly and

slipping will not occur as often.

In addition, magnetic ball is easily influenced by other metal objects in the surrounding such as bolts and

nuts.

Connector Mating and Design

Connector mating orientation should be considered when designing in PCB. We did not consider the

mating orientation as carefully as we should have. Therefore, the intended pins are in the wrong

direction as compared to the actual pins. Fortunately for us, all of the pins are just in the reverse order

(10 to 1 instead of 1 to 10) and they can be easily modified in the program instead of on the PCB.

Future Work

If we had more time, we would design our own labyrinth instead of using the one that we bought online.

We tried to do that this quarter but we did not have enough time due to the mechanical complexity in

making one. We would also design a PCB for the PIC and the motors. Currently, the PIC and the motors

are attached to a breadboard which is not very ideal. We would also like to discuss more about the two

topics below.

Use of Accelerometer to Determine Tilt Angle

One of the greatest challenges in our design is the inconsistent tilt angle of the maze. The servo does

give us the correct tilt but if the maze is run continuously for a period of time, the string in the labyrinth

starts to slip and/or stretch, giving us inconsistencies. The initial tilt of the maze is also important since

the controller adds or subtracts the tilt from the initial reference angle. To overcome this problem, we

can use an accelerometer as feedback to the motor to obtain desired angle.

More Precise Angle Changing Mechanism

Using a string and a rod as the mechanism to change the angle of the maze is not very accurate and

involves some slipping. To overcome these problems, other more advanced angle changing mechanism

can be considered. We have not fully explored and discovered other possibilities but we think that a

geared system might work better.

Conclusion

We made a couple of changes to our design from the initial suggested approach and came out with a

suitable design for our final product. By using the tweaked design, we were successful in building an

automated labyrinth solver. We managed to get the one-axis and the two-axis controllers to function

consistently. Furthermore, we have added more features in our design such as the capability of the

maze to run continuously and repetitively to and fro by itself. We have also successfully included a

wiggling/jerking feature to our maze to prevent the ball from being stuck at any location in the maze.

Appendix A - Mechanical Design and Drawings

Maze 1 Design

Maze 2 Design

Appendix B - Sensors Design and Information

Datasheet for sensor: http://hades.mech.northwestern.edu/images/1/14/Phototransistor-datasheet.pdf

Sensor Schematics

http://hades.mech.northwestern.edu/images/1/14/Phototransistor-datasheet.pdf

6/7/2011 7:21:07 PM f=0.85 C:\Users\Yoke Peng\Documents\eagle\maze sensors\sensors.sch (Sheet: 1/1)

3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND

3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND

3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND

3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND

3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND

3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND

3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND3.3V

GND

M40

M05X2PTH

M10"

3.3V

3.3V

3.3VGN

D

GN

D

M40

M05X2PTH

M25PTH

R1

1-C(SHORT) A

2-E(LONG) K

R2

1-C(SHORT) A

2-E(LONG) K

R3

1-C(SHORT) A

2-E(LONG) K

R4

1-C(SHORT) A

2-E(LONG) K

R5

1-C(SHORT) A

2-E(LONG) K

R6

1-C(SHORT) A

2-E(LONG) K

R7

1-C(SHORT) A

2-E(LONG) K

R8

1-C(SHORT) A

2-E(LONG) K

R9

1-C(SHORT) A

2-E(LONG) KR

10

1-C(SHORT) A

2-E(LONG) K

R11

1-C(SHORT) A

2-E(LONG) K

R12

1-C(SHORT) A

2-E(LONG) K

R13

1-C(SHORT) A

2-E(LONG) K

R14

1-C(SHORT) A

2-E(LONG) K

R15

1-C(SHORT) A

2-E(LONG) K

R16

1-C(SHORT) A

2-E(LONG) K

R17

1-C(SHORT) A

2-E(LONG) K

R18

1-C(SHORT) A

2-E(LONG) K

R19

1-C(SHORT) A

2-E(LONG) K

R20

1-C(SHORT) A

2-E(LONG) K

R21

1-C(SHORT) A

2-E(LONG) K

R22

1-C(SHORT) A

2-E(LONG) K

R23

1-C(SHORT) A

2-E(LONG) K

R24

1-C(SHORT) A

2-E(LONG) K

R25

1-C(SHORT) A

2-E(LONG) K

R26

1-C(SHORT) A

2-E(LONG) K

R27

1-C(SHORT) A

2-E(LONG) K

R28

1-C(SHORT) A

2-E(LONG) K

R29

1-C(SHORT) A

2-E(LONG) K

R30

1-C(SHORT) A

2-E(LONG) K

R31

1-C(SHORT) A

2-E(LONG) K

R32

1-C(SHORT) A

2-E(LONG) K

R33

1-C(SHORT) A

2-E(LONG) K

R34

1-C(SHORT) A

2-E(LONG) K

R35

1-C(SHORT) A

2-E(LONG) K

R36

1-C(SHORT) A

2-E(LONG) K

R37

1-C(SHORT) A

2-E(LONG) K

R38

1-C(SHORT) A

2-E(LONG) K

R39

1-C(SHORT) A

2-E(LONG) K

R40

1-C(SHORT) A

2-E(LONG) KR

41

1-C(SHORT) A

2-E(LONG) K

R42

1-C(SHORT) A

2-E(LONG) K

R43

1-C(SHORT) A

2-E(LONG) K

R44

1-C(SHORT) A

2-E(LONG) K

R45

1-C(SHORT) A

2-E(LONG) K

R46

1-C(SHORT) A

2-E(LONG) K

R47

1-C(SHORT) A

2-E(LONG) K

R48

1-C(SHORT) A

2-E(LONG) K

R49

1-C(SHORT) A

2-E(LONG) K

12345

U$50

6789

1011121314151617181920212223242526272829303132333435363738394041424344454647484950

108642

JP1

97531

12345

JP2

6789

10

12345

U$51

67891011121314151617181920212223242526272829303132333435363738394041424344454647484950

108642

JP3

97531

12345

JP4

6789

101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960

S1

S1

S2

S2

S3

S3

S4

S4

S5

S5

S6

S6

S7

S7

S14

S14

S13

S13

S12

S12

S11

S11

S10

S10

S9

S9

S8

S8

S15

S15

S16S16

S22

S22

S23

S23

S29

S29

S30

S30

S36

S36

S37

S37

S43

S43

S44

S44

S18

S18

S24

S24

S25

S25

S31

S31

S32

S32

S38

S38

S39

S39

S45

S45

S46

S46

S19

S19

S20

S20

S26

S26

S27

S27

S33

S33

S34

S34

S40

S40

S41

S41

S47

S47

S48

S48

S21

S21

S28

S28

S35

S35

S42

S42

S49

S49

S17

S17

X

X

Y

Y

Z

Z

U$1

SFH310

U$2

SFH310

U$3

SFH310

U$4

SFH310

U$5

SFH310

U$6

SFH310

U$7

SFH310

U$8

SFH310

U$9

SFH310

U$10

SFH310

U$11

SFH310

U$12

SFH310

U$13

SFH310

U$14

SFH310

U$15

SFH310

U$16

SFH310

U$17

SFH310

U$18

SFH310

U$19

SFH310

U$20

SFH310

U$21

SFH310

U$22

SFH310

U$23

SFH310

U$24

SFH310

U$25

SFH310

U$26

SFH310

U$27

SFH310

U$28

SFH310

U$29

SFH310

U$30

SFH310

U$31

SFH310

U$32

SFH310

U$33

SFH310

U$34

SFH310

U$35

SFH310

U$36

SFH310

U$37

SFH310

U$38

SFH310

U$39

SFH310

U$40

SFH310

U$41

SFH310

U$42

SFH310

U$43

SFH310

U$44

SFH310

U$45

SFH310

U$46

SFH310

U$47

SFH310

U$48

SFH310

U$49

SFH310

PCB Design

6/7/2011 7:22:11 PM C:\Users\Yoke Peng\Documents\eagle\maze sensors\sensors.brd

XYZ

3.3V

3.3VGND

GND

3.3VXYZ

1

25

50

U$50

U$51

R1

U$1

R2

U$2

R3

U$3

R4

U$4

R5

U$5

R6

U$6

R7

U$7

R8

U$8

R9

U$9

R10

U$10

R11

U$11

R12

U$12

R13

U$13

R14

U$14

R15

U$15

R16

U$16

R17

U$17

R18

U$18

R19

U$19

R20

U$20

R21

U$21

R22

U$22

R23

U$23

R24

U$24

R25

U$25

R26

U$26

R27

U$27

R28

U$28

R29

U$29

R30

U$30

R31

U$31

R32

U$32

R33

U$33

R34

U$34

R35

U$35

R36

U$36

R37

U$37

R38

U$38

R39

U$39

R40

U$40

R41

U$41

R42

U$42

R43

U$43

R44

U$44

R45

U$45

R46

U$46

R47

U$47

R48

U$48

R49

U$49

JP1

JP2

JP3

JP4

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

SFH310

M10

"

M25

PTH

Appendix C - Actuator Information

The figure above represents the schematics of the motor.

Torque calculation:

Assuming that the system is frictionless and the mass are equal on both ends, the required torque of the

motor comes from the mass of the ball and the distance from the axis of rotation. At maximum distance,

the ball is 9cm away from the center. The mass of the ball is 0.0605kg. Therefore, the torque is equal to:

We were unable to find the datasheet for the servo but we managed to obtain the specifications of the

servo from the manufacturer’s website.

SG5010

Weight: 38g

Dimension: 40.2*20.2*43.2mm

Stall torque: 5.5kg*cm(4.8V); 6.5kg*cm(6V);

Operating speed: 0.2sec/60degree(4.8v); 0.16sec/60degree(6v)

Operating voltage: 4.8-6V

Temperature range: 0℃_ 55℃

Dead band width: 10us

Source: http://www.towerpro.com.tw/viewitem1.asp?sn=598&area=53&cat=187

http://www.towerpro.com.tw/viewitem1.asp?sn=598&area=53&cat=187




















Appendix D - Controller Program

D:\Spring 2011\ME 433\Twoaxiscontrol.c Tuesday, June 07, 2011 7:36 PM

/*Twoaxiscontrol.cAuthor: Wei Tong and Yoke Peng LeongLast Updated: 6/7/11Info: This program takes 49 sensor inputs and process them to give a desired output.

We made this program to solve a labyrinth that we have designed.It mainly consists of nested if-else statments that determines what the controller (servo) should do with a given combination of input.The program will then output and change the PWM of the servo motors to tiltthe maze in the required position.

*/

#include <plib.h>#include "NU32v2.h"

#define WAIT 5000000int ServoDuty1, ServoDuty2;

void delay(double n);

int main(void) {int Sens_Arr[16], Sens_Arr2[16];int Sens_New, Sens_New2, Sens_Old, Sens_Old2;int count = 0;int ChangeDir = 0;

// set PIC32 to max computing powerSYSTEMConfig(SYS_FREQ, SYS_CFG_ALL);

// configure for multi-vectored modeINTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);

// enable multi-vector interrupts INTEnableSystemMultiVectoredInt();

// initialize the serial port and its rx interruptinitSerialNU32v2();

// set up the LED pins (G12 and G13) as outputs// and the SW pin (G6) as inputinitLEDs();

// set up sensors initSensors();

// use Timer3 for a 50Hz PWM signal for the RC servo motorOpenTimer3(T3_ON | T2_PS_1_64, 25000); // 80000000Hz / 64ps / 50Hz = 25000

// 25000 is < 2^16-1, so it is good to use// this also means that 25000 corresponds to 100% duty cycle

// Motor 1 ---> around y axis if z is up (pitch)OpenOC1(OC_ON | OC_TIMER3_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // pin D0

-1-


ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 1800SetDCOC1PWM(ServoDuty1); // duty cycle from 0-25000

// the rc servo has a minimum pulse of 0.24ms for 0 deg and a max of 2.6ms for 180 deg// at 1/(80000000/64)s per pulse, this corresponds to a min value of 300 and a max of 3300// and each value has a resolution of (180 degree / 3000 values) = 0.06 degrees

// Motor 2 ---> around x axis if z is up (roll, x moving forward)OpenOC2(OC_ON | OC_TIMER3_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // pin D1ServoDuty2 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 1800SetDCOC2PWM(ServoDuty2); // duty cycle from 0-25000

// the rc servo has a minimum pulse of 0.24ms for 0 deg and a max of 2.6ms for 180 deg// at 1/(80000000/64)s per pulse, this corresponds to a min value of 300 and a max of 3300// and each value has a resolution of (180 degree / 3000 values) = 0.06 degrees

delay(1000);

/* //------Servo Callibration-------ServoDuty1 = 1967; // 100 degrees -> (3000 counts / 180 degrees) * 100 degrees + 300 min val = 1967SetDCOC2PWM(ServoDuty1);delay(1000000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 1800SetDCOC2PWM(ServoDuty1);delay(1000000);ServoDuty1 = 1633; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC2PWM(ServoDuty1);delay(1000000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 1800SetDCOC2PWM(ServoDuty1);delay(1000000);

*/

Sens_Old = 0b0000000000000000;Sens_New = 0b0000000000000000;Sens_Old2 = 0b0000000000000000;Sens_New2 = 0b0000000000000000;count = 0;

//if(SW==0){ // Start when switch is pressedwhile(1){

if (SW==0){ // If button is pressed, do nothing.ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180ServoDuty2 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min

-2-


val = 180SetDCOC1PWM(ServoDuty1);SetDCOC2PWM(ServoDuty2);delay(1000);

}else {/* //------------One-Axis Sensor Array----------

Sens_Arr[0] = SENout1;Sens_Arr[1] = SENout15;Sens_Arr[2] = SENout22;Sens_Arr[3] = SENout29;Sens_Arr[4] = SENout31;Sens_Arr[5] = SENout43;Sens_Arr[6] = SENout14;Sens_Arr[7] = SENout21;Sens_Arr[8] = SENout27;Sens_Arr[9] = SENout28;Sens_Arr[10] = SENout35;Sens_Arr[11] = SENout40;Sens_Arr[12] = SENout42;Sens_Arr[13] = SENout20;Sens_Arr[14] = SENout24;Sens_Arr[15] = SENout33;

int i;for (i=0; i<=15; i++){

Sens_New = (Sens_New << 1) + Sens_Arr[i];}

if (Sens_New == Sens_Old){count++;

}else {

count = 0;}

Sens_Old = Sens_New;if ((Sens_New == 0b0000000000000000)&&(Sens_New2 == 0b0000000000000000)){

ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180ServoDuty2 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);SetDCOC2PWM(ServoDuty2);delay(1000);

}else if (count >= 4000){ // Jerk the maze after a long pause to prevent the ball from being stuck.

count = 0;ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180

-3-


SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1633; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1633; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1633; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1967; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1967; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);delay(100000); ServoDuty1 = 1967; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);delay(100000); sprintf(RS232_Out_Buffer, "Maze didn't move!!! \r\n"); WriteString(UART3, RS232_Out_Buffer);

}else {

-4-


// Do nothing}

*/

//------------Two-Axis Sensor Array----------Sens_Arr[0] = SENout5;Sens_Arr[1] = SENout7;Sens_Arr[2] = SENout10;Sens_Arr[3] = SENout11;Sens_Arr[4] = SENout12;Sens_Arr[5] = SENout18;Sens_Arr[6] = SENout24;Sens_Arr[7] = SENout25;Sens_Arr[8] = SENout27;Sens_Arr[9] = SENout33;Sens_Arr[10] = SENout36;Sens_Arr[11] = SENout37;Sens_Arr[12] = SENout38;Sens_Arr[13] = SENout39;Sens_Arr[14] = SENout40;Sens_Arr[15] = SENout43;

Sens_Arr2[0] = SENout47;Sens_Arr2[1] = SENout48;Sens_Arr2[2] = SENout49;Sens_Arr2[3] = 0;Sens_Arr2[4] = 0;Sens_Arr2[5] = 0;Sens_Arr2[6] = 0;Sens_Arr2[7] = 0;Sens_Arr2[8] = 0;Sens_Arr2[9] = 0;Sens_Arr2[10] = 0;Sens_Arr2[11] = 0;Sens_Arr2[12] = 0;Sens_Arr2[13] = 0;Sens_Arr2[14] = 0;Sens_Arr2[15] = 0;

int i;for (i=0; i<=15; i++){

Sens_New = (Sens_New << 1) + Sens_Arr[i];Sens_New2 = (Sens_New2 << 1) + Sens_Arr2[i];

}

if ((Sens_New == Sens_Old)&&(Sens_New2 == Sens_Old2)){count++;

}else {

count = 0;}

-5-


sprintf(RS232_Out_Buffer, "Counter = %d \r\n",count); WriteString(UART3, RS232_Out_Buffer);Sens_Old = Sens_New;Sens_Old2 = Sens_New2;

if ((Sens_New == 0b0000000000000000)&&(Sens_New2 == 0b0000000000000000)){ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180ServoDuty2 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);SetDCOC2PWM(ServoDuty2);delay(1000);

}else if (count >= 3000){ // Jerk the maze after a long pause to prevent the ball from being stuck.

count = 0;ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1600; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1633; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 1600; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1600; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 2000; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1967; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633

-6-


SetDCOC1PWM(ServoDuty1);delay(100000);ServoDuty1 = 2000; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 2000; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC2PWM(ServoDuty1);delay(100000);ServoDuty1 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 180SetDCOC1PWM(ServoDuty1);delay(100000);sprintf(RS232_Out_Buffer, "Maze didn't move!!! \r\n"); WriteString(UART3,RS232_Out_Buffer);

}else {

//------------------------------------------- SENSOR TEST -------------------------------------------------------

/*sprintf(RS232_Out_Buffer, "Sensor 1 = %d\r\n", SENout1); WriteString(UART3, RS232_Out_Buffer); sprintf(RS232_Out_Buffer, "Sensor 2 = %d\r\n", SENout2);WriteString(UART3, RS232_Out_Buffer);

// sprintf(RS232_Out_Buffer, "Sensor 3 = %d\r\n", SENout3);WriteString(UART3, RS232_Out_Buffer);// sprintf(RS232_Out_Buffer, "Sensor 4 = %d\r\n", SENout4);WriteString(UART3, RS232_Out_Buffer);

sprintf(RS232_Out_Buffer, "Sensor 5 = %d\r\n", SENout5);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 6 = %d\r\n", SENout6);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 7 = %d\r\n", SENout7);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 8 = %d\r\n", SENout8);WriteString(UART3, RS232_Out_Buffer);

// sprintf(RS232_Out_Buffer, "Sensor 9 = %d\r\n", SENout9);WriteString(UART3, RS232_Out_Buffer);

sprintf(RS232_Out_Buffer, "Sensor 10 = %d\r\n", SENout10);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 11 = %d\r\n", SENout11);

-7-


WriteString(UART3, RS232_Out_Buffer);// sprintf(RS232_Out_Buffer, "Sensor 12 = %d\r\n", SENout12);WriteString(UART3, RS232_Out_Buffer);// sprintf(RS232_Out_Buffer, "Sensor 13 = %d\r\n", SENout13);WriteString(UART3, RS232_Out_Buffer);

sprintf(RS232_Out_Buffer, "Sensor 14 = %d\r\n", SENout14);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 15 = %d\r\n", SENout15);WriteString(UART3, RS232_Out_Buffer);

// sprintf(RS232_Out_Buffer, "Sensor 16 = %d\r\n", SENout16);WriteString(UART3, RS232_Out_Buffer);// sprintf(RS232_Out_Buffer, "Sensor 17 = %d\r\n", SENout17);WriteString(UART3, RS232_Out_Buffer);

sprintf(RS232_Out_Buffer, "Sensor 18 = %d\r\n", SENout18);WriteString(UART3, RS232_Out_Buffer);


sprintf(RS232_Out_Buffer, "Sensor 20 = %d\r\n", SENout20);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 21 = %d\r\n", SENout21);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 22 = %d\r\n", SENout22);WriteString(UART3, RS232_Out_Buffer);


sprintf(RS232_Out_Buffer, "Sensor 24 = %d\r\n", SENout24);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 25 = %d\r\n", SENout25);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 26 = %d\r\n", SENout26);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 27 = %d\r\n", SENout27);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 28 = %d\r\n", SENout28);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 29 = %d\r\n", SENout29);WriteString(UART3, RS232_Out_Buffer);






sprintf(RS232_Out_Buffer, "Sensor 35 = %d\r\n", SENout35);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 36 = %d\r\n", SENout36);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 37 = %d\r\n", SENout37);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 38 = %d\r\n", SENout38);

-8-


WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 39 = %d\r\n", SENout39);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 40 = %d\r\n", SENout40);WriteString(UART3, RS232_Out_Buffer);


sprintf(RS232_Out_Buffer, "Sensor 42 = %d\r\n", SENout42);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 43 = %d\r\n", SENout43);WriteString(UART3, RS232_Out_Buffer);

// sprintf(RS232_Out_Buffer, "Sensor 44 = %d\r\n", SENout44);WriteString(UART3, RS232_Out_Buffer);// sprintf(RS232_Out_Buffer, "Sensor 45 = %d\r\n", SENout45);WriteString(UART3, RS232_Out_Buffer);// sprintf(RS232_Out_Buffer, "Sensor 46 = %d\r\n", SENout46);WriteString(UART3, RS232_Out_Buffer);

sprintf(RS232_Out_Buffer, "Sensor 47 = %d\r\n", SENout47);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 48 = %d\r\n", SENout48);WriteString(UART3, RS232_Out_Buffer);sprintf(RS232_Out_Buffer, "Sensor 49 = %d\r\n", SENout49);WriteString(UART3, RS232_Out_Buffer);delay(10000);

*/

/*//-------------------------------------------- ONE-AXIS ---------------------------------------------------------

if (SENout49==0){ChangeDir = 1;

}else if (SENout1==0){

ChangeDir = 0;}

if (ChangeDir == 0){ServoDuty2 = 1592; // 77.5 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 1800SetDCOC2PWM(ServoDuty2); // duty cycle from 0-25000if ((SENout1==0)||(SENout15==0)||(SENout22==0)||(SENout29==0)||(SENout31==0)||(SENout43==0)){

ServoDuty1 = 1633; // 80 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(1000);

}else if ((SENout14==0)||(SENout21==0)||(SENout27==0)||(SENout28==0)||(SENout35==0)||(SENout40==0)||(SENout42==0)){

ServoDuty1 = 1967; // 100 degrees -> (3000 counts / 180 degrees) *

-9-


100 degrees + 300 min val = 1967SetDCOC1PWM(ServoDuty1);delay(1000);

} else if ((SENout18==0)||(SENout20==0)||(SENout24==0)||(SENout33==0)){


}

else { delay(1000);// Do nothing}

}else if (ChangeDir == 1){

//------------------------------------------- ONE-AXIS (R) ------------------------------------------------------

ServoDuty2 = 2008; // 102 degrees -> (3000 counts / 180 degrees) * 90 degrees + 300 min val = 1800SetDCOC2PWM(ServoDuty2); // duty cycle from 0-25000if ((SENout24==0)||(SENout36==0)||(SENout8==0)||(SENout22==0)||(SENout29==0)){


}else if ((SENout49==0)||(SENout7==0)||(SENout21==0)||(SENout33==0)||(SENout28==0)||(SENout35==0)||(SENout20==0)||(SENout42==0)){


} else if ((SENout31==0)||(SENout27==0)||(SENout40==0)){


} else {

delay(1000);// Do nothing}

}

-10-


*/

//-------------------------------------------- TWO-AXIS ---------------------------------------------------------

if (SENout43==0){ChangeDir = 1;


ChangeDir = 0;}

if (ChangeDir == 0){if (SENout1==0){

ServoDuty1 = 1700; // 82 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(1000);ServoDuty2 = 1590; // 85 degrees -> (3000 counts / 180 degrees) * 78 degrees + 300 min val = 1600SetDCOC2PWM(ServoDuty2);delay(1000);

}if(SENout2==0){

ServoDuty1 = 1800; // 102 degrees -> (3000 counts / 180 degrees) * 102 degrees + 300 min val = 200SetDCOC1PWM(ServoDuty1);ServoDuty2 = 1580; // 80 degrees -> (3000 counts / 180 degrees) * 78 degrees + 300 min val = 1600SetDCOC2PWM(ServoDuty2);delay(1000);


ServoDuty1 = 1950; // 82 degrees -> (3000 counts / 180 degrees) * 80 degrees + 300 min val = 1633SetDCOC1PWM(ServoDuty1);delay(75000);ServoDuty2 = 1590; // 80 degrees -> (3000 counts / 180 degrees) * 78 degrees + 300 min val = 1600SetDCOC2PWM(ServoDuty2);delay(1000);

}else if ((SENout37==0)||(SENout39==0)){


}else if ((SENout7==0)||(SENout5==0)||(SENout11==0)||(SENout47==0)){// ServoDuty1 = 1967; // 100 degrees -> (3000 counts / 180 degrees) * 100

-11-


degrees + 300 min val = 1967ServoDuty1 = 2000; // 102 degrees -> (3000 counts / 180 degrees) * 102 degrees + 300 min val = 200SetDCOC1PWM(ServoDuty1);delay(1000);

}else if ((SENout49==0)||(SENout48==0)||(SENout26==0)||(SENout33==0)||(SENout36==0)||(SENout38==0)||(SENout39==0)){

// Turn 90if(SENout2==0){

ServoDuty1 = 1967; // 102 degrees -> (3000 counts / 180 degrees) * 102 degrees + 300 min val = 200SetDCOC1PWM(ServoDuty1);

}ServoDuty2 = 2000; // 100 degrees -> (3000 counts / 180 degrees) * 102 degrees + 300 min val = 2000SetDCOC2PWM(ServoDuty2);delay(1000);

}else if ((SENout6==0)||(SENout10==0)||(SENout25==0)||(SENout18==0)||(SENout40==0)){

// Turn < 90ServoDuty2 = 1600; // 80 degrees -> (3000 counts / 180 degrees) * 78 degrees + 300 min val = 1600SetDCOC2PWM(ServoDuty2);delay(1000);

}else if ((SENout27==0)||(SENout24==0)){

// Turn = 90if(SENout24==0){


}else if(SENout27==0){


}ServoDuty2 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 78 degrees + 300 min val = 1600SetDCOC2PWM(ServoDuty2);delay(1000);

}else {

// Do nothing}

}else if (ChangeDir == 1){

//------------------------------------------- TWO-AXIS (R) ------------------------------------------------------

-12-


if ((SENout1==0)||(SENout2==0)||(SENout43==0)||(SENout12==0)||(SENout26==0)){


}else if ((SENout40==0)||(SENout25==0)||(SENout11==0)||(SENout38==0)){// ServoDuty1 = 1967; // 100 degrees -> (3000 counts / 180 degrees) * 100 degrees + 300 min val = 1967


}else if ((SENout48==0)||(SENout47==0)||(SENout36==0)||(SENout37==0)||(SENout38==0)||(SENout39==0)||(SENout33==0)||(SENout27==0)){

// Turn 90ServoDuty2 = 2000; // 100 degrees -> (3000 counts / 180 degrees) * 102 degrees + 300 min val = 2000SetDCOC2PWM(ServoDuty2);delay(1000);

}else if ((SENout5==0)||(SENout7==0)||(SENout10==0)||(SENout24==0)||(SENout18==0)){

// Turn < 90ServoDuty2 = 1600; // 80 degrees -> (3000 counts / 180 degrees) * 78 degrees + 300 min val = 1600SetDCOC2PWM(ServoDuty2);delay(1000);

}else {

// Do nothing}

}} // End else for jerk and dark.

/*ServoDuty2 = 1625; // 79 degrees -> (3000 counts / 180 degrees) * 79 degrees + 300 min val = 1617SetDCOC2PWM(ServoDuty2);delay(1000000);ServoDuty2 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 78 degrees + 300 min val = 1600SetDCOC2PWM(ServoDuty2);delay(1000000);ServoDuty2 = 1983; // 101 degrees -> (3000 counts / 180 degrees) * 101 degrees + 300 min val = 1983SetDCOC2PWM(ServoDuty2);delay(1000000);ServoDuty2 = 1800; // 90 degrees -> (3000 counts / 180 degrees) * 78

-13-


degrees + 300 min val = 1600SetDCOC2PWM(ServoDuty2);delay(1000000);

*/} // End switch if} // End while

return 0;} // End main

void delay(double n) {int i;for(i = 0; i < n; i++);

}

-14-