Sie sind auf Seite 1von 128

Faculty of Engineering

Department of Mechanical Engineering

BACHELOR OF TECHNOLOGY PROGRAMME


MECHANICAL / MANUFACTURING ENGINEERING

TM3101 Mechanical Engineering Design


(2011)
Design Project

Team 3 B
Supervisor : Prof Lu Li
Andrew Lim Hock Khim

A0066195X

Lin Shaodun

A0066078X

Leong Kang Siang

A0066104R

Lin Mingkun

A0066174A

Maung Guang Xi

U0800858R

Mohamad Yazid Bin Ismail

U0800867B

Li Qiming

A0066164E

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

Table of Contents

Page

Acknowledgements

Abstract

1.0

Introduction

1.1

Aim

1.2

Background

1.3

Methodology

1.4

Scope

2.0

Discussion on Various Operating Mechanisms

10

for an Autonomous Robots

3.0

2.1

Group Timelines

11

2.2

Report Writing Tasking

13

Design of the Chassis for the working functions

14

of an Autonomous Robots
3.1

3.2
4.0

Chassis Design

15

3.1.1

Design 1

15

3.1.2

Design 2

16

3.1.3

Design 3

17

Final Selection

Design of the Arms and Grippers for the working functions

18

of an Autonomous Robots
4.1

4.2

5.0

Arm Design

18

4.1.1

Selection of Designs

19

4.1.2

Final Selection of Design

21

Gripper Design

22

4.2.1

Selection of Designs

22

4.2.2

Final Selection of Design

25

Materials and Parts Selection for an Autonomous Robot

26

5.1

Selection of Material for Chassis

26

5.1.1

28

Selection of Materials for Arm and Gripper

5.2

PCB Assembly

30

5.3

Robot Arm Assembly

32

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

5.4
6.0

7.0

9.0

10.0

34

Torques for Motors

35

6.1

Required Torque for Theta Motor

35

6.2

Required Torque for Arm Motor

37

Troubleshooting

39

7.1

Mechanical Parts

39

7.1.1

Robot Arm timing gear issue

40

7.1.2

Insufficient Arm motor torque

40

7.1.3

Mounting of arm motor not rigid enough

40

7.1.4

Chassis stiffness improvement

40

7.1.5

Low track tension

41

7.2

8.0

Gripper Assembly

Electrical Parts

42

7.2.1

Low voltage supply to DC motors

42

7.2.2

Low current supply to DC motors

42

7.2.3

Wrong wiring of servo motor

42

7.2.4

Detection of presence of empty can

42

Calculation of Motors Requirement

42

8.1

Gripper Motor

42

8.2

Arm Motor

43

8.3

Theta Motor

44

8.4

DC Drive Motor

45

Control for activation of devices

46

9.1

46

Control Circuit

Testing of Program

57

10.1

Testing and Integration of Moving Parts

57

10.2

Testing on the Operation of Moving Parts

57

10.3

Testing on a Mock Up Arena for Accuracy of Sensors

11.0

Recommendation

58

12.0

Conclusion

59

13.0

References

59

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

14.0

Appendices

60

Appendix A

Research

Appendix B

Costing Details

Appendix C

Other Concepts

Appendix D

Detailed Drawings

Appendix E

Program Codes

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

Acknowledgements

With special thanks and gratitude to:


Prof Jerry Fuh
Course Lecturer

A/Prof Y F Zhang
Course Lecturer & Project Coordinator

Prof Lu Li
Project Mentor

We would like to express our most sincere thanks to the above mentioned and also
those who had helped us in one way or another in the successful completion of our
project.

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

Abstract
This project aims to design and build an autonomous robot to move along a
designated track on an arena with utmost accuracy with the aid of sensors being
correctly positioned. This autonomous robot is programmed to pick an empty coke
from an elevated platform after having passed through a gantry and place this empty
can on another platform with different height at the most efficient timing of 3 to 5 sec.
This machine would demonstrate its capability to maneuver through a 300 mm
wide and 450 mm high gantry to pick up an empty Coke can from a designated
elevated platform on the left and transport it to a platform on the right in the shortest
possible time. This is made possible through a detailed program to activate the driving
motor of the chassis, motor and belt bringing the long arm downwards, sensors and
other devices to react at the precise timing.
Using the design methodology, our group was able to successfully design and
build a functioning robot, sophisticated in appearance and with multiple operations.
Though our group did not manage to get the robot to complete the task, we have
managed to impress the judge through our own unique design and the different
functional movements.
We have shown that we were able to display and showcase the many different
methods and technique of pick and place operations. This is important as it provides
us with a framework to hone our mechanical and electrical design in whatever
situation the industries have to offer, instead of remaining stagnated to current
ideology. Our team members have learned to work collectively and holistically
throughout the project cycle, this has not only created a deep bonding among us but
also enhanced our teambuilding skill.

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

1.0

Introduction

1.1

Aim

The aim of this project is to design and build an autonomous robot to navigate
through a rectangular gate, pick up an empty drink can from a raised platform in the
left and place the can onto a raised platform in the right in the shortest possible time.
The objective of this project not only build upon our fundamental in the way to
connect every moving parts together but also how to inject the auto-capability
software to the hardware through integration use programming. This is achieved by
many session of brainstorming to get the best mechanical approach and also the most
effective and efficient way to meet our objective. This project also enabled us to
explore deeper into new methods instead of using the existing ones in the design of
our autonomous robot. This is especially crucial in our developmental stage as we
further embark on our remaining journey in this Bachelor of Technology programme.

1.2

Background

The machine must be self-contained with no form of remote control for all the
functional tasks. The machine must fit inside the 200 x 150 mm start area with no
height limit. The machine must not weight more than 5 kg.

1.3

Methodology

The following approaches were adopted in the study:


a.

Detailed study on the various chassis available in the market from

wheels to track.

1.4

b.

Detailed study on the various arms and grippers.

c.

Detailed study on motors and sensors

d.

Detailed study on the various methods of pick and place

Scope

This report investigates the different mechanisms used in the construction of an


autonomous robot, the features and performances derived from the application of
many different methods of pick and place. It also details the exploration of three

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

different design setups in terms of cost effectiveness, availability of parts, technical


support, effectiveness and efficiency.
This report also cover materials, hardware and software aspects of the final stage
including specification of the various components, software programming as well as
static and dynamic testing.
1.4.1 Design Parameters / Specifications
Arena
a.

The competition arena is 800 mm wide and 1200 mm long.

b.

The arena is painted matt black, with border all around.

c.

The lines shown in the arena are retro-reflective tape (50 mm in width)

for guiding.
.

Figure 1 Floor plan of the competition arena

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

Coke Can
A typical empty Coke Can is made of aluminum, with shape that is cylindrical and
with an overall dimension, dia 50mm x height 80mm.

Elevated Platforms
Both raised platform will be of the same dimension of 100 by 100 mm. The height of
the platform can be different, between 20 mm to 50 mm, in steps of 10 mm.

Gates
The gate is 300 mm wide. The exact location of the gate will not be made known until
the competition, but shall be constrained between 450 mm and 750 mm from the
starting base line, in steps of 50 mm. The lower 50 mm of the gate shall be made retro
reflective for sensing purposes.

Machine
For the machine dimension, there is no specific dimension that we must adhere to.
But our machine must strictly obey the following conditions stated;
The machine must be self-contained.
a.

No form of remote control or external power is allowed.

b.

The machine must fit inside the 200 x 150 (mm) starting box.

c.

The machine must not weigh more than 5 kg.

d.

There is no height limit

The main focus is to ensure speed, stability and accuracy of the machine during
operation.

TM3101 Mechanical Engineering Design

Faculty of Engineering
Department of Mechanical Engineering

2.0

Discussion on Various Operating Mechanisms and Approaches for our

Autonomous Robot
The discussion on the arms and grippers and the mechanism to operate both of them
had given us a good brain storming session, coupled with the advice given by our
teacher had certainly provided us with some good decision making. To continue with
the momentum with our project, we have decided to meet every Saturday from 1400
to 1700 hrs and discuss the best ways to connect all the moving parts more
effectively.

2.1

Project Timeline

Before a project timeline was charted for this project, our team shared their areas of
expertise and how each can effectively contribute to the successful completion of this
project.

TM3101 Mechanical Engineering Design

10

Faculty of Engineering
Department of Mechanical Engineering

Name

Role

Task Allocated

LIM HOCK KHIM


ANDREW

Team Lead

1. Project overall coordination


2. Prepare Project Report
3. Prepare Gantt Chart using Microsoft
Project
4. Purchasing of fabrication and standard
parts

LIN SHAODUN

Mechatronics
Designer

LEONG KANG
SIANG

Mechanical
Designer

1.
2.
3.
4.
5.
1.

LIN MINGKUN

Manufacturing

1. Fabricate Parts,
2. Prepare alternative design concepts for
Gripper and Chassis ,
3. Support Robot Assembly.

MAUNG GUANG
XI

Designer

1. Prepare alternative design concepts for


Gripper and Chassis ,
2. Support Robot Assembly.

MOHAMAD
YAZID BIN
ISMAIL

Designer

1. Fabricate Parts,
2. Prepare alternative design concepts for
Gripper and Chassis ,
3. Support Robot Assembly.

LI QIMING

Software
Engineer

1. Prepare Flow Chart for Robot Control


2. Coding and Debugging using C.

TM3101 Mechanical Engineering Design

Detail Mechanical design,


Prepare BOM.
Purchase standard parts.
Conduct Brainstorm meetings
Design concept review
Prepare alternative design concepts for
Gripper and Chassis ,
2. Support Robot Assembly.

11

Faculty of Engineering
Department of Mechanical Engineering

TM3101 Mechanical Engineering Design

12

Faculty of Engineering
Department of Mechanical Engineering

2.2

Report Writing Tasking

Other report
stuffs
Project
management

Resource
planning

Group
Report

LIM HOCK KHIM AND


REW

Design drawings,
screen captures

Programs

LIN SHAODUN

LI QIMING

Dummy
Arena

Hand sketch
ideas

Other report
stuffs

Decision
matrix

Design
philosophy

Other report
stuffs

Flow Chart

MAUNG GUANG
XI

Group
Report

Design
calculations,
Motor torque
requirement,
Comparison
time of
alternatives

LIN MINGKUN

MOHAMAD
YAZID BIN
ISMAIL

Chassis motor torque and


speed requirement
calculation base on
different loads

Theta motor torque and


speed calculation base
on different loads

Arm motor torque and


speed calculation base
on different loads

Gripper motor torque and


speed calculation base
on different loads

LEONG KANG
SIANG

LIN MINGKUN

Design
calculations,
Motor torque
requirement,
time

Chassis design
alternatives

Sensor alternatives

Comparison of
alternatives

Arm design
alternatives

Robot Sequence
alternatives

Gripper design
alternatives

Control design
alternatives

Resource
planning
Human resources

Initial study

External resources

Project planning

TM3101 Mechanical Engineering Design

LEONG KANG
SIANG

MAUNG
GUANG XI

MOHAMAD
YAZID BIN
ISMAIL

13

Faculty of Engineering
Department of Mechanical Engineering

A project task allocation was agreed upon as an interim measure to get everyone
moving forward.
S/No
1

Task Needed
Calculation base on different loads for the
following:

Task Allocation
LIN MINGKUN
LEONG KANG SIANG

Chassis motor torque and speed


Arm motor torque and speed
Theta motor torque and speed
Gripper motor torque and speed
2

Design alternatives for the following:


Chassis
Arm design

MOHAMAD YAZID BIN


ISMAIL
MAUNG GUANG XI

Gripper design
Sensor
Robot Sequence
Control design
3

Resource planning for the following:

ANDREW LIM

Human resources
External resources
Initial study
Project planning
4

Programming and Debugging

LI QIMING

Design drawing and Fabrication

LIN SHAODUN

TM3101 Mechanical Engineering Design

14

Faculty of Engineering
Department of Mechanical Engineering

3.0

Design of the Chassis for the working functions of an Autonomous Robots

3.1

Chassis Design

Chassis Design is the most vital and significant aspect in this project. In order to
make the robot move, execute and complete the task within the fastest time in a
stable condition, selection of materials, size, width, length, height are
quintessential from an engineering standpoint. This accounts for 60% of the
entire design and every factor has to carefully consider.
3.1.1

Design 1

One driving motor (6 Wheel Drive)

Pros:
o

Highest maneuverability.

Higher reliability.

Fastest speed.

Cons:
o

Highest cost.

Need more motor driver port.

Complicate software.

TM3101 Mechanical Engineering Design

15

Faculty of Engineering
Department of Mechanical Engineering

3.1.2

Design 2

One driving motor with one steering motor (4 Wheel Drive)

Pros:
o

Easy direction control.

Using one DC gear motor and one servo motor.

Easy control algorithm.

Fastest speed.

Cons:
o

Highest cost.

Need more motor driver port.

Complicate software.

Less stable.

3.1.3

Design 3

Two driving motor with track wheel

TM3101 Mechanical Engineering Design

16

Faculty of Engineering
Department of Mechanical Engineering

3.2

Pros:
o

Easy direction control.

Using two DC gear motor and track wheel set.

Easy control algorithm.

