TM3101 Design Project Report PDF

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
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
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
14.0
Appendices
60
Appendix A
Research
Appendix B
Costing Details
Appendix C
Other Concepts
Appendix D
Detailed Drawings
Appendix E
Program Codes
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.
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.
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
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
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.
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.
10
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

MOHAMAD
YAZID BIN
ISMAIL
Designer
1. Fabricate Parts,
LI QIMING
Software
Engineer
1. Prepare Flow Chart for Robot Control

2. Coding and Debugging using C.
Detail Mechanical design,

Prepare BOM.
Purchase standard parts.
Conduct Brainstorm meetings
Design concept review
Prepare alternative design concepts for
11
12
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

on different loads
Gripper motor torque and

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
LEONG KANG
SIANG
MAUNG
GUANG XI
MOHAMAD
YAZID BIN
ISMAIL
13
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
14
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.
15
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
16
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.
17
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
Arm
18
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
19
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
20
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
21
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
22
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
23
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.
24
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)
25
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.
26
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)
27
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
28
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
29
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
30
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
31
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
32
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
33
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)
34
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)
35
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)
36
30
Torque Required vs. Displacement time
Torque Required (kg.cm)
25
20
Torque Required
Torque Supplied
15
10
5

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
30
25
Reaction force
available
20
15
10
5
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.
37
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.
38
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.
39
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)
40
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)
41
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
42
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
43
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.
44
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.
45
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.
46
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
Therefore, minimum torque required to move robot is 0.136 Nm or 1.39 kg.cm.
47
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.
48
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
49
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)
Finish
50
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

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

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

sensor output high?
Is rear IR sensor
output high?
STOP, proceed to
Pick Up Sequence
N
51
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
52
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
53
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
54
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
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
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
56
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
57
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.
58
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.
(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
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.
60
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
}
Object Detected by
Side sensor x_S and
Line tracking by
sensor x_L & x_R
61
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
Sensing
the influence of
ADCON0.GO=1;
Indicator
variant Height of
while(ADCON0.GO);
the stage and
lighting conditions
if(x_THETA==0)
ARM_MOTION_DOWN_HOLD();
if((x_S<100) & (x_THETA==0))
......................................................
{
x_THETA=1;
ADCON0=0b11000001; //distance sensor1
ADCON0.GO=1;
while(ADCON0.GO);
62
Optimizations by IR sensors:
63
Auto compensation sensing and gripping system:

Error
Angle
X_d2
Sensor
To
Detect
CAN
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
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.
65
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
66
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
SCREW & NUT FOR

MOUNTING BRACKET
67
Design D
Design E
Design F
Design A
Design B
Design C
Design D
Design E
Design F
(C) GRIPPER DESIGN
68
(D) Sensors
Design A
Design B
Design C
Design D
Design E
Design F
69
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

and extension cable

Ltd
Li QiMing
SGD 21.72
Receipt
Missing
11

X2

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
70
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)
A simple Flipping Arm (2-DOF)
71
Appendix D Detailed Drawings

