PRODUCED BY AN AUTODESK STUDENT VERSION

o
MAIN FUNCTION STOP TURN 90 LEFT BACKWARD
o
TURN 90 BACKWARD
START STOP
LEFT

Vleft = 0 Vleft = 0 rpm Vleft = -50 rpm

i = 1, flag = 0, value[i] = 0, Vright = 0 Vright = 300 rpm Vright = -50 rpm
turn = 0, black = 0

send velocity signal to send velocity signal to send velocity signal to

encoders encoders encoders
YES NO
i >= 6

RETURN RETURN RETURN

read value[i]
YES
black >= 3

flag = flag + 1
CALCULATE ERROR CALCULATE VELOCITY AND REVOLUTIONS OF WHEELS
NO NO
value[i] > 300 value[i] = 50 CALCULATE VELOCITY AND REVOLUTIONS
CALCULATE ERROR OF WHEELS
NO
black = 0 YES
en = 0, e = 0, p = 0, d = 0,
calculate error ep = 0, kp = 3.85 ref _ v = 95
YES YES

PRODUCED BY AN AUTODESK STUDENT VERSION

PRODUCED BY AN AUTODESK STUDENT VERSION

i=6 ki = 0.00105, kd = 0.00015

calculate velocity and 15 x (-2xvalue[1]-value[2]+value[4]+2xvalue[5])

revolutions of wheels _________________________________ ref_right ref_left
NO e=
value[1]+value[2]+value[4]+value[5] = ref_v + en = ref_v - en

value[i] = 100
YES p=e i=i+e
flag = 6 ref_right<0 YES YES
ref_left<0
turn = turn + 1
stop d = e -ep
NO NO NO
ref_right = 0 ref_left = 0
black = black + 1
YES ep = e
flag = 4
YES YES
ref_right>250 ref_left>250
error = kp x p + ki x i + kd x d
turn > 0 i=i+1
NO
ref_right=250 ref_left=250
NO
o
RETURN send right wheel velocity send left wheel velocity
turn 90 left = ref_right = ref_left

backward
RETURN

END Pham Duy

Drawn by Dai An
Tu Giao
1/12
Flow Chart
Instructor Pham Cong Bang

HCMC UNIVERSITY OF TECHNOLOGY

FACULTY OF MECHANICAL ENGINEERING LINE FOLLOWING ROBOT
PRODUCED BY AN AUTODESK STUDENT VERSION
Sheet 01