Small turning radius.

Creative design.

More stability.

Cons:
o

More mechanical parts.

Average speed.
Final Selection

This year, our group has decided to come out a different, unique and creative design
concept compared with others team. We knew that a four wheels drive concept as
well a six wheel drive concept will work since these have been proven from the past
competition. In order to benefit more from this learning process, we have decided not
to adopt similar design concept.

TM3101 Mechanical Engineering Design

17

Faculty of Engineering
Department of Mechanical Engineering

4.0
Design of the Arms and Grippers for the working functions of an
Autonomous Robots
Initially we were thinking of using the flip over type instead of rotating after hearing
from our lecturer's comments so as to minimise complexity and keep the mechanism
as simple as possible as last year, more than 50% failed to accomplish the task. As
there was only one group who had used the rotating type due to longer time in the
motion. Beside timing and completion of mandatory task, the overall outlook of the
robot will also be taken into consideration; hence detailed discussions were carried
out on the gripper, arm and turntable and also the flip-over design as well as the
moving mechanism design. More designs were proposed for brainstorming, such as
the below diagram. Fine-tuning was also proposed on the design for the arm, theta
and gripper to achieve better results. Some sketches and simple diagrams were drawn
to show how to improve the design
4.1 Arm Design
Arm design is like the human body in this project, it acts as a coordinator to link the
chassis and gripper together. Hence it is very vital and every factors need to be
examine carefully to ensure that the arm design is fully functional and compatible
with the other parts.
4.1.1 Selection of Designs
Though there were many designs being researched and discussed upon (Appendix A),
but several sessions of brainstorming, our team had narrowed down to the following
three arm designs:
Design A:
Gear

Servo Motor

Base

Gripper

TM3101 Mechanical Engineering Design

Arm

18

Faculty of Engineering
Department of Mechanical Engineering

This arm design uses a one piece concept which comprises arm, gripper and base to
be linked together as shown above. However the materials used in this arm design
must be light-weight in order to support the various parts weight.
Design B:
Servo
Motor
Arm

Base

Gripper

Timing Belt

This arm design comprises a unique appearance and using the mechanism of timing
belt to operate the arm motion.
Design C:

Servo
Motor

Arm

Gripper

Base

TM3101 Mechanical Engineering Design

19

Faculty of Engineering
Department of Mechanical Engineering

This arm design uses simple mechanism with motor attached to the gear to move up
or down of the arm.

Aesthetics

Producibility
Creativity
Use of Standard Parts
Reliability
Cost

After serious investigation and consideration, our team had come out with the above
ARROW showing the six criteria in the Decision Matrix. The purpose of the
decision matrix is to allow us to analyze and grade them according to the various
designs.
Below is the table of Decision Matrix:

Criteria

Weight (%)

Design A

Design B

Design C

Cost

10

Reliability

25

18

20

18

Use of standard parts

15

12

10

Creativity

15

12

10

Reproducibility

25

17

20

18

Aesthetics

10

Total

100%

65

80

71

TM3101 Mechanical Engineering Design

20

Faculty of Engineering
Department of Mechanical Engineering

Overall
Design A

65%

3rd

Design B

80%

1st

Design C

71%

2nd

4.1.2 Final selection of design:

Design 2 had emerged champion in the table of Decision Matrix, with the
uniqueness and innovative mechanism making use of timing belt to tilt the
gripper parallel to the floor at which angle the arm make, our team had
unanimously agreed to use this design for the arm.
4.2 Gripper Design:
Gripper design is extremely important in this project as it perform the most crucial
execution action to pick and place the can from one platform to another. In order to
make the robot to perform the desired action, all factors need to be taken seriously
into consideration. Similar to the arm design, thorough research were done and many

TM3101 Mechanical Engineering Design

21

Faculty of Engineering
Department of Mechanical Engineering

mechanisms of this nature were discovered and discussed (Appendix A), however
only 3 sets of designs are discussed here.
Design A:

Worm Gear

Base Plate

Gripper

Servo
Motor
Design A uses a 4 bar linkage mechanism and by attaching a motor to the worm gear,
the gripper is able to open/close easily.
Design B:
Actuator

Gripper

Base
Plate

TM3101 Mechanical Engineering Design

22

Faculty of Engineering
Department of Mechanical Engineering

Design B is a little rigid as it has more parts as compared to the other designs and it
takes slightly more time to operate the gripper motion.

Design C:
Servo
Motor

Gripper

Gear

Design C had an innovative outlook and with the help of the servo motor attaching to
the gear, the gearing mechanism can easily operate the motion of the gripper.
Below is the table of Decision Matrix:
Criteria

Weight(%)

Design A

Design B

Design C

Cost

10

Reliability

25

18

15

23

Use of standard parts

15

10

12

Creativity

15

10

10

12

Producibility

25

18

18

22

Aesthetics

10

Total

100%

71

65

86

Overall
Design A

71%

2nd

Design B

65%

3rd

Design C

86%

1st

TM3101 Mechanical Engineering Design

23

Faculty of Engineering
Department of Mechanical Engineering

4.2.2 Final Selection of design:

Design C had emerged champion in the table of Decision Matrix, with its
innovative appearance and yet simple gearing mechanism, our team had also
unanimously agreed to use this design.
Any finalized design were discussed and decided by majority of the team members,
hence all the designs were finalized with group consent so as to prevent any conflict
in the event of the robot failing to complete mandatory task. Many consensuses were
put up and agreed upon.

TM3101 Mechanical Engineering Design

24

Faculty of Engineering
Department of Mechanical Engineering

5.0

Materials and Parts Selection for an Autonomous Robot

5.1

Selection of Material for Chassis

The robot chassis design is based on Robo-PICA 2.0 robot kit purchased from
Innovative Experiment Co. Ltd. The Robo-PICA 2.0 robot kit consist the following
Mechanical components:

Motor Gearbox Uses


a 4.5V (9V max.) and
180 mA DC motor with
a ratio of 48:1; torque
4kg/cm; comes with the
mounting.

Track wheel set - includes 3lengths of Track wheel, many


support wheels and sprockets,
axels and shaft bases.

The Plate set and 4types of the colourmixed Plastic Joiner


(10 of Straight Joiner, 10
of Right-angle Joiner, 10
of Obtuse Joiner and
3/5/12 Holes straight
joiners)

Many sizes of Screw and


Nut
(Screw:3x6mm.,3x10mm.,
3x15mm.,3x25mm. and
3x35mm, 3mm. nuts), Flat
head screws and Thumb
screws.
Set of Plastic Spacers
(Length: 3mm. 15mm. and
25 mm.)
Hex Standoffs: 3x30mm

Originally the DC motor and track wheel set is mounted on a plastic universal plate,
but this kind of structure is lack of rigidity also cannot ensure each component are
accurately aligned and installed, which definitely will affect the performance of the
robot.
In order to overcome this problem, the base plate of the robot chassis must be
redesigned, featured with locating pins to precisely locate all the components. (Figure
5.1)

TM3101 Mechanical Engineering Design

25

Faculty of Engineering
Department of Mechanical Engineering

Original Chassis

Improved Chassis
Figure 5.1
For instance, locating pins are added onto base plate to locate the two DC motors,
which is to ensure the drive sprockets mounts on DC motor shaft are parallel to
moving direction, so that the robot will move in a straight line hence reduce the needs
of line tracking correction. (Figure 5.2)

Locating Pins

Locating Pins
Original Mounting

Improved Mounting

Figure 5.2
The same locating pin concept is applied to mounting of long angled shaft base as
well: (Figure 5.3).
With these locating pins, it is not only easing the alignment of components when
mounts onto base plate, but also shortens the total assembly time. When there is a
need to remove the components from the assembly during debugging, we do not need
to concern about whether it can be put back to the original position, as the parts will
be in the same position with simply align it against the locating pins.

TM3101 Mechanical Engineering Design

26

Faculty of Engineering
Department of Mechanical Engineering

Locating Pins

Locating Pins
Original Mounting

Figure 5.3

Improved Mounting

For material selection of robot chassis assembly, at initial stage, A6061-T6


(Aluminum) was chosen as the material for Base plate and PCB mounting plate, but
since the robot equips with a very long arm, it is necessary to make sure the chassis is
heavy enough to counter balance the moment caused by robot arm and gripper.
(Figure 5.4) So choosing steel as chassis material is not for the consideration of
higher strength, just to ensure the CG of robot is within the track contact area when
the robot arm is at pick-n-place position. (In fact, more counter weight was installed
onto chassis during actual test as the displacement of robot arm is not so smooth,
which produces unbalance force and cause robot jumps during when arm moves
down. This kind of dynamic behavior has not been considered during design stage as
its complicated and difficult to predict.)
5.1.1 Selection of Materials for Arm and Gripper

Robot CG if using Steel


as Chassis material
Robot CG if using Al as
Chassis material

Figure 5.4
The overall chassis design looks like this in exploded view: (Figure 5.5)

TM3101 Mechanical Engineering Design

27

Faculty of Engineering
Department of Mechanical Engineering

Figure 5.5
Functionality of each component is briefly illustrated as follow: (Figure 5.6)
PCB mounting plate
DC Motor
Track

Theta Shaft

Counter Weight

Guiding wheels
Theta motor
Base Plate

Main Sprocket

Line tracking sensor

Side IR Sensor
Bottom IR Sensor

Figure 5.6

TM3101 Mechanical Engineering Design

28

Faculty of Engineering
Department of Mechanical Engineering

5.2 PCB Assembly


The Robo-PICA 2.0 robot kit comes with RBX-877V2.0 Robot Experiment Board as
follow: (Figure 5.7)

Figure 5.7
The main microcontroller is the PIC16F887. Figure 5.8 shows the operating diagram
of RBX-877 board.

Figure 5.8

TM3101 Mechanical Engineering Design

29

Faculty of Engineering
Department of Mechanical Engineering

The technical feature of RBX-877V2.0 Robot Experiment Board as follow:


Controlled by PIC16F887 Microcontroller with 8Kword memory. Run with
20MHz clock.
Download the program via ICD2 jack.
LCD16x2 display with LED back light and jumper to on/off control.
Piezo speaker.
A LED monitor.
Drive 2-DC motors 4.5V to 6V and 3-RC Servo motors (in range 4.8 to 6V).
9-Programmable ports support all analogue and digital input/output.
I2C bus port.
UART port for interfacing the serial device such as RS-232 transceiver, XBee
module and Bluetooth.
Supply voltage from 4 of AA batteries.
2.375 x 6.25 Inches size.
The first problem encountered when integrate the PCB into robot design is: the length
of the PCB is 160mm which exceed the robot design specification (150x200mm),
therefore the PCB cannot be mounted as suggested in robot kit manual, it only can be
mounting vertically or horizontally in 200mm direction. Mounting the PCB in vertical
direction has several benefits:
1) It will be convenience for wiring.
2) The mounting structure is simpler.
3) Overall design looks neater.

RBX-877 Board

Hence the final design of PCB


assembly looks like this: (Figure 5.9)
12V Li-ion Polymer
Battery
12V to 6V DC converter

PCB mounting post

Figure 5.9

TM3101 Mechanical Engineering Design

30

Faculty of Engineering
Department of Mechanical Engineering

5.3 Robot Arm Assembly


The robot arm assembly mounts onto theta shaft of robot chassis, it can rotate 90to
enable gripper reach the Coke can and perform pick-n-place task.
Both Robot arm and Gripper assembly must be light weight to reduce the moment
inertia. If the robot arm is too heavy, more torque is required for both arm motor and
theta motor. Hence the material of the parts inside these two assemblies mainly use
A6061-T6 (Aluminum).
Compare with steel, Aluminum can achieve same strength with lesser mass. The
mechanical properties of A6061-T6 as follow:
Ultimate Tensile Strength 310 MPa
Tensile Yield Strength 276 MPa

Bearing Yield 386 MPa


Strength
Poisson's Ratio 0.33

Elongation at Break 12 %

Fatigue Strength 96.5 MPa

Modulus of Elasticity 68.9 GPa

Shear Modulus 26 GPa

Ultimate Bearing Strength 607 MPa

Shear Strength 207 MPa

The robot arm assembly comes with a unique feature is : it use a pair of timing gear
and a timing belt to maintain the gripper always at horizontal position , this is useful
as according to TM3101 design specification the Coke can might be placed in
different height, with this feature, the height different can be compensated easily.
(Figure 5.10)

Movable timing gear


Fixed timing gear

Timing belt

Figure 5.10

TM3101 Mechanical Engineering Design

31

Faculty of Engineering
Department of Mechanical Engineering

The drive servo motor use a pair of bevel gear to transmit power, the reason of not
using direct drive is because of space constraint when theta axis rotates. Since the
gear ratio is 1:1, the torque is only affected by mechanical efficiency of bevel gear.
(Figure 5.11)

Bevel Gears

Figure 5.11
Another end of timing gear is mounted on an adjustable block, so that the timing belt
tension can be adjusted properly. (Figure 5.12)

Figure 5.12

TM3101 Mechanical Engineering Design

32

Faculty of Engineering
Department of Mechanical Engineering

Overall arm assembly is shown as follow: (Figure 5.13)

Figure 5.13

