I'm working on a TWDD line-following robot using qtr-8a reflectance sensor array. The sensors are connected to BeagleBone Black, and BBB is sending the speed serially to an arduino Due. My approach is using a PID controller for the sensor so the error equals to zero when the robot is centred on the line and a positive/negative error depending on the robot's position. Applying Trial and error method I finally reached a Kp value that tracks straight lines perfectly. However, I'm still unable to turn and stay on the line even on a similarly smooth turns. I guess this is related to the Kd value. I'm not using the integral part Ki since the error is keep increasing. I tried to set conditions when the robots is drifting away from the line but it's not working probably (even without the conditions it is somehow turning smoother but then losing the line) I'm using the following draft code:
from bbio import *
import time
integral = 0
last_prop = 0
Kp = 20
Ki= 0
Kd = 150
amax = list(0 for i in range(0,8))
amin = list(1024 for i in range(0,8))
timeout = time.time() + 10
# Read ADC data from MCP3008
# ch: 0-7, ADC channel
# cs: 0-1, SPI chip select
# See MCP3008 datasheet p.21
def adc_read(cs, ch):
spidata = SPI1.transfer(cs, [1,(8+ch)<<4, 0])
data = ((spidata[1] & 3) << 8) + spidata[2]
return data
def setup():
# SPI1 setup
Serial2.begin(9600)
Serial5.begin(9600)
pinMode(GPIO1_7, OUTPUT)
digitalWrite(GPIO1_7, LOW)
SPI1.begin()
SPI1.setMaxFrequency(0,50000) # => ~47kHz, higher gives occasional false readings
# SPI1.setMaxFrequency(1,50000)
calibrate()
# reading the IR sensor data
def read_sensors():
sensors = []
for i in range(8):
sensors.append(adc_read(0,i))
return sensors
# calculating the error from PID controller
def calc_pid(x,sp):
global integral, last_prop , Kp, Ki ,Kd
set_point = sp
pos = sensor_average(x)/sensor_sum(x)
prop = pos - set_point
integral = integral + prop
deriv = prop - last_prop
last_prop = prop
error = (prop*Kp + integral*Ki + deriv*Kd)/100
return error
def get_position(s):
return sensor_average(s)/sensor_sum(s)
def sensor_average(x):
avg = 0
for i in range(8):
avg += x[i]*i*100
return avg
def sensor_sum(x):
sum = 0
for i in range(8):
sum += x[i]
return sum
def get_sensor(x):
j= read_sensors()[x]
return j
def calc_setpoint(x,y):
avg = 0
sum = 0
for i in range(8):
avg += (x[i]-y[i])*i*100
sum += x[i]-y[i]
return avg/sum
# calibrate the sensors reading to fit any given line
def calibrate():
global amin
global amax
while(time.time() < timeout):
for i in range(0,8):
amin[i] = min(amin[i],read_sensors()[i])
amax[i] = max(amax[i],read_sensors()[i])
digitalWrite(GPIO1_7, HIGH)
digitalWrite(GPIO1_7,LOW)
# calculating the correspondent speed
def calc_speed(error):
avg_speed = 150
min = 100
pos = get_position(read_sensors())
speed = []
if(error < -35):
right = avg_speed -(2*error)
left = avg_speed + (2*error)
elif(error > 35):
right =avg_speed - (2*error)
left = avg_speed + (2*error)
else:
right = avg_speed - (error)
left = avg_speed + (error)
speed.append(right)
speed.append(left)
return speed
def loop():
s = read_sensors()
setpoint = calc_setpoint(amax,amin)
position = get_position(s)
err = calc_pid(s,setpoint)
print err
#print "divided by 100:"
speeds = calc_speed(err)
print speeds
right_motor = speeds[0]
left_motor = speeds[1]
Serial2.write(right_motor)
Serial5.write(left_motor)
delay(10)
run(setup,loop)
PS: the sent speed over serial is limited to 255 and I'm multiplying it by a factor from the Arduino side.