About The Line Following Module

The robot comes equipped with a 4 channel IR module attached to the bottom of the robot. There are LEDs on top of the module that indicate the state of the sensors automatically without the need to write any code.

From the datasheet

Infrared sensor data:

  • Address: 0x0A
  • Type: Read
  • Range: 0x0-0xF
  • Additional Info:
    • 1 bit = 1 sensor state
    • 0 means LED is lit, 1 means LED is off
    • If we label sensors from left to right accordingly: s0, s1, s2, s3; then output byte corresponds to: s1, s0, s2, s3

Label sensors from left to right: s0, s1, s2, s3; respectively

So if we get an output 0b0101 Then:

\(2^3\) \(2^2\) \(2^1\) \(2^0\)
s1 s0 s2 s3
0 1 0 1

Then that implies:

  • s0: LED off
  • s1: LED lit
  • s2: LED lit
  • s3: LED off

To get the value of a certain bit, for instance this \(\text{0b1}\underline{1}\text{01}\)

  1. Shift to the right by \(n\) where \(n\) is the amount of places to the left of lowest bit (or \(2^n\) in base 10). meaning 0b1101 >> 2 results in 0b11.
  2. Remove all bits to the left by using the AND operation with 0b1. 0b11&0b01 outputs 1 which is exactly the binary digit we're trying to retrieve.

Calibrating The Sensors

To correctly output the right value based on your surroundings, we need to calibrate the sensors. Place all the sensors over a black material and adjust accordingly:

  1. Turn all the knob until no LED lights up
  2. Turn all the knobs, just until LED lights up and keep it there

Double check:

  1. remove from surface
  2. check that LED doesn't light up

Reading Data From Sensor

reg = 0x0A
desired_blocks_of_data = 1
track_data = bot.read_data_array(reg, desired_blocks_of_data)

Formatting the data

Pulling data out from the list and converting to int type

track_data = int(track_data[0])

If we convert it to binary, we get an output as such:

0b1011

reg = 0x0A
desired_blocks_of_data = 1
w_for_ir = widgets.Text(
    description='State: ',
    disabled=True   
)

display(w_for_ir)
while True:
    track_data = bot.read_data_array(reg, desired_blocks_of_data)[0]
    w_for_ir.value = bin(track_data)

Simple Line Following Robot

From the code, we can simplify the state, 0 and 1 as such:

  • 0: couldn't receive ir signal
  • 1: could receive ir signal

or:

  • 0: on black rails
  • 1: off black rails

We'll flip all the bits so that it's easier to work with.

Recall this table:

\(2^3\) \(2^2\) \(2^1\) \(2^0\)
s1 s0 s2 s3

or

s0 s1 s2 s3
\(2^2\) \(2^3\) \(2^1\) \(2^0\) 
# first rewrite the bits into a list of bool (not for efficiency but for convenience)
def rewrite_list(track_data:int):
    # first invert the bits (easier to work with)
    track_data ^= 1111
    ret_list = list()
    ret_list.append(bool((track_data >> 2 ) & 1))
    ret_list.append(not bool((track_data >> 3) & 1))
    ret_list.append(bool((track_data >> 1 ) & 1))
    ret_list.append(bool((track_data) & 1))
    return ret_list

After inverting the bits:

  • 0: off black rails
  • 1: on black rails

Bang Bang Control

Uses simple if-else statements to stay on the line.

Input to action table

Input Result
all off blackline stop
s0 on black line rotate left
s3 on black line rotate right
otherwise continue straight 

reg = 0x0A
desired_blocks_of_data = 1
speed = 100
while True:
    track_data = bot.read_data_array(reg, desired_blocks_of_data)[0]
    s = rewrite_list(track_data)
    if not (s[0] or s[1] or s[2] or s[3]):
        bot.Ctrl_Muto(0, 0)
        bot.Ctrl_Muto(1, 0)
        bot.Ctrl_Muto(2, 0)
        bot.Ctrl_Muto(3, 0)
    elif s[0]:
        bot.Ctrl_Muto(0, -speed)
        bot.Ctrl_Muto(1, -speed)
        bot.Ctrl_Muto(2,speed)
        bot.Ctrl_Muto(3, speed)
    elif s[3]:
        bot.Ctrl_Muto(0, speed)
        bot.Ctrl_Muto(1, speed)
        bot.Ctrl_Muto(2, -speed)
        bot.Ctrl_Muto(3, -speed)

    else:
        bot.Ctrl_Muto(0, speed)
        bot.Ctrl_Muto(1, speed)
        bot.Ctrl_Muto(2, speed)
        bot.Ctrl_Muto(3, speed)

More complicated bang bang algorithm:

# first rewrite the bits into a list of bool (not for efficiency but for convenience)

def rewrite_list(track_data:int):
    track_data ^= 1111
    ret_list = list()
    ret_list.append(bool((track_data >> 2 ) & 1))
    ret_list.append(not bool((track_data >> 3) & 1))
    ret_list.append(bool((track_data >> 1 ) & 1))
    ret_list.append(bool((track_data) & 1))
    return ret_list    

POWER = 0.2
# 90 is constant. control only power
# power, turn ranges from -1, 0 and 1, with -1 being left, 1 begin right.
move_robot_wrapper = lambda power, turn: move_robot(90, power*POWER, turn*POWER, bot)

map_to_control = {
    "forward": (1, 0),
    "rotate left":(0, -1),
    "rotate right":(0, 1),
    "extreme left":(-1, -1),
    "extreme right":(-1, 1),
    "stop":(0,0)
}
move_robot_wrapper(*map_to_control["extreme right"])
time.sleep(1)
move_robot_wrapper(*map_to_control["stop"])