5.4 Gripper Assembly


The function of Gripper assembly is to grip the Coke can. In order to achieve this
objective, a touch sensor is installed in gripper jaw, when the touch sensor is
activated, the PIC will know the gripping is successful, otherwise the
PIC will command the gripper servo motor to rotate further degrees. A
Sharp GP2D120 distance sensor is installed to detect the
Servo Motor

stage height difference.


Overall gripper assembly is
shown as follow: (Figure 5.14)
In order to reduce the total
mass of gripper assembly, all
fabricated parts use Aluminum
material.

Distance sensor
Touch sensor

Figure 5.14

TM3101 Mechanical Engineering Design

33

Faculty of Engineering
Department of Mechanical Engineering

6.0

Torque for Motors

6.1 Required Torque for Theta and Arm Motor


After finished 3D modeling in SolidWorks, it is very convenience to use SolidWorks
add-ins like SolidWorks Motion and SolidWorks Simulation to verify the design.
SolidWorks Simulation is a design validation tool that shows engineers how their
designs will behave as physical objects. SolidWorks Simulation provides core
simulation tools to test strength and safety, analyze the kinematics, and simulate real
world performance to help you make the decisions that improve quality.
SolidWorks Motion is a virtual prototyping tool that provides motion simulation
capabilities to ensure designs function properly. SolidWorks Motion enables
engineers to size motors/actuators, determine power consumption, layout linkages,
develop cams, understand gear drives, size springs/dampers, and determine how
contacting parts behave.
The Motion Simulation result will provide a baseline for selection of servo motors, it
also generates displacement profile for each axis, as a reference for programming of
robot motion.
6.1.1 Required Torque for Theta motor
Here is the procedure of setting up the Motion Simulation:
a) Suppress the mates (assembly constraints) of the theta shaft to chassis
assembly, which allows rotating of arm and gripper assembly.
b) Define the Motion study frame rate at 200FPS and assume all redundant mates
are placed by rigid bushings.
c) Define gravity =9.81m/s2 at Z axis direction.
d) Define a motor attach to theta shaft and rotates relate to PCB plate, assume it
takes one second to rotate 90, and assume the

displacement profile is

Cycloidal motion.
Here is the screen capture when of Motion Simulation interface. (Figure 6.1)

TM3101 Mechanical Engineering Design

34

Faculty of Engineering
Department of Mechanical Engineering

Figure 6.1
After Motion study finished, we can plot the displacement curve, angular velocity and
peak torque required, etc. (Figure 6.2)

Angular Velocity
Angular Displacement

Torque Required

Figure 6.2
By varying the total displacement time, we can define what is the suitable
acceleration which will not exceed the torque output from Theta servo motor. (Figure
6.3)

TM3101 Mechanical Engineering Design

35

Faculty of Engineering
Department of Mechanical Engineering
12

Torque Required vs. Displacement time

Torque Required (kg.cm)

10

Torque Required
Torque Supplied

8
6
4
2

Time to move 90 (Sec)

0.50

0.75

1.00

1.25

1.50

1.75

2.00

Figure 6.3
Above graph shows the torque required increases exponentially when the
displacement time become shorter, since the torque supplied is 2.5kg from Tahmazo
TS-1033MG motor, we can see the feasible displacement time is 1.50 second , after
consider mechanical efficiency, actual motion acceleration condition, and resistance
force created by cable routing.
6.1.2 Required Torque for Arm motor
Apply the same principle to Arm motor, the torque curve looks like follow: (Figure
6.4)

Angular Velocity

Angular Displacement

Torque Required

Figure 6.4
By varying the total displacement time, we can define what is the suitable
acceleration which will not exceed the torque output from Arm servo motor. (Figure
6.5)

TM3101 Mechanical Engineering Design

36

Faculty of Engineering
Department of Mechanical Engineering
30

Torque Required vs. Displacement time

Torque Required (kg.cm)

25
20
Torque Required
Torque Supplied

15
10
5

Time to move 90 (Sec)


0
0.00

0.25

0.50

0.75

1.00

1.25

1.50

Figure 6.5
Above graph shows the total displacement time should be larger than 0.50 second,
otherwise the arm motor might stall when lift up from zero degree position. Of course
this data just a rough guide line as it does not consider the factors like bevel gear
mechanical efficiency and cable routing tension. And the actual movement of arm
cannot be smooth which creates higher acceleration, hence the torque require will be
even higher.
Torque applied on robot arm need to be counter balanced by the frictional force
produced by the robot weight, if the theta moving too fast, the whole robot might
shift position, which will affect the accuracy of robot arm movement. Below
graph show some consideration of this issue:
Torque applied

40

Reaction force study with diferent Friction coefficient

Reaction force Required (N)

35

Reaction Force
Track to Floor u=0.6
Track to Floor u=0.8
Track to Floor u=1.0

30
25

Reaction force
available

20
15
10
5

Time to move 90 (Sec)

0
0.50

0.75

1.00

1.25

1.50

1.75

2.00

From the graph we can see if we set the theta motion time to 1.5 seconds and the
servo motor can move smoothly, the torque to drive robot arm should not cause whole
robot shift position at worst case scenario.

TM3101 Mechanical Engineering Design

37

Faculty of Engineering
Department of Mechanical Engineering

7.1 Troubleshooting
After completed assembly and wiring, many issues encountered during
debugging, below are some of the mechanical and electrical problems
investigated and resolved.
7.1.1 Robot Arm timing gear issue.
Issue: Gripper cannot maintain at horizontal position when arm rotates.
Root cause: Mechanism design failure.
Solution: Change design as follow. (Figure 7.1)
Initially the timing gear is fixed onto the arm plate, this incorrect design causes
gripper attached at the end of arm not stays in horizontal position when arm
rotates. After found out this root cause, change the mounting of timing gear from
arm plate to bearing holder plate so that the timing gear will not move together
with the arm and the timing belt mechanism always maintain the gripper in
horizontal position.

Bearing Holder Plate

Timing Gear

Arm Plate

Figure 7.1
7.1.2 Low voltage supply to DC motors.
Issue: Robot moving very slowly as the rpm of DC motor is slow.
Root cause: The output voltage from H-bridge is low.
Solution: After studied the circuit diagram and the specification of some
components, we know the Voltage regulator KIA7042 can withstand input
voltage up to 12V, and the H-bridge driver L293D can have input up to 30V,
(Figure 7.2) so we decided to increase supply voltage from 6.0V to 7.5V, by
change the battery holder from AAx4 to AAx6, and fabricated one dummy
battery to obtain 1.5x5=7.5V supply. With this modification, the robot can move
50% faster.

TM3101 Mechanical Engineering Design

38

Faculty of Engineering
Department of Mechanical Engineering

5.5 V

Voltage regulator

4.3 V

Figure 7.2

H-bridge driver

The maximum voltage for DC motor is 9V, but we did not use 9V as supply as the
current-limiting resistors become very hot with higher voltage supplied, this has
been confirmed by thermal image. (Figure 7.3) In order not to damage the PIC
board, we decide only go for 7.5V as maximum allowable input voltage.

Figure 7.3
7.1.3 Low current supply to RC Servo motors.
Issue: The built in servo motor port only supply 200mA each.
Root cause: Circuit design limitation.
Solution: The servo motor used in the robot takes 300~600mA, so we have to
use external power supply to provide higher current to servo motor otherwise it
will not produce enough torque. We choses 2800mAH 12V Li-ion Polymer
rechargeable battery, and made a 12V to 6V converter board to step down the
supply for 3 servo motors.

TM3101 Mechanical Engineering Design

39

Faculty of Engineering
Department of Mechanical Engineering

Figure 7.4
7.1.4 Wrong wiring of servo motor
Issue: Unable to command the servo motors.
Root cause: Forgot to connect the GND pin of servo ports when using external
power supply.
Solution: Re-connect the GND pin as follow: (Figure 7.5)

Figure 7.5
7.1.5 Arm motor torque is insufficient.
Issue: The torque of arm motor (Tower pro SG5010) is insufficient.
Root cause: Supplier put wrong specification on the website. This motor is
purchased from www.dealextreme.com , on that website, it states the torque is
15kg.cm at 6V, but in fact, on motor manufacturers website, the SG501o torque
is actually 6.5kg.cm, so definitely it will not able to lift up the robot arm.
Solution: Change Tower pro SG5010 to Hitec HSR-5980SG motor. (24kg.cm @
6V)
TM3101 Mechanical Engineering Design

40

Faculty of Engineering
Department of Mechanical Engineering

7.1.6 Mounting of arm motor is not rigid enough.


Issue: Mounting of arm motor is not rigid enough cause robot arm not moving
smoothly.
Root cause: Did not perform FEA study for the arm motor mounting bracket.
Solution: Change design from sheet metal bracket to a solid Aluminum bracket.

Before

After
Figure 7.6

7.1.7 Need detection the present of Coke can


Issue: Need detection the present of Coke can
Root cause: To improve the accuracy of arm movement.
Solution: Add another distance sensor on the gripper to detect the present of
Coke can.

Additional
distance sensor

Figure 7.7
7.1.8 High load acting on theta motor directly.
Issue: High force directly acting on the theta motor shaft when robot arm
rotates.
Root cause: PCB plate mounts on DC motor directly and it is in over-hang
condition which is not rigid enough when robot arm moves.
Solution: Use the old arm motor mounting bracket to improve the PCB plate
mounting rigidity. (Figure 7.8)

TM3101 Mechanical Engineering Design

41

Faculty of Engineering
Department of Mechanical Engineering

Support bracket

Before

After
Figure 7.8

7.1.9 Low track tension


Issue: The track loose tension over time and it might slip from the sprocket
during running.
Root cause: a) Poor quality track: the track become permanently deformed over
time, b) The track is joined from 3 shorter tracks; hence the joint portion is easily
deformed or elongated.
Solution: Change the driving method from track to wheel. (Note: No time to
execute this change). (Figure 7.9)

Weakest link

Figure 7.9

TM3101 Mechanical Engineering Design

42

Faculty of Engineering
Department of Mechanical Engineering

7.1.10 DC gear motor shaft deformed.


Issue: DC gear motor shaft deformed under high belt tension, the sprocket is
wobbling when turns and cause robot cannot move in straight line.
Root cause: The shaft is made from plastic and cannot withstand high lateral
load.
Solution: Change a new DC gear motor with metal shaft (Note: No time to
execute this change). (Figure 7.10)

Figure 7.10

TM3101 Mechanical Engineering Design

43

Faculty of Engineering
Department of Mechanical Engineering

8.0

Calculation on Motors Requirement

8.1

Gripper Motor
Gripper Motor Calculation
FG = Required gripping force [N] per gripper jaw
MG = Gripping torque = FG x r
r = Distance between gripper zero point and gripping point
m = Work piece mass [kg]
Assuming friction at bearing is negligible.
g = Acceleration due to gravity (9.81m/s2) is required if
acting against the acceleration, a
= Angle of V-gripper finger
= Coefficient of friction between gripper finger and work
piece

Assuming friction at bearing is negligible.