6v to 12v PCB
Arm Shafts
72
Arm
73
Base Plates
74
Battery Holder
75
Bearing Plates - A
76
Bearing Plates - B
Bearing Plate - C
77
Bearing Shaft A & B
78
Bearing Shaft C
Collar
79
Distance Sensor Bracket
Jaw Flat
80
Gripper Gear & Gripper
81
Jaw & Line Tracking Sensor Mounting
82
Link Plate & Mounting Bracket
83
PCB Cover & Mounting
84
PCB Post and PCB
85
Pulley Adjust Block & Robot Arm
86
Robot Chassis & Robot
87
Round Nut and Sensor Mounting
88
Servo Motor Shaft
89
SG 90 Bracket
90
SG90 Coupling & Spacer
91
Theta Plate & Shaft
92
Timing Belt Gear
93
Touch Sensor Mounting & TS1033 Coupling
94
Weight
95
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;
// Pin direction
sbit LCD_RS_Direction
sbit LCD_EN_Direction
sbit LCD_D7_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;
96
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;
97
TRISA.F0=1;
TRISA.F1=1;
/**********testing**********/
Lcd_Init();
ANSEL = 0xFF; // PORTA ==> Analog
TRISA = 0xFF; // PORTA ==> input
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_Out(1,1,"Home Pos");
Init_Home();
//press button again
Forward(255); //MOVE
Lcd_Out(1,1,"Move,Arm low");
/****Smart auto-sensing & comparing system****/
x_THETA=ARM_MOTION_DOWN();//ARM LOWER DOWN
while(1)
{
ADCON0.GO=1;
while(ADCON0.GO);
if(x_THETA==0)
if((x_S<100) & (x_THETA==0))
{
x_THETA=1;
ADCON0.GO=1;
while(ADCON0.GO);
x_NHEIGHT= (ADRESH*4)+(ADRESL/64);
}
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;
}
if((x_THETA==3) & (x_S<500))
98
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;
break;
}
}
99
while(1)
{
ADCON0.GO=1;
while(ADCON0.GO);
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.GO=1;
while(ADCON0.GO);
x_D1= (ADRESH*4)+(ADRESL/64);
ADCON0.GO=1;
while(ADCON0.GO);
WordToStr(x_D1,txt_DIST);
Lcd_Out(1,9,txt_DIST);
x_DIFF=x_D1-x_NHEIGHT;
ADCON0.GO=1;
while(ADCON0.GO);
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
/*
ADCON0.GO=1;
while(ADCON0.GO);
ADCON0.GO=1;
100
while(ADCON0.GO);
WordToStr(x_D1,txt_DIST);
Lcd_Out(1,9,txt_DIST);
if(x_DIFF>150) //& (x_D2>100)) //240
break;
}
*/
Lcd_Out(1,1,"Gripper Close");
k = GPR_CLOSE();
Lcd_Out(1,1,"Arm Right");
//THETA_RIGHT();
THETA_RIGHT_noRC5(k);
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;
101
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);
102
//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;
103
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)
104
{
// 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();
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;
105
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:
106
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);
107
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;
108
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:
109
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:
110
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
111
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
//max over from 550
112
{
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);
113
//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--)
{
{
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);
}
}
114
}
*/
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++)
{
{
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.GO=1;
while(ADCON0.GO);
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
115
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****/
while(1)
{
ADCON0.GO=1;
while(ADCON0.GO);
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);
116
{
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()
{
117
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--)
{
{
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--)
{
{
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;
{
//4
118
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;
{
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;
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);
119
PORTB.F4=0;
Delay_ms(20);
*/
//}
THETA_LEFT_HOLD_CLOSE(k);
/*
ADCON0.GO=1;
while(ADCON0.GO);
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);
120
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;
/*
if(k!=1)
{
while(1)
{
ADCON0.GO=1;
while(ADCON0.GO);
if(x_DIFF>=250)
break;
if(x_DIFF<250)
{
for(m=30;m>0;m--)
{
PORTB.F4=1;
PORTC.F5=1;
121
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);
}
}
*/
//}
122
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++)
{
{
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--)
{
{
PORTC.F5=1;
PORTB.F4=1;
123
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;
}
/************************************/
************/
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();
Lcd_Out(1,1,"SENSOR1 = "); // Show Text
while(PORTA.F4)
{
ADCON0.GO=1;
while(ADCON0.GO);
WordToStr(x,data_);
Delay_ms(100);
}
Delay_ms(500);
while(PORTA.F4)
{
124
ADCON0.GO=1;
while(ADCON0.GO);
WordToStr(x,data_);
Delay_ms(100);
}
Delay_ms(500);
while(PORTA.F4)
{
ADCON0.GO=1;
while(ADCON0.GO);
WordToStr(x,data_);
Delay_ms(100);
}
Delay_ms(500);
while(PORTA.F4)
{
ADCON0.GO=1;
while(ADCON0.GO);
WordToStr(x,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
125
}
}
//****************************************************
//****************************************************
//********** 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();
126
}
/************************************/
/********** 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;
}
/************************************/
127

************/
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
}
128

TM3101 Design Project Report PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

TM3101 Design Project Report PDF

Uploaded by

Copyright:

Available Formats

Faculty of Engineering

Department of Mechanical Engineering

BACHELOR OF TECHNOLOGY PROGRAMME

TM3101 Mechanical Engineering Design

Leong Kang Siang

Mohamad Yazid Bin Ismail

TM3101 Mechanical Engineering Design

Discussion on Various Operating Mechanisms

for an Autonomous Robots

Report Writing Tasking

Design of the Chassis for the working functions

Design of the Arms and Grippers for the working functions

Final Selection of Design

Final Selection of Design

Materials and Parts Selection for an Autonomous Robot

Selection of Material for Chassis

Selection of Materials for Arm and Gripper

Robot Arm Assembly

TM3101 Mechanical Engineering Design

Torques for Motors

Required Torque for Theta Motor

Required Torque for Arm Motor

Robot Arm timing gear issue

Insufficient Arm motor torque

Mounting of arm motor not rigid enough

Chassis stiffness improvement

Low track tension

Low voltage supply to DC motors

Low current supply to DC motors

Wrong wiring of servo motor

Detection of presence of empty can

Calculation of Motors Requirement

Control for activation of devices

Testing and Integration of Moving Parts

Testing on the Operation of Moving Parts

Testing on a Mock Up Arena for Accuracy of Sensors

TM3101 Mechanical Engineering Design

TM3101 Mechanical Engineering Design

With special thanks and gratitude to:

TM3101 Mechanical Engineering Design

TM3101 Mechanical Engineering Design

The following approaches were adopted in the study:

Detailed study on the various chassis available in the market from

Detailed study on the various arms and grippers.

Detailed study on motors and sensors

Detailed study on the various methods of pick and place

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

TM3101 Mechanical Engineering Design

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

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

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

Figure 1 Floor plan of the competition arena

TM3101 Mechanical Engineering Design

No form of remote control or external power is allowed.

The machine must not weigh more than 5 kg.

There is no height limit

TM3101 Mechanical Engineering Design

Discussion on Various Operating Mechanisms and Approaches for our

TM3101 Mechanical Engineering Design

LIM HOCK KHIM

1. Project overall coordination

1. Prepare alternative design concepts for

1. Prepare Flow Chart for Robot Control