ROLLBOTMICRO
31
else
if
((
data
[
0
]
<
threshold
)
&&
(
data
[
1
]
<
threshold
)
&&
(
data
[
2
]
<
threshold
)
&&
(
data
[
3
]
<
threshold
))
return
5
;
else
if
((
data
[
1
]
<
threshold
)
&&
(
data
[
2
]
<
threshold
)
&&
(
data
[
3
]
<
threshold
)
&&
(
data
[
4
]
<
threshold
))
return
5
;
else
if
((
data
[
0
]
>
threshold
)
&&
(
data
[
1
]
>
threshold
)
&&
(
data
[
2
]
>
threshold
)
&&
(
data
[
3
]
>
threshold
)
&&
(
data
[
4
]
>
threshold
))
return
6
;
}
}
Finally, we just need to read the position of the robot and adjust the related parameters via the
PD formula below, thus realizing the function of following lines.
MotorSpeed = P * Signa D * (SignalValue - LastError)
SignalValue
is the gap between the position of RollbotMicro detected and that of the black line.
LastError
is the error value accumulated till the last time.
SignalValue
–
LastError
means the
rate of change of the relative position.
Motorpeed
=
int
(
P
*
SignalValue
+
D
*
(
SignalValue
-
LastError
));
LastError
=
0
;
LastError
=
SignalValue
;
Speed_L
=
BASE_SPEED
+
Motorpeed
;
Speed_R
=
BASE_SPEED
-
Motorpeed
;
if
(
Speed_R
>
MAX_SPEED
)
Speed_R
=
MAX_SPEED
;
if
(
Speed_L
>
MAX_SPEED
)
Speed_L
=
MAX_SPEED
;
if
(
Speed_R
<
MIN_SPEED
)
Speed_R
=
MIN_SPEED
;
if
(
Speed_L
<
MIN_SPEED
)
Speed_L
=
MIN_SPEED
;
Motor
.
Motordrive
(
Speed_Dir
,
Speed_L
,
Speed_R
);
SunFounder