(

Assume a = 3 m/s2

Mass of can, m = 35 g
Coefficient of Friction, = 0.5 (Aluminum to Aluminum
contact)
2 = 120o , = 60o
Distance, r = 75mm

Therefore, minimum torque required to grip is 0.0291Nm or 0.297 kgcm.

TM3101 Mechanical Engineering Design

44

Faculty of Engineering
Department of Mechanical Engineering

8.2

Arm Motor

Arm Motor Calculation

Assuming friction at bevel gear and bearing is negligible.

Assume a = 3 m/s2
Mass of arm assembly, m = 350 g

)
(

Distance from C.G, x = 275mm

Therefore, minimum torque required to raise arm is 1.233Nm or 12.56 kg.cm.

TM3101 Mechanical Engineering Design

45

Faculty of Engineering
Department of Mechanical Engineering

8.3

Theta Motor

Theta Motor Calculation

Assuming constant angular acceleration and deceleration.


Area under graph = 41o + 48o
= 89o
= 1.553 rad
=
= 3.106 rad/s
=
= 6.212 rad/s2

Ix = 177654.68 g.mm2 = 0.000178 kg.m2


Iy = 6352187.60 g.mm2 = 0.00635 kg.m2
Iz = 6382390.57 g.mm2 = 0.00638 kg.m2
Since Iz is the greatest, therefore second
moment of inertia, I = Iz
Assume time taken to rotate 89o = 1 sec

Therefore, minimum torque required to turn is 0.040Nm or 0.408 kg.cm.

TM3101 Mechanical Engineering Design

46

Faculty of Engineering
Department of Mechanical Engineering

8.4

DC Drive Motor

DC Drive Motor Calculation

Linear velocity, v (m/s)

Drive Wheel

g
r

1.25

2.5

Time (s)

Assuming constant linear acceleration and deceleration.


Assuming friction between drive wheel and track is negligible.
Area under graph = 0.65 m

Assume time taken to travel 0.65m = 2.5 sec


Assume Coefficient of Friction, = 0.4 (Track and
ground contact)

v=
= 0.52 m/s

Radius of Drive Wheel, r = 15.7 mm

a=

Mass of robot, m = 2 kg

= 0.416 m/s
(

For different values of Coefficient of Friction,


= 0.4, Torque required =0.136 Nm or 1.39 kgcm
= 0.6, Torque required =0.198 Nm or 2.02 kgcm
= 0.8, Torque required =0.259 Nm or 2.64 kgcm
= 1.0, Torque required =0.321 Nm or 3.27 kgcm

Therefore, minimum torque required to move robot is 0.136 Nm or 1.39 kg.cm.

TM3101 Mechanical Engineering Design

47

Faculty of Engineering
Department of Mechanical Engineering

9.0

Control for activation of devices

Popular programming language for PIC could be assembly and C code, but before
coming to real programming stage, we need to understand the schematic diagram of
the PIC, registers, functions and I/Os of each pin of the micro controller. After many
tries on the PIC with LED lights, we moved on to explore the controlling of servo
motor, we faced the challenge when we made use of interrupts and PWM of PIC
programming. In this area we have limited resources and professionals and also
constrains in terms of mechanical design, budget and functionalities of PIC. Despite
all these limitations we have eventually come up with a set of programming codes
that meet the robot operational requirement (See Appendix E Program codes)
9.1

Control Circuit

The control circuit of the various sequences to activate the devices installed onto our
autonomous robot was collectively discussed and a few flow charts were mapped up
depicting the stages of operation and movement.

TM3101 Mechanical Engineering Design

48

Faculty of Engineering
Department of Mechanical Engineering

General Flow Chart


START
Gripper Close
Sequence

Power On

Arm Hoisting
Sequence

Port Initialization
Sequence

Place Sequence
Is the Start
Button Pressed?

Standby Mode
Arm Lowering
Sequence for
Place

Line Tracking
Sequence
Gripper Open
Sequence
Pick Up
Sequence
STOP, End of
Operation
Arm Lowering
Sequence for
Pickup

TM3101 Mechanical Engineering Design

49

Faculty of Engineering
Department of Mechanical Engineering

Port Initialization Sequence

START
Beep for finish
Port Initialization

Delay
500mS

Power On

Initialize Port
RA0~5 as Input

Initialize Port
RE0 as Input

Initialize Port
RB0~5 as Output

Initialize Port
RC0~5 as Output

Send pulse to Port


RB4 to set Theta to
home (0o position)

Delay
500mS

Send pulse to Port


RB5 to set Arm to
home (0o position)

Delay
500mS

Disable Port RC1 and


RC2, DC motor A and
B off

Delay
100mS

Beep (x1) for


finish Motors
Initialization

Display Ready
Message on LCD

Initialize RD2~7
for LCD

Initialize Buzzer

Send pulse to Port


RC5 to set Gripper to
home (0o position)

TM3101 Mechanical Engineering Design

Finish

50

Faculty of Engineering
Department of Mechanical Engineering

Line Tracking Sequence


START

Enable Port RC2


& RC1 to start
DC Motor A & B

Chassis move
straight
forward

Enable back Port


RC2/RC1 to start back
Motor A/B

Y
N

Is left and right IR


sensor output equal?

Sensor left/right output


high, disable Port
RC2/RC1, stop Motor
A/B to allow chassis turn
left/right to correct path.

Y
N

Is left and right IR


sensor output equal?

N
N

Is side IR Sensor
output high?

Y
Robot moved past gate,
send pulse to Port RB5 to
lower arm to 45o

Is left and right IR


sensor output high?

Robot continues
to move forward

Disable Port RC1 and RC2


to stop Motor A and B,
reverse current to move
chassis backward

Robot move forward


in slower profile

Disable Port RC1 and RC2 to


stop Motor A and B, send
pulse to RC0 to beep (x1)
buzzer

Y
Y

Is left and right IR


sensor output high?

Is rear IR sensor
output high?

STOP, proceed to
Pick Up Sequence

N
TM3101 Mechanical Engineering Design

51

Faculty of Engineering
Department of Mechanical Engineering

Pick Up Sequence
START

Send pulse to Port


RB4 to rotate arm
41o CCW

Send pulse to RC0


to beep (x1) buzzer

STOP, proceed to
Arm Lowering
Sequence for Pickup

TM3101 Mechanical Engineering Design

52

Faculty of Engineering
Department of Mechanical Engineering

Arm Lowering Sequence for Pickup


START, arm is
already lowered
by 45o

Send pulse to Port


RB5 to further
lower arm

N
IR distance sensor
feedback height
above platform is 5cm

Y
Send pulse to RC0
to beep (x1) buzzer

STOP, proceed to
Gripper Close
Sequence

Time taken more


than 2 sec?

Y
Send pulse to RC0
to beep
(continuous) buzzer

Display Time Out


Error Message on
LCD

STOP, rectification
required

TM3101 Mechanical Engineering Design

53

Faculty of Engineering
Department of Mechanical Engineering

Gripper Close Sequence


START

Send pulse to Port


RC5 to close gripper

N
Touch sensor
contact close

Time taken more


than 1 sec?

Send pulse to RC0


to beep (x1) buzzer

Send pulse to RC0


to beep
(continuous) buzzer

STOP, proceed to
Arm Hoisting
Sequence

Display Time Out


Error Message on
LCD

STOP, rectification
required

TM3101 Mechanical Engineering Design

54

Faculty of Engineering
Department of Mechanical Engineering

Arm Hoisting Sequence/Place Sequence


START

Send pulse to Port


RB5 to hoist arm by
30o

Send pulse to RC0


to beep (x1) buzzer

STOP, proceed to
Place Sequence

TM3101 Mechanical Engineering Design

START

Send pulse to Port


RB4 to rotate arm
89o CW

Send pulse to RC0


to beep (x1) buzzer

STOP, proceed to
Arm Lowering
Sequence for Place

55

Faculty of Engineering
Department of Mechanical Engineering

Arm Lowering Sequence for Place


START, arm is
somewhere at
midway position

Send pulse to Port


RB5 to lower arm

N
IR distance sensor
feedback height
above platform is 5cm

Y
Send pulse to RC0
to beep (x1) buzzer

STOP, proceed to
Gripper Open
Sequence

Time taken more


than 2 sec?

Y
Send pulse to RC0
to beep
(continuous) buzzer

Display Time Out


Error Message on
LCD

STOP, rectification
required

TM3101 Mechanical Engineering Design

56

Faculty of Engineering
Department of Mechanical Engineering

Gripper Open Sequence


START

Send pulse to Port


RC5 to open gripper

Touch sensor
contact open

Y
Send pulse to RC0
to beep (x3) buzzer

STOP, End of
Operation

TM3101 Mechanical Engineering Design

57

Faculty of Engineering
Department of Mechanical Engineering

10.0

Testing of Program

Programmer has tested out all the major electronic components to be used for the
project using sample robotic machine provided by the lecturer and proceed to do
optimization of the programming part. The main objective for this testing on the
components is to ascertain whether controlling of multiple actions is possible and at
the same time maintaining the stability of the entire system.

10.1

Testing and Integration of Moving Parts

Input power limitation of the board


The PIC board can take up to 6v = 4 AA batteries, but the actual measurement taken
by us gave 4.2v output power to the DC motor and also its not enough capacities of
power to supply 3 servo motors include one with 30kg torque.
Solution:
By studying online materials of using alternative supply of power to the servo motor
with signals still be sent out by the PIC, we managed to rewire the whole connecting
structure with a separate external power supply dedicated to the 3 servo motors to
conserve energy of AA batteries used by the PIC and DC motor.
Modify the existing 4 AA batteries holder to 6 slots with one dummy battery, because
we want to raise the supply voltage to 7.5v meaning the DC motor will run faster with
high input of power. Picture below shows the wiring system.

TM3101 Mechanical Engineering Design

58

Faculty of Engineering
Department of Mechanical Engineering

10.2

Testing on the Operation of Moving Parts

The real test of program after assembly of our robot take days and nights on refining
programming code, numerous times of loading and running our robot on our self
made dummy arena and actual arena. Thinking optimization from software point of
view based on the fixed design is very challenging, because electrical, mechanical and
even software issues would come up at the same time while observation did not help
in most of the time if issues happened internally.

Problem Arises

Remedial Actions

(1) Weak mechanism lock inside the timing (1) Tighten screw on the shaft inside the
belt.
timing belt by 1st dismantling the arm from
the chassis.

(2) Insufficient torque of the servo motor to (2) Replace servo motor with higher torque
lift up the arm due to incorrect specification which increased our overall cost.
from vendor.
(3) Servo motor failure half way during the (3) Replace with new servo motors.
testing progress.
(4) DC motor running in low speed and the
tracks are not moving straight which caused
our robot unable to reach the right position
for pick & place.

TM3101 Mechanical Engineering Design

(4) Supply 7.5v overloaded power to the PIC


board and additional sensor installed in front
of the gripper with programming sensors,
servo motors work simultaneously to
compensate the wrong positioning issue of
our robot.

59

Faculty of Engineering
Department of Mechanical Engineering

10.3

Testing on a Mock Up Arena for Accuracy of Sensors

In the programming approach to this project, we created Line & Object tracking
function for correctness of the moving path of the robot, Smart auto-sensing &
comparing system for the sensors to automatically compare the black and white color
on the stage with changing environment meaning to avoid incorrect reading of the
sensor due to different lighting setup, Auto compensation sensing and gripping
system for incorrect positioning of our robot before pick & place the can.

TM3101 Mechanical Engineering Design

60

Faculty of Engineering
Department of Mechanical Engineering

Line & Object Tracking function:

x_L
x_R
R

x_S

/****Line & Object Tracking****/


ADCON0=0b11001001; //side sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_S=(ADRESH*4)+(ADRESL/64);
PWM1_Set_Duty(255);
// Motor A
PWM2_Set_Duty(255);
// Motor B
if((x_L<200) & (x_R>700))
{
PWM1_Set_Duty(255);
// Motor A
PWM2_Set_Duty(0);
// Motor B
}
if((x_L>700) & (x_R>200))
{
PWM1_Set_Duty(0);
// Motor A
PWM2_Set_Duty(255);
// Motor B
}

TM3101 Mechanical Engineering Design

Object Detected by
Side sensor x_S and
Line tracking by
sensor x_L & x_R

61

Faculty of Engineering
Department of Mechanical Engineering

Smart auto-sensing & comparing system:

x_THETA
Stage
/****Smart auto-sensing & comparing system****/
x_THETA=ARM_MOTION_DOWN();//ARM LOWER DOWN
Lcd_Cmd(_LCD_CLEAR);
while(1)
Dynamic variable
{
x_THETA to avoid
Auto
ADCON0=0b11001001; //side sensor
Sensing
the influence of
ADCON0.GO=1;
Indicator
variant Height of
while(ADCON0.GO);
the stage and
x_S=(ADRESH*4)+(ADRESL/64);
lighting conditions
if(x_THETA==0)
ARM_MOTION_DOWN_HOLD();
if((x_S<100) & (x_THETA==0))
......................................................
{
x_THETA=1;
ARM_MOTION_DOWN_HOLD();
ADCON0=0b11000001; //distance sensor1
ADCON0.GO=1;
while(ADCON0.GO);

TM3101 Mechanical Engineering Design

62

Faculty of Engineering
Department of Mechanical Engineering

Optimizations by IR sensors:

TM3101 Mechanical Engineering Design

63

Faculty of Engineering
Department of Mechanical Engineering

Auto compensation sensing and gripping system:


Error
Angle
X_d2

Sensor
To
Detect
CAN

TM3101 Mechanical Engineering Design

Auto
Compensation
Angle

/****Auto compensation sensing


and gripping system****/
ADCON0=0b11011001;
//distance sensor2
while(1)
Auto
{
Compensation
ADCON0.GO=1; Indicator
while(ADCON0.GO);
x_D2=
(ADRESH*4)+(ADRESL/64);
if(x_D2>=400)
break;
if(x_D2<400)
{
for(m=30;m>0;m--)
..................................
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;

64

Faculty of Engineering
Department of Mechanical Engineering

11.0

Recommendation

There were many problem areas that we did not anticipate initially, though on
hindsight there were areas that could be improved, especially on the selection of
motors as regards to quality compared with lower price.
There are also pros and cons of sensors that can be good indicators by forming a close
loop system to play a role in feedback control, but sensors are too sensitive due to
reflection, lightings, humidity, hence in our case need programming code to handle
medium range reflecting due to crowded people around the arena

12.0

Conclusion

Although it was hard work spending many hours each week on the project, we have
gained significantly through the intensive research and detailed brainstorming session
that we have embarked on from start to find the most unique and elegant way to
design and build our machine. We have learned many new things that were not
covered in our lectures, like organizational and interpersonal skill.
Finally we have managed to acquire the many new skills and in depth knowledge
from numerous testing and troubleshooting to effectively integrate both hardware and
software.

TM3101 Mechanical Engineering Design

65

Faculty of Engineering
Department of Mechanical Engineering

13.0

References

External Resources
http://www.sparkfun.com
http://www.sgbotic.com/
http://www.amainobj.com/
http://shop.rotor.com.sg/
http://www.sparkfun.com/datasheets/Robotics/Robo-PICA2007.pdf
http://www.streetdirectory.com/asia_travel/travel/travel_id_2810/travel_site_1706
A-Main Objectives Pte Ltd

TM3101 Mechanical Engineering Design

66

Faculty of Engineering
Department of Mechanical Engineering

14.0

Appendices
Appendix A

Research

(A) CHASSIS
Design A

Design B

Design C

Design D

Design E

Design F

Design B

Design C

(B) ARM DESIGN


Design A

GRIP HANDLE

GRIP STATIC
FRAME
BOLT & NUT

DELRIN MOUNTING
GRIP LINKAGE

SCREW & NUT


GRIP CENTRAL BASE

MOUNTING BRACKET
(L-SHAPE)

HARD WIRE CONNECTING SERVO


MOTOR TO GRIPPER
ROBOT ARM

SERVO MOTOR

TM3101 Mechanical Engineering Design

SCREW & NUT FOR


MOUNTING BRACKET

67

Faculty of Engineering
Department of Mechanical Engineering

Design D

Design E

Design F

Design A

Design B

Design C

Design D

Design E

Design F

(C) GRIPPER DESIGN

TM3101 Mechanical Engineering Design

68

Faculty of Engineering
Department of Mechanical Engineering

(D) Sensors
Design A

Design B

Design C

Design D

Design E

Design F

TM3101 Mechanical Engineering Design

69

Faculty of Engineering
Department of Mechanical Engineering

Appendix B Costing Details


S/N

Description

Paid to

Paid by

Amount

Receipt

Fabrication and Standard


parts

WHITS Technologies Pte Ltd

Lin Shaodun

SGD 1059.8

Page 3-8

Robotics Kit PICA 2.0

Sparkfun Electronics

Lin Shaodun

SGD 247

Page 9

Gears, Li-ion Battery,


connectors, components for
12V to 6V board

Continental Electronics
Enterprise

Lin Shaodun

SGD 70.1

Page 10

IR sensor x 2

James Yong

Lin Shaodun

SGD 20

Page 11

TS-1033MG RC servo motor


and extension cable

Singapore Hobby Shop Pte


Ltd

Lin Shaodun

SGD 21.72

Page 11

SG5010 RC servo motor


SG90 RC servo motor x4

DealExtreme.com

Lin Shaodun

USD 20.82

Page 12

AAx6 Battery box, 9V


battery

Continental Electronics
Enterprise

Lin Shaodun

SGD 4.90

Page 13

5V Voltage Regulator

A-Main Objectives Pte Ltd

Li QiMing

SGD 10

Page 14

Electronics Components for


PIC testing

Kai Chin Computer System


Pte Ltd

Li QiMing

SGD 39.5

Page 15

10

TS-1033MG RC servo motor


and extension cable

Singapore Hobby Shop Pte


Ltd

Li QiMing

SGD 21.72

Receipt
Missing

11

TS-1033MG RC servo motor


X2

Singapore Hobby Shop Pte


Ltd

Lin Shaodun

SGD 32.96

Page 16

12

New motor shaft and bracket

WHITS Technologies Pte Ltd

Lin Shaodun

SGD 53.5

Page 17

Grand Total SGD 1608.5

TM3101 Mechanical Engineering Design

70

Faculty of Engineering
Department of Mechanical Engineering

Appendix C Concepts

Concepts Comparison

Sensor to
detect the
stage height

This joint will make


sure the gripper suits
to the stage height

Gap
Sensor to detect
the stage height

A 3-DOF Arm

For single flipping arm concept,


if you try to tilt the arm to suit
the stage height difference, the
Can will be tilted
Overshoot

A simple Flipping Arm (2-DOF)

TM3101 Mechanical Engineering Design

A simple Flipping Arm (2-DOF)

71

Faculty of Engineering
Department of Mechanical Engineering

Appendix D Detailed Drawings


6v to 12v PCB

Arm Shafts

TM3101 Mechanical Engineering Design

72

Faculty of Engineering
Department of Mechanical Engineering

Arm

TM3101 Mechanical Engineering Design

73

Faculty of Engineering
Department of Mechanical Engineering

Base Plates

TM3101 Mechanical Engineering Design

74

Faculty of Engineering
Department of Mechanical Engineering

Battery Holder

TM3101 Mechanical Engineering Design

75

Faculty of Engineering
Department of Mechanical Engineering

Bearing Plates - A

TM3101 Mechanical Engineering Design

76

Faculty of Engineering
Department of Mechanical Engineering

Bearing Plates - B

Bearing Plate - C

TM3101 Mechanical Engineering Design

77

Faculty of Engineering
Department of Mechanical Engineering

Bearing Shaft A & B

TM3101 Mechanical Engineering Design

78

Faculty of Engineering
Department of Mechanical Engineering

Bearing Shaft C

Collar

TM3101 Mechanical Engineering Design

79

Faculty of Engineering
Department of Mechanical Engineering

Distance Sensor Bracket

Jaw Flat

TM3101 Mechanical Engineering Design

80

Faculty of Engineering
Department of Mechanical Engineering

Gripper Gear & Gripper

TM3101 Mechanical Engineering Design

81

Faculty of Engineering
Department of Mechanical Engineering

Jaw & Line Tracking Sensor Mounting

TM3101 Mechanical Engineering Design

82

Faculty of Engineering
Department of Mechanical Engineering

Link Plate & Mounting Bracket

TM3101 Mechanical Engineering Design

83

Faculty of Engineering
Department of Mechanical Engineering

PCB Cover & Mounting

TM3101 Mechanical Engineering Design

84

Faculty of Engineering
Department of Mechanical Engineering

PCB Post and PCB

TM3101 Mechanical Engineering Design

85

Faculty of Engineering
Department of Mechanical Engineering

Pulley Adjust Block & Robot Arm

TM3101 Mechanical Engineering Design

86

Faculty of Engineering
Department of Mechanical Engineering

Robot Chassis & Robot

TM3101 Mechanical Engineering Design

87

Faculty of Engineering
Department of Mechanical Engineering

Round Nut and Sensor Mounting

TM3101 Mechanical Engineering Design

88

Faculty of Engineering
Department of Mechanical Engineering

Servo Motor Shaft

TM3101 Mechanical Engineering Design

89

Faculty of Engineering
Department of Mechanical Engineering

SG 90 Bracket

TM3101 Mechanical Engineering Design

90

Faculty of Engineering
Department of Mechanical Engineering

SG90 Coupling & Spacer

TM3101 Mechanical Engineering Design

91

Faculty of Engineering
Department of Mechanical Engineering

Theta Plate & Shaft

TM3101 Mechanical Engineering Design

92

Faculty of Engineering
Department of Mechanical Engineering

Timing Belt Gear

TM3101 Mechanical Engineering Design

93

Faculty of Engineering
Department of Mechanical Engineering

Touch Sensor Mounting & TS1033 Coupling

TM3101 Mechanical Engineering Design

94

Faculty of Engineering
Department of Mechanical Engineering

Weight

TM3101 Mechanical Engineering Design

95

Faculty of Engineering
Department of Mechanical Engineering

Appendix E Program Codes


Assumption on platform height: Left platform is lower than right one
a

Robot moves straight (arm and gripper are ready to pick after the pole) about 1~2 secs.

Grip the can once the robot stop moving about 0.5-1 sec.

Command the arm lift up and rotate at the same time to the right platform about 1-2secs.

Release the can about 0.5 sec or lower down the arm with distance sensor to detect the
height and place the can nicely about 1-2 secs.

The robot may able to complete the task about 7 secs.

Final Testing Programming Codes


#include<motor.h>
#include<adc.h>
#include<servop.h>
// Lcd pinout settings
sbit LCD_RS at RD2_bit;
sbit LCD_EN at RD3_bit;
sbit LCD_D7 at RD7_bit;
sbit LCD_D6 at RD6_bit;
sbit LCD_D5 at RD5_bit;
sbit LCD_D4 at RD4_bit;
// Pin direction
sbit LCD_RS_Direction
sbit LCD_EN_Direction
sbit LCD_D7_Direction
sbit LCD_D6_Direction
sbit LCD_D5_Direction
sbit LCD_D4_Direction

at
at
at
at
at
at

TRISD2_bit;
TRISD3_bit;
TRISD7_bit;
TRISD6_bit;
TRISD5_bit;
TRISD4_bit;

char i;
int Adc;
#include<mfunction.h>
unsigned int j;
int i_t;
char txt_THETA[4];
char txt_L_R[4];
char txt_DIST[4];
int x;
int x_L;
int x_R;
int x_B;
int x_S;
int x_D1,x_D2;
int x_SERVO;
int x_L_R;
int x_THETA;
int x_REPOS;
int x_DELAYC;
TM3101 Mechanical Engineering Design

96

Faculty of Engineering
Department of Mechanical Engineering
int x_NHEIGHT;
int x_DIFF;
int k;
char data_[6];
void THETA_LEFT()
{
for(j=42;j<65;j++)
//qiming 61
{
for(x_SERVO=2;x_SERVO>0;x_SERVO--)
{
PORTB.F4=1;
PORTB.F5=1;
PORTC.F5=1;
Delay_us(700);
PORTB.F5=0;
SERVO_P_NEW(j-28);
PORTC.F5=0;
SERVO_P_NEW(80-j);
//80-j
PORTB.F4=0;
Delay_ms(19); //20
Delay_us(950);
}
ADCON0=0b11010001; //left sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_L= (ADRESH*4)+(ADRESL/64);
ADCON0=0b11010101; //right sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_R= (ADRESH*4)+(ADRESL/64);
ADCON0=0b11001101; //behind sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_B= (ADRESH*4)+(ADRESL/64);
if((x_L>700) & (x_R>700) & (x_L_R!=5))
{
x_L_R=4;
}
if(x_L_R==4)//((x_B<80) & (x_L_R==4))
{
Delay_ms(80);
Motor_Brake_New();
Motor_Stop();
x_L_R=5;
}
}
}
void main()
{
ANSELH.F3=0;
ANSEL.F1=1;
TRISB.F4=0;
TRISB.F5=0;
TRISC.F5=0;
TRISA.F5=1;
TRISE.F0=1;
TRISA.F3=1;
TRISA.F2=1;
TM3101 Mechanical Engineering Design

97

Faculty of Engineering
Department of Mechanical Engineering
TRISA.F0=1;
TRISA.F1=1;
/**********testing**********/
Lcd_Init();
ANSEL = 0xFF; // PORTA ==> Analog
TRISA = 0xFF; // PORTA ==> input
Lcd_Cmd(_LCD_CLEAR);
Lcd_Cmd(_LCD_CURSOR_OFF); // LCD cursor off
//Lcd_Out(1,1,"Initializing..."); // Show Text
/***************************/
while(PORTA.F4); //press button
Forward(255);
x_L_R=0;
x_THETA=0;
Motor_Init();
Change_Duty(255);
Motor_Brake();
Delay_ms(300);
Lcd_Cmd(_LCD_CLEAR);
Lcd_Out(1,1,"Home Pos");
Init_Home();
//press button again
Forward(255); //MOVE
Lcd_Cmd(_LCD_CLEAR);
Lcd_Out(1,1,"Move,Arm low");
/****Smart auto-sensing & comparing system****/
x_THETA=ARM_MOTION_DOWN();//ARM LOWER DOWN
Lcd_Cmd(_LCD_CLEAR);
while(1)
{
ADCON0=0b11001001; //side sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_S=(ADRESH*4)+(ADRESL/64);
if(x_THETA==0)
ARM_MOTION_DOWN_HOLD();
if((x_S<100) & (x_THETA==0))
{
x_THETA=1;
ARM_MOTION_DOWN_HOLD();
ADCON0=0b11000001; //distance sensor1
ADCON0.GO=1;
while(ADCON0.GO);
x_NHEIGHT= (ADRESH*4)+(ADRESL/64);
}
if((x_THETA==1) & (x_S<500))
ARM_MOTION_DOWN_HOLD();
if((x_THETA==1) & (x_S>500))
{
x_THETA=2;//reflection tape
ARM_MOTION_DOWN_HOLD();
}
if((x_THETA==2) & (x_S>200))
ARM_MOTION_DOWN_HOLD();
if((x_THETA==2) & (x_S<200))
{
x_THETA=3;
ARM_MOTION_DOWN_HOLD();
}
if((x_THETA==3) & (x_S<500))
ARM_MOTION_DOWN_HOLD();
TM3101 Mechanical Engineering Design

98

Faculty of Engineering
Department of Mechanical Engineering
if((x_THETA==3) & (x_S>500))
{
THETA_LEFT();
x_THETA=4;
}
if(x_THETA==4)
THETA_LEFT_HOLD_OPEN();
//Motor_Brake();
ADCON0=0b11010001; //left sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_L= (ADRESH*4)+(ADRESL/64);
ADCON0=0b11010101; //right sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_R= (ADRESH*4)+(ADRESL/64);
/****Line Tracking****/
PWM1_Set_Duty(255);
// Motor A
PWM2_Set_Duty(255);
// Motor B
if((x_L<200) & (x_R>700))
{
PWM1_Set_Duty(255);
// Motor A
PWM2_Set_Duty(0);
// Motor B
}
if((x_L>700) & (x_R>200))
{
PWM1_Set_Duty(0);
// Motor A
PWM2_Set_Duty(255);
// Motor B
}
/*********************/
if(x_L_R==5)
{
WordToStr(x_THETA,txt_THETA);
Lcd_Out(1,1,txt_THETA);
WordToStr(x_L_R,txt_L_R);
Lcd_Out(1,5,txt_L_R);
break;
}
if((x_L_R==0) & (x_L>700) & (x_R>700))
x_L_R=1;
if((x_L_R==1) & ((x_L<100) || (x_R<100)))
x_L_R=2;
if((x_L_R==2) & (x_L>700) & (x_R>700))
x_L_R=3;
if(x_L_R==3)// & (x_B<80))
{
Delay_ms(80);
Motor_Brake_New();
Motor_Stop();
x_L_R=5;
WordToStr(x_THETA,txt_THETA);
Lcd_Out(1,1,txt_THETA);
WordToStr(x_L_R,txt_L_R);
Lcd_Out(1,5,txt_L_R);
break;
}
WordToStr(x_THETA,txt_THETA);
Lcd_Out(1,1,txt_THETA);
WordToStr(x_L_R,txt_L_R);
Lcd_Out(1,5,txt_L_R);
}
TM3101 Mechanical Engineering Design

99

Faculty of Engineering
Department of Mechanical Engineering
while(1)
{
THETA_LEFT_HOLD_OPEN();
ADCON0=0b11001101; //behind sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_B= (ADRESH*4)+(ADRESL/64);
if((x_L_R==4) || (x_L_R==5))//(x_L_R==5)
break;
if(x_B<80)
{
Motor_Brake_New();
Motor_Stop();
break;
}
}
x_DIFF=0;
x=0;
while(x_DIFF<200)
{
ADCON0=0b11000001; //distance sensor1
ADCON0.GO=1;
while(ADCON0.GO);
x_D1= (ADRESH*4)+(ADRESL/64);
ADCON0=0b11011001; //distance sensor2
ADCON0.GO=1;
while(ADCON0.GO);
x_D2= (ADRESH*4)+(ADRESL/64);
Lcd_Cmd(_LCD_CLEAR);
WordToStr(x_D1,txt_DIST);
Lcd_Out(1,9,txt_DIST);
x_DIFF=x_D1-x_NHEIGHT;
ADCON0=0b11001101; //behind sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_B= (ADRESH*4)+(ADRESL/64);
if(x_D2>400)
break;
if((x_DIFF>200) && (x_B<100)) //& (x_D2>100))
break;
if(x_B<100) //& (x_D2>100)) //240
break;
if(x==0)
{
Forward(255);
x=1;
}
}
if(x==1)
{
Motor_Brake_New();
Motor_Stop();
}

//240

/*
THETA_LEFT_HOLD_OPEN();
ADCON0=0b11000001; //distance sensor1
ADCON0.GO=1;
while(ADCON0.GO);
x_D1= (ADRESH*4)+(ADRESL/64);
ADCON0=0b11011001; //distance sensor2
ADCON0.GO=1;
TM3101 Mechanical Engineering Design

100

Faculty of Engineering
Department of Mechanical Engineering
while(ADCON0.GO);
x_D2= (ADRESH*4)+(ADRESL/64);
Lcd_Cmd(_LCD_CLEAR);
WordToStr(x_D1,txt_DIST);
Lcd_Out(1,9,txt_DIST);
x_DIFF=x_D1-x_NHEIGHT;
if(x_DIFF>150) //& (x_D2>100)) //240
break;
}
*/
Lcd_Cmd(_LCD_CLEAR);
Lcd_Out(1,1,"Gripper Close");
k = GPR_CLOSE();
Lcd_Cmd(_LCD_CLEAR);
Lcd_Out(1,1,"Arm Right");
//THETA_RIGHT();
THETA_RIGHT_noRC5(k);
Lcd_Cmd(_LCD_CLEAR);
Lcd_Out(1,1,"Open, Done");
//Lcd_Out(2,1, "Task Completed!");
GPR_OPEN(x_NHEIGHT,k);
while(1);
/*
while(1)
{
PORTC.F5=1;
//THETA
PORTB.F4=1;
//ARM
PORTB.F5=1;
//GRIPPER
Delay_us(1000);
PORTC.F5=0;
Delay_us(200);
PORTB.F5=0;
Delay_us(700);
PORTB.F4=0;
Delay_us(2000);
}
while(PORTA.F4)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(1500);
PORTB.F5=0;
PORTB.F4=0;
Delay_us(550);
PORTC.F5=0;
Delay_ms(20);
}
ARM_MOTION_DOWN();
while(1)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(1200);
PORTB.F5=0;
Delay_us(300);
PORTC.F5=0;
Delay_us(300);
PORTB.F4=0;
TM3101 Mechanical Engineering Design

101

Faculty of Engineering
Department of Mechanical Engineering
Delay_ms(20);
}
*/
/*
while(PORTA.F4) //arm
{
//PORTC.F5=1;
PORTB.F4=1;
//PORTB.F5=1;
//Delay_us(600);
//600
//PORTB.F5=0;
Delay_us(1500);
//600
PORTB.F4=0;
//Delay_us(2050);
//400
//PORTC.F5=0;
Delay_ms(20);
//400us
}
while(PORTA.F4)//theta
{
PORTC.F5=1;
//PORTB.F4=1;
//PORTB.F5=1;
//Delay_us(600);
//600
//PORTB.F5=0;
//Delay_us(600);
//600
//PORTB.F4=0;
Delay_us(2050);
//400
PORTC.F5=0;
Delay_ms(20);
//400us
}
//Delay_ms(1000);
//ARM_MOTION_UP(0,3);
//while(PORTA.F4)
while(1)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(800);
PORTC.F5=0;
Delay_us(400);
PORTB.F4=0;
Delay_us(300);
PORTB.F5=0;
Delay_us(500);
//PICK_POS2_GPR();
// Delay_ms(1000);
// HOME_GPR();
//
Delay_ms(1000);
}
while(PORTA.F4);
Motor_Init();
Change_Duty(255);
Motor_Brake();
Forward(255);
//Delay_ms(3000);
//Motor_Brake();
//while(PORTA.F4);
//Delay_ms(1000);

TM3101 Mechanical Engineering Design

102

Faculty of Engineering
Department of Mechanical Engineering
//ARM_MOTION_DOWN(4,0);
Motor_Brake();
while(PORTA.F4)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(600);
PORTB.F5=0;
Delay_us(600);
PORTB.F4=0;
Delay_us(400);
PORTC.F5=0;
Delay_us(400);
}
//HOME_GPR();
*/
/*while(PORTA.F4)
{
PORTB.F5=1;
Delay_us(1300);
PORTB.F5=0;
Delay_us(700);
}
*/
/*
for(i=200;i>0;i--)
{
PORTB.F4=1;
PORTB.F5=1;
Delay_us(1200);
PORTB.F4=0;
Delay_us(300);
PORTB.F5=0;
Delay_us(500);
}
ARM_MOTION_UP(0,3);
while(PORTA.F4)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(800);
PORTC.F5=0;
Delay_us(400);
PORTB.F4=0;
Delay_us(300);
PORTB.F5=0;
Delay_us(500);
//PICK_POS2_GPR();
// Delay_ms(1000);
// HOME_GPR();
//
Delay_ms(1000);
}
Delay_ms(1000);
PORTB.F5=0;
while(PORTA.F4)
{
PORTC.F5=1;
TM3101 Mechanical Engineering Design

103

Faculty of Engineering
Department of Mechanical Engineering
PORTB.F4=1;
//PORTB.F5=1;
Delay_us(800);
PORTC.F5=0;
Delay_us(400);
//PORTB.F5=0;
Delay_us(50);
PORTB.F4=0;
Delay_us(750);
}
while(PORTA.F4);
while(1)
{
// PICK_POS2();
Delay_ms(1000);
// PLACE_POS2();
Delay_ms(1000);
}
while(PORTA.F4)
{
Init_Home();
//HOME_THETA();
//PICK_POS1_ARM();
//HOME_GPR();
//Delay_ms(1000);
//PICK_POS1_GPR();
//PICK_POS1_THETA();
//Delay_ms(1000);
//for(j=1000;j>0;j--)
//PICK();
}
Delay_ms(1000);
//Forward(255);
while(PORTA.F4)
{
//Test();
}
//Motor_Stop();
Delay_ms(1000);
while(PORTA.F4);
while(1)
{
// PICK_POS1_ARM();
ADCON0=0b11001001;
ADCON0.GO=1;
while(ADCON0.GO);
x_S =(ADRESH*4)+(ADRESL/64);
if(x_S<600)
break;
}

//RA2

//PICK_POS2();
while((x_L<600 || x_R<600) && x_D<100)
TM3101 Mechanical Engineering Design

104

Faculty of Engineering
Department of Mechanical Engineering
{
// PICK_POS1_ARM();
ADCON0=0b11010001;
ADCON0.GO=1;
while(ADCON0.GO);
x_L =(ADRESH*4)+(ADRESL/64);
ADCON0=0b11010101;
ADCON0.GO=1;
while(ADCON0.GO);
x_R =(ADRESH*4)+(ADRESL/64);
ADCON0=0b11001101;
ADCON0.GO=1;
while(ADCON0.GO);
x_B =(ADRESH*4)+(ADRESL/64);
ADCON0=0b11000001;
ADCON0.GO=1;
while(ADCON0.GO);
x_D =(ADRESH*4)+(ADRESL/64);
}

//RA5

//RE0

//RA3

//RA0

Motor_Stop();
while(PORTA.F1);
while(PORTA.F4);
Delay_ms(1000);
Lcd_Init();
ANSEL = 0xFF; // PORTA ==> Analog
TRISA = 0xFF; // PORTA ==> input
Lcd_Cmd(_LCD_CURSOR_OFF); // LCD cursor off
Lcd_Out(1,1,"SENSOR1 = "); // Show Text
ADCON0=0b11000001; // Select Analog1 RC_Mode and ADON
while(1)
{
ADCON0.GO=1;
while(ADCON0.GO);
x= (ADRESH*4)+(ADRESL/64);
WordToStr(x,data_);
Lcd_Out(1,10,data_);
Delay_ms(100);
}
*/
}
void SERVO_P_NEW(int k)
{
switch(k){
case 0:
break;
case 1:
Delay_us(25);
break;
case 2:
Delay_us(50);
break;
case 3:
Delay_us(75);
break;
case 4:
Delay_us(100);
break;
TM3101 Mechanical Engineering Design

105

Faculty of Engineering
Department of Mechanical Engineering
case 5:
Delay_us(125);
break;
case 6:
Delay_us(150);
break;
case 7:
Delay_us(175);
break;
case 8:
Delay_us(200);
break;
case 9:
Delay_us(225);
break;
case 10:
Delay_us(250);
break;
case 11:
Delay_us(275);
break;
case 12:
Delay_us(300);
break;
case 13:
Delay_us(325);
break;
case 14:
Delay_us(350);
break;
case 15:
Delay_us(375);
break;
case 16:
Delay_us(400);
break;
case 17:
Delay_us(425);
break;
case 18:
Delay_us(450);
break;
case 19:
Delay_us(475);
break;
case 20:
Delay_us(500);
break;
case 21:
Delay_us(525);
break;
case 22:
Delay_us(550);
break;
case 23:
Delay_us(575);
break;
case 24:
Delay_us(600);
break;
case 25:
TM3101 Mechanical Engineering Design

106

Faculty of Engineering
Department of Mechanical Engineering

case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case
case

Delay_us(625);
break;
26:
Delay_us(650);
break;
27:
Delay_us(675);
break;
28:
Delay_us(700);
break;
29:
Delay_us(725);
break;
30:
Delay_us(750);
break;
31:
Delay_us(775);
break;
32:
Delay_us(800);
break;
33:
Delay_us(825);
break;
34:
Delay_us(850);
break;
35:
Delay_us(875);
break;
36:
Delay_us(900);
break;
37:
Delay_us(925);
break;
38:
Delay_us(950);
break;
39:
Delay_us(975);
break;
40:
Delay_us(1000);
break;
41:
Delay_us(1025);
break;
42:
Delay_us(1050);
break;
43:
Delay_us(1075);
break;
44:
Delay_us(1100);
break;
45:
Delay_us(1125);

TM3101 Mechanical Engineering Design

107

Faculty of Engineering
Department of Mechanical Engineering
break;
case 46:
Delay_us(1150);
break;
case 47:
Delay_us(1175);
break;
case 48:
Delay_us(1200);
break;
case 49:
Delay_us(1225);
break;
case 50:
Delay_us(1250);
break;
case 51:
Delay_us(1275);
break;
case 52:
Delay_us(1300);
break;
case 53:
Delay_us(1325);
break;
case 54:
Delay_us(1350);
break;
case 55:
Delay_us(1375);
break;
case 56:
Delay_us(1400);
break;
case 57:
Delay_us(1425);
break;
case 58:
Delay_us(1450);
break;
case 59:
Delay_us(1475);
break;
case 60:
Delay_us(1500);
break;
case 61:
Delay_us(1525);
break;
case 62:
Delay_us(1550);
break;
case 63:
Delay_us(1575);
break;
case 64:
Delay_us(1600);
break;
case 65:
Delay_us(1625);
break;
TM3101 Mechanical Engineering Design

108

Faculty of Engineering
Department of Mechanical Engineering
case 66:
Delay_us(1650);
break;
case 67:
Delay_us(1675);
break;
case 68:
Delay_us(1700);
break;
case 69:
Delay_us(1725);
break;
case 70:
Delay_us(1750);
break;
case 71:
Delay_us(1775);
break;
case 72:
Delay_us(1800);
break;
case 73:
Delay_us(1825);
break;
case 74:
Delay_us(1850);
break;
case 75:
Delay_us(1875);
break;
case 76:
Delay_us(1900);
break;
case 77:
Delay_us(1925);
break;
case 78:
Delay_us(1950);
break;
case 79:
Delay_us(1975);
break;
case 80:
Delay_us(2000);
break;
case 81:
Delay_us(2025);
break;
case 82:
Delay_us(2050);
break;
case 83:
Delay_us(2075);
break;
case 84:
Delay_us(2100);
break;
case 85:
Delay_us(2125);
break;
case 86:
TM3101 Mechanical Engineering Design

109

Faculty of Engineering
Department of Mechanical Engineering
Delay_us(2150);
break;
case 87:
Delay_us(2175);
break;
case 88:
Delay_us(2200);
break;
case 89:
Delay_us(2225);
break;
case 90:
Delay_us(2250);
break;
case 91:
Delay_us(2275);
break;
case 92:
Delay_us(2300);
break;
default :
break;
}
}
/*
void SERVO_P(int k)
{
switch(k){
case 0:
break;
case 1:
Delay_us(100);
break;
case 2:
Delay_us(200);
break;
case 3:
Delay_us(300);
break;
case 4:
Delay_us(400);
break;
case 5:
Delay_us(500);
break;
case 6:
Delay_us(600);
break;
case 7:
Delay_us(685);
break;
case 8:
Delay_us(785);
break;
case 9:
Delay_us(885);
break;
case 10:
Delay_us(985);
break;
case 11:
TM3101 Mechanical Engineering Design

110

Faculty of Engineering
Department of Mechanical Engineering
Delay_us(1080);
break;
case 12:
Delay_us(1180);
break;
case 13:
Delay_us(1280);
break;
case 14:
Delay_us(1370);
break;
case 15:
Delay_us(1470);
break;
case 16:
Delay_us(1570);
break;
case 17:
Delay_us(1670);
break;
case 18:
Delay_us(1770);
break;
case 19:
Delay_us(1870);
break;
case 20:
Delay_us(1960);
default :
break;
}
}
*/
/*
void Read_Adc()
{
//ADCON0=0b11000001; // Select Analog2 RC_Mode and ADON
ADCON0.GO=1; // Start Convert
while(ADCON0.GO); // Wait Until Convert Complete
Adc=(ADRESH*4)+(ADRESL/64); // 10 bit Data ==> Adc
}
void HOME_THETA()
{
for(i=5000;i>0;i--)
{
PORTC.F5=1;
Delay_us(1720);
PORTC.F5=0;
Delay_us(280);
}
}

//-90

void HOME_ARM()
{
for(i=200;i>0;i--)
{
PORTB.F4=1;
Delay_us(800);

//90

TM3101 Mechanical Engineering Design

111

Faculty of Engineering
Department of Mechanical Engineering
PORTB.F4=0;
Delay_us(1200);
}
}
void HOME_GPR()
{
for(i=200;i>0;i--)
{
PORTB.F5=1;
Delay_us(600);
PORTB.F5=0;
Delay_us(1400);
}
}

//90

void PICK_POS1_THETA()
{
for(i=5000;i>0;i--)
{
PORTC.F5=1;
Delay_us(1000);
PORTC.F5=0;
Delay_us(1000);
}
}

//0

void PICK_POS1_ARM()
{
//for(i=200;i>0;i--)
//{
PORTB.F4=1;
Delay_us(800);
PORTB.F4=0;
Delay_us(1200);
//}
}

//30

void PICK_POS1_GPR()
{
for(i=200;i>0;i--)
{
PORTB.F5=1;
Delay_us(800);
PORTB.F5=0;
Delay_us(1200);
}
}

//45

void PICK_POS2_THETA()
{
for(i=200;i>0;i--)
{
PORTC.F5=1;
Delay_us(1500);
PORTC.F5=0;
Delay_us(500);
}
}

//0

void PICK_POS2_ARM()

//30

TM3101 Mechanical Engineering Design

//max over from 550

112

Faculty of Engineering
Department of Mechanical Engineering
{
for(i=200;i>0;i--)
{
PORTB.F4=1;
Delay_us(1650);
PORTB.F4=0;
Delay_us(250);
}
}
void PICK_POS2_GPR()
{
for(i=200;i>0;i--)
{
PORTB.F5=1;
Delay_us(1300);
PORTB.F5=0;
Delay_us(700);
}
}
*/
void Init_Home()
{
while(PORTA.F4)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(1500);
PORTB.F5=0;
PORTB.F4=0;
Delay_us(550);
PORTC.F5=0;
Delay_ms(20);
}
}
/*
void HOME_HOLD()
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(1500);
PORTB.F5=0;
PORTB.F4=0;
Delay_us(550);
PORTC.F5=0;
Delay_ms(20);
}

//45

void Test()
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(1200);
PORTB.F4=0;
Delay_us(200);
PORTC.F5=0;
PORTB.F5=0;
//Delay_us(100);
TM3101 Mechanical Engineering Design

113

Faculty of Engineering
Department of Mechanical Engineering
//PORTB.F4=0;
Delay_us(600);
}
void PICK_POS1()
{
PICK_POS1_THETA();
PICK_POS1_ARM();
PICK_POS1_GPR();
}

//POS 0 DEG
//POS 30 DEG
//POS 45 DEG

void PICK_POS2()
{
for(i=200;i>0;i--)
{
PORTC.F5=1;
Delay_us(1650);
PORTC.F5=0;
Delay_us(350);
}
}
void PLACE_POS2()
{
for(i=200;i>0;i--)
{
PORTC.F5=1;
Delay_us(800);
PORTC.F5=0;
Delay_us(1200);
}
}
void PICK()
{
PORTB.F5=1;
Delay_us(1500);
PORTB.F5=0;
Delay_us(500);
}
void ARM_MOTION_DOWN_OLD(int l,int m)
{
int j;
int x_SERVO;
for(j=l;j>=m;j--)
{
for(x_SERVO=100;x_SERVO>0;x_SERVO--)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(600);
PORTB.F5=0;
SERVO_P(6-j);
PORTB.F4=0;
SERVO_P(4+j);
PORTC.F5=0;
SERVO_P(4);
}
}
TM3101 Mechanical Engineering Design

114

Faculty of Engineering
Department of Mechanical Engineering
}
*/
int ARM_MOTION_DOWN()
{
int x_S,x_THETA=0;
int j;
int x_SERVO;
for(j=10;j>0;j--)
{
PORTC.F5=1;
//THETA
PORTB.F4=1;
//ARM
PORTB.F5=1;
//GRIPPER
Delay_us(700);
PORTB.F5=0;
Delay_us(350);
PORTC.F5=0;
Delay_us(450);
PORTB.F4=0;
Delay_ms(20);
}
for(j=17;j<41;j++)
{
for(x_SERVO=2;x_SERVO>0;x_SERVO--)
{
PORTC.F5=1;
//THETA
PORTB.F4=1;
//ARM
PORTB.F5=1;
//GRIPPER
Delay_us(700);
PORTB.F5=0;
Delay_us(350);
PORTC.F5=0;
SERVO_P_NEW(j);
PORTB.F4=0;
Delay_ms(20);
ADCON0=0b11001001; //side sensor
ADCON0.GO=1;
while(ADCON0.GO);
x_S=(ADRESH*4)+(ADRESL/64);
if((x_S<100) & (x_THETA==0))
{
x_THETA=1;
}
if((x_THETA==1) & (x_S<500));
if((x_THETA==1) & (x_S>500))
{
x_THETA=2;//reflection tape
}
if((x_THETA==2) & (x_S>200));
if((x_THETA==2) & (x_S<200))
{
x_THETA=3;
}
}
}
return x_THETA;
}
void ARM_MOTION_DOWN_HOLD()
{
PORTC.F5=1;
//THETA
PORTB.F4=1;
//ARM
TM3101 Mechanical Engineering Design

115

Faculty of Engineering
Department of Mechanical Engineering
PORTB.F5=1;
Delay_us(700);
PORTB.F5=0;
Delay_us(350);
PORTC.F5=0;
Delay_us(1100);
PORTB.F4=0;
Delay_ms(19);
Delay_us(950);

//GRIPPER

int THETA_LEFT_HOLD_HCLOSE()
{
int x;
int j;
int k=1;
int x_D2;
int m;
for(x=0;x<50;x++)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(700);
PORTB.F5=0;
Delay_us(900);
//qiming800
PORTC.F5=0;
Delay_us(550);
//qiming650
PORTB.F4=0;
Delay_ms(20);
}
/****Auto compensation sensing and gripping system****/
ADCON0=0b11011001; //distance sensor2
while(1)
{
ADCON0.GO=1;
while(ADCON0.GO);
x_D2= (ADRESH*4)+(ADRESL/64);
if(x_D2>=400)
break;
if(x_D2<400)
{
for(m=30;m>0;m--)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(700);
PORTB.F5=0;
SERVO_P_NEW(36-k);
//qiming32-k
PORTC.F5=0;
SERVO_P_NEW(18+k);
//qiming22+k
PORTB.F4=0;
Delay_ms(20);
}
}
k++;
}
if(k!=1);
TM3101 Mechanical Engineering Design

116

Faculty of Engineering
Department of Mechanical Engineering
{
for(x=28;x<55;x++)
{
for(j=2;j>0;j--)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
SERVO_P_NEW(x);
PORTB.F5=0;
SERVO_P_NEW(64-k-x);
//qiming60-k-x
PORTC.F5=0;
SERVO_P_NEW(18+k);
//qiming22+k
PORTB.F4=0;
Delay_ms(20);
}
}
//qimingk=60-k;
return k;
}
if(k==1)
//qiming k==1
{
for(x=28;x<55;x++)
{
for(j=5;j>0;j--)
//qiming 2
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
SERVO_P_NEW(x);
PORTB.F5=0;
SERVO_P_NEW(64-x);
//qiming 60-x
PORTC.F5=0;
Delay_us(450);
//qiming550
PORTB.F4=0;
Delay_ms(20);
}
}
}
}
void THETA_LEFT_HOLD_CLOSE(int k)
{
int m;
for(m=100;m>0;m--)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
SERVO_P_NEW(62-k); //qiming k
PORTC.F5=0;
SERVO_P_NEW(4+k);
//qiming 68-k
PORTB.F5=0;
Delay_us(350);
PORTB.F4=0;
Delay_ms(20);
}
}
void THETA_LEFT_HOLD_OPEN()
{
TM3101 Mechanical Engineering Design

117

Faculty of Engineering
Department of Mechanical Engineering
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(700);
PORTB.F5=0;
Delay_us(900);
PORTC.F5=0;
Delay_us(550);
PORTB.F4=0;
Delay_ms(20);

//qiming800
//qiming650

}
/*
void THETA_RIGHT()
{
int j;
int x_SERVO;
for(j=51;j>20;j--)
{
for(x_SERVO=1000;x_SERVO>0;x_SERVO--)
{
PORTC.F5=1;
PORTB.F5=1;
PORTB.F4=1;
SERVO_P_NEW(j);
PORTC.F5=0;
SERVO_P_NEW(55-j);
PORTB.F5=0;
Delay_us(500);
PORTB.F4=0;
Delay_us(2000);
}
}
for(j=21;j>14;j--)
{
for(x_SERVO=1000;x_SERVO>0;x_SERVO--)
{
PORTC.F5=1;
PORTB.F5=1;
PORTB.F4=1;
SERVO_P_NEW(j);
PORTC.F5=0;
SERVO_P_NEW(55-j);
PORTB.F5=0;
SERVO_P_NEW(32-j);
PORTB.F4=0;
Delay_us(2000);
}
}
}
*/
void THETA_RIGHT_noRC5(int k)
{
int j;
int x_SERVO;
int x_SERVO2;
x_SERVO2=58;
for(j=20;j>11;j=j--)
{
x_SERVO2=x_SERVO2-2;
for(x_SERVO=30;x_SERVO>0;x_SERVO--)
{
TM3101 Mechanical Engineering Design

//4

118

Faculty of Engineering
Department of Mechanical Engineering
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
SERVO_P_NEW(x_SERVO2+k);
PORTC.F5=0;
//PORTB.F5=0;
SERVO_P_NEW(62-x_SERVO2-k);
//PORTC.F5=0;
//Delay_us(1500);
PORTB.F5=0;
//Delay_us(50); QIMING
//Delay_us(500);
SERVO_P_NEW(j);
PORTB.F4=0;
Delay_ms(20);
}
}
x_SERVO2=40;
//32
for(j=16;j<22;j++)
{
if((j!=16) & (j!=21))
x_SERVO2=x_SERVO2-4;
if(j==21)
x_SERVO2=24;
for(x_SERVO=50;x_SERVO>0;x_SERVO--)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
SERVO_P_NEW(x_SERVO2-k);
PORTC.F5=0;
//PORTB.F5=0;
SERVO_P_NEW(43-x_SERVO2+k);
Delay_us(400);
PORTB.F5=0;
//Delay_us(20);
//PORTC.F5=0;
SERVO_P_NEW(j);
PORTB.F4=0;
Delay_ms(20);
}
}
}