reg = 0x0A
desired_blocks_of_data = 1
while True:
    track_data = bot.read_data_array(reg, desired_blocks_of_data)[0]
    s = rewrite_list(track_data)
    if s[0] and s[1] and s[2] and s[3]:
        move_robot_wrapper(*map_to_control["extreme right"])
    elif (not s[0]) and (not s[1]) and (not s[2]) and (not s[3]):
        move_robot_wrapper(*map_to_control["stop"])
    elif s[0] and s[1] and s[2] and (not s[3]):
        move_robot_wrapper(*map_to_control["extreme left"])
    elif (not s[0]) and s[1] and s[2] and s[3]:
        move_robot_wrapper(*map_to_control["extreme right"])
    elif (s[0] and (not s[1]) and (not s[2]) and (not s[3])) or (not s[2]):
        move_robot_wrapper(*map_to_control["rotate left"])
    elif ((not s[0]) and (not s[1]) and (not s[2]) and s[3]) or (not s[1]):
        move_robot_wrapper(*map_to_control["rotate right"])
    else:
        move_robot_wrapper(*map_to_control["forward"])

    time.sleep(0.1)


reg = 0x0A
desired_blocks_of_data = 1
while True:
    track_data = bot.read_data_array(reg, desired_blocks_of_data)[0]
    s = rewrite_list(track_data)
    if not (s[0] or s[1] or s[2] or s[3]):
        move_robot_wrapper(*map_to_control["stop"])
    elif s[0]:
        move_robot_wrapper(*map_to_control["rotate left"])
    elif s[3]:
        move_robot_wrapper(*map_to_control["rotate right"])

    else:
        move_robot_wrapper(*map_to_control["forward"])

Stopping:

move_robot_wrapper(*map_to_control["stop"])

PID controller

A PID controller stands for Proportional, Integral, Derivative.

\[ \text{control} = k_p \times \text{error} + k_i \times \int{(\text{error})}dt + k_d \times \frac{d}{dt}(\text{error}) \]

\(k_p\), \(k_i\) and \(k_d\) can be determined experimentally.

# implementing a PID controller
def pid(k_p, k_i, k_d, e, e_prev, e_sum, t_prev):
    t = time.time()
    t_diff = t - t_prev
    e_diff = e - e_prev
    e_sum += e
    e = k_p*e + k_i*e_sum + k_d*(e_diff/t_diff)
    return e, e_sum, t


reg = 0x0A
desired_blocks_of_data = 1
e_sum = 0
e_prev = 0
t_prev = time.time()
weight = [-4, -1, 1, 4]
base_speed = 50
while True:
    track_data = bot.read_data_array(reg, desired_blocks_of_data)[0]
    s = rewrite_list(track_data)
    k_p = 70
    k_i = 0.5
    k_d = 0.002
    e = sum([s[i]*weight[i] for i in range(len(s))])
    e_prev, e_sum, t_prev = pid(k_p, k_i, k_d, e, e_prev, e_sum, t_prev)
    # print(e_prev)
    left_speed = int(max(min(255, base_speed + e_prev), -255))
    right_speed = int(max(min(255, base_speed - e_prev), -255))
    bot.Ctrl_Muto(0, left_speed)
    bot.Ctrl_Muto(1, left_speed)
    bot.Ctrl_Muto(2, right_speed)
    bot.Ctrl_Muto(3, right_speed)



reg = 0x0A
desired_blocks_of_data = 1
e_sum = 0
e_prev = 0
t_prev = time.time()
weight = [-4, -1, 1, 4]
base_speed = 50
while True:
    track_data = bot.read_data_array(reg, desired_blocks_of_data)[0]
    s = rewrite_list(track_data)
    k_p = 70
    k_i = 0.5
    k_d = 0.002
    e = sum([s[i]*weight[i] for i in range(len(s))])
    e_prev, e_sum, t_prev = pid(k_p, k_i, k_d, e, e_prev, e_sum, t_prev)
    # print(e_prev)
    left_speed = int(max(min(255, base_speed + e_prev), -255))
    right_speed = int(max(min(255, base_speed - e_prev), -255))
    bot.Ctrl_Muto(0, left_speed)
    bot.Ctrl_Muto(1, left_speed)
    bot.Ctrl_Muto(2, right_speed)
    bot.Ctrl_Muto(3, right_speed)

Less jittery, more error prone, implemented anti-windup


reg = 0x0A
desired_blocks_of_data = 1
e_sum = 0
e_prev = 0
t_prev = time.time()
t_windup = t_prev
windup_delay = 10
weight = [-3, -1, 1, 3]
base_speed = 50
try:
    while True:
        track_data = bot.read_data_array(reg, desired_blocks_of_data)[0]
        s = rewrite_list(track_data)
        k_p = 90

        k_i = 0.05
        k_d = 0.0022
        if time.time() - t_windup > windup_delay:
            t_windup = time.time()
            e_sum = 0
        e = sum([s[i]*weight[i] for i in range(len(s))])
        e_prev, e_sum, t_prev = pid(k_p, k_i, k_d, e, e_prev, e_sum, t_prev)
        # print(e_prev)
        left_speed = int(max(min(255, base_speed + e_prev), -255))
        right_speed = int(max(min(255, base_speed - e_prev), -255))
        bot.Ctrl_Muto(0, left_speed)
        bot.Ctrl_Muto(1, left_speed)
        bot.Ctrl_Muto(2, right_speed)
        bot.Ctrl_Muto(3, right_speed)
finally:
        bot.Ctrl_Muto(0, 0)
        bot.Ctrl_Muto(1, 0)
        bot.Ctrl_Muto(2, 0)
        bot.Ctrl_Muto(3, 0)

Some Settings

weights base_speed k_p k_i k_d
[-3, -1, 1, 3] 50 175 0 0.002