Scribd
Upload
35 views11 pages

UGEB Project FSM

1) The document describes the major functions and state diagram of an online robot arm project demo. 2) It includes functions for reading serial input, moving and positioning the pen and arm joints, drawing shapes, detecting disk patterns, and transforming coordinates between frames of reference. 3) The arm has 6 motors connected to hardware ports to control 4 joints along the arm and a gripper, and line trackers are used to detect disk patterns marked with binary codes.

Uploaded by

Alexia Ho
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PPTX, PDF, TXT or read online on Scribd
35 views11 pages

UGEB Project FSM

1) The document describes the major functions and state diagram of an online robot arm project demo. 2) It includes functions for reading serial input, moving and positioning the pen and arm joints, drawing shapes, detecting disk patterns, and transforming coordinates between frames of reference. 3) The arm has 6 motors connected to hardware ports to control 4 joints along the arm and a gripper, and line trackers are used to detect disk patterns marked with binary codes.

Uploaded by

Alexia Ho
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PPTX, PDF, TXT or read online on Scribd
You are on page 1/ 11

ugeb-online-project_demo-v1 major functions description

SerialEvent() pen_move(int dir)


Set delta_move = 1 if input is ‘+’ Move pen away paper if dir = 0
or ‘-’
Move pen to paper if dir = 1
Set got_command = 1 if input is
between 97 to 122

Set got_disk_command = 1 if
input is between 49 to 55
S_read_data

If delta_move !=0,
if ‘w’ transit to S_twist_move.
else transit to S_xy_move.
Clear all past jobs.
Else If got_command !=0,
transit to S_change_fn.
Clear all past jobs.
Else If got_disk_command !=0,
transit to S_disk_fn.
Clear all past jobs.

Else { if draw_square !=0, transit to S_draw_sq.


else if draw_circle !=0, transit to S_draw_circle.
else if disk_operation!=0, transit to S_find_disk_pattern.

if job_start is 2, calculate time and


display of current job
S_disk_fn S_xy_move

Set parameters so that motor will stop Move the pen attached to arm in X or Y
at the user input selection (if current direction, or move the least joint angle
pattern is the selection, disk will move
until it is not, then search it again) that
detected by line tracker sensors.
Input ‘1’ for pattern 001, S_move_selected_joint
‘2’ for 010
‘3’ for 011 Move the selected joint
‘4’ for 100
‘5’ for 101
‘6’ for 110
‘7’ for 111
Transit to S_find_disk_pattern S_change_fn

According to the user input, perform the


task.
Major FSM State diagram of ugeb-online-project_demo-v1

S_read_data
S_micro_move
_arm

S_draw_sq
S_draw_circle
Generate fx, fy, ft from
array c [ ][ ]. Then Generate fx,fy,ft from
frame_transform() S_micro_find_angles yx,xc,
G_radius and
Calculate joint angle by IK, cir_pen_grad. Then
find the three joints angles frame_transform()

S_micro_transform2control
Map to control commands of
robot arm
S_find_disk_pattern
Read pattern, if it is the one needed, stop motor and show
how much time it take. Transit to S_read_data always.
Checking the movement of arm
Due to gravity and the accuracy of motors, the
real movements of arm have a large different to
the desired positions.
Below is the setting of demo. You can check the
performance of the arm.

4 points linked with dash lines


checking (x (mm), y, (mm), angle
of gripper in degree)
(-50,315,110;),
(-80,315,110;),
(-80,260,110;),
(-50,260,110;)
Solid lines are vertical or
horizontal to ground.
Arm movement
• Functions S_micro_find_angles, S_micro_transform2control and S_micro_move_arm can find the joint angles
of the middle three up/down motors if you give the gripper angle and x and y location of it.
• If it cannot find a solution or it is out of the shoulder motor limit, error will shown.
• Motors may move in different speed apparently, and they will reach it’s target setting at the same time.
• The shoulder rotate motor, wrist rotate motor and gripper motor are controlled by other functions.
• We make an Excel spreadsheet (check_data4draw_circle) to check if the setting is good or not. You can use it
to check setting beforehand.
Hardware connection
The connection of hardware is listed below:
Disk motor to D2
Arm motor A to D4
Arm motor B to D5
Arm motor C to D6
Arm motor D to D8
Arm motor E to D9
Arm motor F to D10
Line tracker sensor (right hand side) to A14
Line tracker sensor (middle) to A13
Line tracker sensor (left hand side) to A12
Disk pattern definition.
1,pattern 001 5,pattern 101

3,pattern 011
6,pattern 110

7,pattern 111
2,pattern 010

4,pattern 100
Center ring, ring 1
Ring 2

Ring 3

Ring 4
Additional information

It means we can find the coordinate of the tip point in the


base Cartesian
coordinate frame if we have the geometry of the robot and
all joint variables.
Y = l1 cos θ1 + l2 cos (θ1 + θ2) + l3 cos (θ1 + θ2 + θ3) (5.65)
X = l1 sin θ1 + l2 sin (θ1 + θ2) + l3 sin (θ1 + θ2 + θ3) (5.66)

You might also like