//+9
//qiming60 -x_SERVO2

//+9

//15

//qiming+k
//qiming-k

//+9

int GPR_CLOSE()
{
int x;
int k;
ADCON0=0b11000101; // Select Analog1 RC_Mode and ADON
k=THETA_LEFT_HOLD_HCLOSE();
// while(1)
//{
//for(x=10;x>0;x--)
//{
/*
PORTB.F4=1;
PORTB.F5=1;
Delay_us(1600);
PORTB.F5=0;
Delay_us(550);
TM3101 Mechanical Engineering Design

119

Faculty of Engineering
Department of Mechanical Engineering
PORTB.F4=0;
Delay_ms(20);
*/
//}
THETA_LEFT_HOLD_CLOSE(k);
/*
ADCON0.GO=1;
while(ADCON0.GO);
x= (ADRESH*4)+(ADRESL/64);
if(x<900)
break;
}
*/
/*
for(x=10;x>0;x--)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(100);
PORTC.F5=0;
Delay_us(100);
PORTB.F5=0;
//Delay_us(100);
//PORTC.F5=0;
Delay_us(550);
PORTB.F4=0;
Delay_ms(20);
}
*/
return k;
}
void GPR_OPEN(int x_NHEIGHT, int k)
{
int x;
if(k!=1)
{
for(x=30;x>0;x--)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
SERVO_P_NEW(25-k);//qiming-k
PORTC.F5=0;
SERVO_P_NEW(35+k);//qiming870
+k
PORTB.F5=0;
Delay_us(650);
PORTB.F4=0;
Delay_ms(20);
}
while(PORTA.F4)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
SERVO_P_NEW(25-k);//qiming-k 630
PORTC.F5=0;
SERVO_P_NEW(35+k);//qiming+k
PORTB.F5=0;
Delay_us(800);
TM3101 Mechanical Engineering Design

120

Faculty of Engineering
Department of Mechanical Engineering
PORTB.F4=0;
Delay_ms(20);
}
}
if(k==1)
{
for(x=30;x>0;x--)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(630);
PORTC.F5=0;
Delay_us(870);
PORTB.F5=0;
Delay_us(650);
PORTB.F4=0;
Delay_ms(20);
}
while(PORTA.F4)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(630);
PORTC.F5=0;
Delay_us(720);
PORTB.F5=0;
Delay_us(800);
PORTB.F4=0;
Delay_ms(20);
}
}
}
void GPR_OPEN_OLD(int x_NHEIGHT,int k) //QIMING
{
int x=1;
int x_D1;
int x_DIFF;
//int k=1;
int m;
/*
ADCON0=0b11000001; //distance sensor1
if(k!=1)
{
while(1)
{
ADCON0.GO=1;
while(ADCON0.GO);
x_D1= (ADRESH*4)+(ADRESL/64);
x_DIFF=x_D1-x_NHEIGHT;
if(x_DIFF>=250)
break;
if(x_DIFF<250)
{
for(m=30;m>0;m--)
{
PORTB.F4=1;
PORTC.F5=1;
TM3101 Mechanical Engineering Design

121

Faculty of Engineering
Department of Mechanical Engineering
PORTB.F5=1;
SERVO_P_NEW(25-k);
PORTC.F5=0;
SERVO_P_NEW(39+K);
PORTB.F5=0;
Delay_us(475);
PORTB.F4=0;
Delay_ms(20);
}
}
x++;
}
*/
/*
if(k<=23)
{
for(x=50;x>0;x--)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
SERVO_P_NEW(25-k);
PORTC.F5=0;
SERVO_P_NEW(33+K);
PORTB.F5=0;
Delay_us(500);
PORTB.F4=0;
Delay_ms(20);
}
while(PORTA.F4)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
SERVO_P_NEW(23-k);
PORTC.F5=0;
SERVO_P_NEW(28+K);
PORTB.F5=0;
Delay_us(550);
PORTB.F4=0;
Delay_ms(20);
}
}
if(k>23)
{
while(PORTA.F4)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(630);
PORTC.F5=0;
Delay_us(720);
PORTB.F5=0;
Delay_us(800);
PORTB.F4=0;
Delay_ms(20);
}
}
*/
//}
TM3101 Mechanical Engineering Design

122

Faculty of Engineering
Department of Mechanical Engineering
k=1;
if(k==1)
{
for(x=30;x>0;x--)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(630);
PORTC.F5=0;
Delay_us(870);
PORTB.F5=0;
Delay_us(650);
PORTB.F4=0;
Delay_ms(20);
}
while(PORTA.F4)
{
PORTB.F4=1;
PORTC.F5=1;
PORTB.F5=1;
Delay_us(630);
PORTC.F5=0;
Delay_us(720);
PORTB.F5=0;
Delay_us(800);
PORTB.F4=0;
Delay_ms(20);
}
}
}
/*
void ARM_MOTION_UP(int l,int m)
{
int j;
int x_SERVO;
for(j=l;j<m+1;j++)
{
for(x_SERVO=100;x_SERVO>0;x_SERVO--)
{
PORTC.F5=1;
PORTB.F4=1;
PORTB.F5=1;
Delay_us(800);
PORTC.F5=0;
SERVO_P(4-j);
PORTB.F4=0;
SERVO_P(3+j);
PORTB.F5=0;
SERVO_P(5);
}
}
for(j=m;j>=l;j--)
{
for(x_SERVO=100;x_SERVO>0;x_SERVO--)
{
PORTC.F5=1;
PORTB.F4=1;
TM3101 Mechanical Engineering Design

123

Faculty of Engineering
Department of Mechanical Engineering
PORTB.F5=1;
Delay_us(800);
PORTC.F5=0;
SERVO_P(4-j);
PORTB.F4=0;
SERVO_P(3+j);
PORTB.F5=0;
SERVO_P(5);
}
}
}
*/
/********** Stop Motor
************/
void Motor_Brake()
{
Pwm1_Start();
Pwm2_Start();
PORTB.F1 =1;
PORTB.F2 =1;
PORTD.F0 =1;
PORTD.F1 =1;
PORTC.F1 =1;
PORTC.F2 =1;
}
/************************************/
/********** Stop Motor
************/
void Motor_Brake_New()
{
//Pwm1_Start();
//Pwm2_Start();
PORTB.F1 =1;
PORTB.F2 =1;
PORTD.F0 =1;
PORTD.F1 =1;
PORTC.F1 =1;
PORTC.F2 =1;
}
/************************************/
/*
void SENSOR_TEST
{
Lcd_Init();
ANSEL = 0xFF; // PORTA ==> Analog
TRISA = 0xFF; // PORTA ==> input
Lcd_Cmd(_LCD_CURSOR_OFF); // LCD cursor off
Lcd_Out(1,1,"SENSOR1 = "); // Show Text
ADCON0=0b11010001; // Select Analog1 RC_Mode and ADON
while(PORTA.F4)
{
ADCON0.GO=1;
while(ADCON0.GO);
x= (ADRESH*4)+(ADRESL/64);
WordToStr(x,data_);
Lcd_Out(1,10,data_);
Delay_ms(100);
}
Delay_ms(500);
ADCON0=0b11001101; // Select Analog1 RC_Mode and ADON
while(PORTA.F4)
{
TM3101 Mechanical Engineering Design

124

Faculty of Engineering
Department of Mechanical Engineering
ADCON0.GO=1;
while(ADCON0.GO);
x= (ADRESH*4)+(ADRESL/64);
WordToStr(x,data_);
Lcd_Out(1,10,data_);
Delay_ms(100);
}
Delay_ms(500);
ADCON0=0b11001001; // Select Analog1 RC_Mode and ADON
while(PORTA.F4)
{
ADCON0.GO=1;
while(ADCON0.GO);
x= (ADRESH*4)+(ADRESL/64);
WordToStr(x,data_);
Lcd_Out(1,10,data_);
Delay_ms(100);
}
Delay_ms(500);
ADCON0=0b11000001; // Select Analog1 RC_Mode and ADON
while(PORTA.F4)
{
ADCON0.GO=1;
while(ADCON0.GO);
x= (ADRESH*4)+(ADRESL/64);
WordToStr(x,data_);
Lcd_Out(1,10,data_);
Delay_ms(100);
}
Delay_ms(500);
}
*/
char motor_duty_= 127;
// Defalt PWM 50%
char motor_init_=0;
// Status initial
//
//
//
//

*** Motor A
PD0 ====>
PD1 ====>
PC2 ====>

*****
1A
1B
1E (PWM1)

//
//
//
//

*** Motor B
PB1 ====>
PB2 ====>
PC1 ====>

*****
2A
2B
2E (PWM2)

//****************************************************
//********** Initial Motor Function ******************
//****************************************************
void Motor_Init()
{
if (motor_init_==0)
// First time ?
{
motor_init_=1;
// Status
ANSELH.F0=0;
// RB1 ==> Digital IO
ANSELH.F2=0;
// RB2 ==> Digital IO
TRISB.F1=0;
// Motor B 2A
TRISB.F2=0;
// Motor B 2B
TRISD.F0=0;
// Motor A 1A
TRISD.F1=0;
// MOtor A 1B
Pwm1_Init(5000);
// Initail PWM 1E
Pwm2_Init(5000);
// Initail PWM 2E
TM3101 Mechanical Engineering Design

125

Faculty of Engineering
Department of Mechanical Engineering
}
}
//****************************************************
//****************************************************
//********** Control Duty Cycle *********************
//****************************************************
void Change_Duty(char speed)
{
if (speed != motor_duty_)
// Check Same old speed
{
motor_duty_=speed;
// Save for old speed
PWM1_Set_Duty(speed);
// Motor A
PWM2_Set_Duty(speed);
// Motor B
}
}
//****************************************************
/********** Motor A Forward ********/
void Motor_A_FWD()
{
Pwm1_Start();
PORTD.F0 =0;
PORTD.F1 =1;
}
/************************************/
/********** Motor B Forward ********/
void Motor_B_FWD()
{
Pwm2_Start();
PORTB.F1 =0;
PORTB.F2 =1;
}
/************************************/
/********** Motor A Backward *******
void Motor_A_BWD()
{
Pwm1_Start();
PORTD.F0 =1;
PORTD.F1 =0;
}
************************************/
/********** Motor B Backward *******
void Motor_B_BWD()
{
Pwm2_Start();
PORTB.F1 =1;
PORTB.F2 =0;
}
************************************/
/********** Go Forward
************/
void Forward(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_FWD();
Motor_B_FWD();
TM3101 Mechanical Engineering Design

126

Faculty of Engineering
Department of Mechanical Engineering
}
/************************************/
/********** Go Backward ************
void Backward(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_BWD();
Motor_B_BWD();
}
************************************/
/********** Spin Left
*************
void S_Right(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_FWD();
Motor_B_BWD();
}
************************************/
/********** Spin Right
************
void S_Left(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_BWD();
Motor_B_FWD();
}
************************************/
/*
void T_LEFT()
{
PWM1_Set_Duty(0);
// Motor A
PWM2_Set_Duty(255);
// Motor B
}
void T_RIGHT()
{
PWM1_Set_Duty(255);
// Motor A
PWM2_Set_Duty(0);
// Motor B
}
*/
/********** Motor A Off ************/
void Motor_A_Off()
{
Pwm1_Stop();
PORTD.F0 =0;
PORTD.F1 =0;
}
/************************************/
/********** Motor B Off ************/
void Motor_B_Off()
{
Pwm2_Stop();
PORTB.F1 =0;
PORTB.F2 =0;
}
/************************************/
TM3101 Mechanical Engineering Design

127

Faculty of Engineering
Department of Mechanical Engineering

/********** Stop Motor


************/
void Motor_Stop()
{
Change_Duty(0);
//
Pwm1_Change_Duty(0);
// Motor A
//
Pwm2_Change_Duty(0);
// Motor B
Motor_A_Off();
Motor_B_Off();
motor_init_=0;
}
/************************************/
int Adc_Rd(char ch)
// Low 8 Channel ADC Read
{
int dat=0;
// Save Adc
if ((ch>=0) && (ch<=3))
// CH0-CH3
TRISA |= (1<<ch);
else if (ch==4)
// CH4
TRISA |= 0x20;
else if ((ch>=5) && (ch<=7)) // CH5-CH7
TRISE |= (1<<(ch-5));
ANSEL |=(1<<ch);
// set Channel to Analog
ADCON0 = (0xC1 + (ch*4));
// Select ADC Channel
Delay_us(10);
// Acquisiton delay
ADCON0.GO=1;
// Start conversion
while(ADCON0.GO);
// conversion done?
dat = (ADRESH*4)+(ADRESL/64); // Sum highbyte and lowbyte
return dat;
// Return Value
}

TM3101 Mechanical Engineering Design

128