Firstly, I wanted to thank kwagyeman for the OpenMV project. I started arduino projects a few months ago and always wanted to try computer vision. I tried and, till now, always failed to achieve Raspsberry Pi based project in vision. Installing OpenCV on a Raspberry is a real challenge for a newbie like me
By following your tutorial on DIY robocar, I have made a line following car using the blob detection method only by building the basic rover and adjusting the threshold to follow a white line. And it works well !
Now I would like to try the linear regression method. I’m now copying, pasting and replacing parts of both codes you’ve posted (linear regression and blob detection) to obtain a code that could work.
I tried the following code :
BINARY_VIEW = True
GRAYSCALE_THRESHOLD = (230, 255)
MAG_THRESHOLD = 4
THETA_GAIN = 40.0
RHO_GAIN = -1.0
P_GAIN = 0.7
I_GAIN = 0.0
I_MIN = -0.0
I_MAX = 0.0
D_GAIN = 0.1
import sensor, image, pyb, math, time
from pyb import LED
from pyb import Pin, Timer
tim = Timer(4, freq=1000) # Frequency in Hz
cruise_speed = 60 # how fast should the car drive, range from 0 to 100
steering_direction = -1 # use this to revers the steering if your car goes in the wrong direction
steering_gain = 1.7 # calibration for your car's steering sensitivity
steering_center = 60 # set to your car servo's center point
kp = 0.8 # P term of the PID
ki = 0.0 # I term of the PID
kd = 0.4 # D term of the PID
blue_led = LED(3)
old_error = 0
measured_angle = 0
set_angle = 90 # this is the desired steering angle (straight ahead)
p_term = 0
i_term = 0
d_term = 0
old_time = pyb.millis()
radians_degrees = 57.3 # constant to convert from radians to degrees
def constrain(value, min, max):
if value < min :
return min
if value > max :
return max
else:
return value
ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=0)
ch2 = tim.channel(2, Timer.PWM, pin=Pin("P8"), pulse_width_percent=0)
def steer(angle):
global steering_gain, cruise_speed, steering_center
angle = int(round((angle+steering_center)*steering_gain))
angle = constrain(angle, 0, 180)
angle = 90 - angle
angle = radians_degrees * math.tan(angle/radians_degrees) # take the tangent to create a non-linear response curver
left = (90 - angle) * (cruise_speed/100)
left = constrain (left, 0, 100)
right = (90 + angle) * (cruise_speed/100)
right = constrain (right, 0, 100)
print ("left: ", left)
print ("right: ", right)
# Generate a 1KHz square wave on TIM4 with each channel
ch1.pulse_width_percent(left)
ch2.pulse_width_percent(right)
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQQVGA)
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.skip_frames(time = 2000)
clock = time.clock()
def line_to_theta_and_rho(line):
if line.rho() < 0:
if line.theta() < 90:
return (math.sin(math.radians(line.theta())),
math.cos(math.radians(line.theta() + 180)) * -line.rho())
else:
return (math.sin(math.radians(line.theta() - 180)),
math.cos(math.radians(line.theta() + 180)) * -line.rho())
else:
if line.theta() < 90:
if line.theta() < 45:
return (math.sin(math.radians(180 - line.theta())),
math.cos(math.radians(line.theta())) * line.rho())
else:
return (math.sin(math.radians(line.theta() - 180)),
math.cos(math.radians(line.theta())) * line.rho())
else:
return (math.sin(math.radians(180 - line.theta())),
math.cos(math.radians(line.theta())) * line.rho())
def line_to_theta_and_rho_error(line, img):
t, r = line_to_theta_and_rho(line)
return (t, r - (img.width() // 2))
old_result = 0
old_time = pyb.millis()
i_output = 0
output = 90
while(True):
clock.tick()
img = sensor.snapshot().histeq()
if BINARY_VIEW:
img.binary([GRAYSCALE_THRESHOLD])
img.erode(1)
line = img.get_regression([(255, 255)], robust=True)
print_string = ""
if line and (line.magnitude() >= MAG_THRESHOLD):
img.draw_line(line.line(), color=127)
t, r = line_to_theta_and_rho_error(line, img)
new_result = (t * THETA_GAIN) + (r * RHO_GAIN)
delta_result = new_result - old_result
old_result = new_result
new_time = pyb.millis()
delta_time = new_time - old_time
old_time = new_time
p_output = new_result
i_output = max(min(i_output + new_result, I_MAX), I_MIN)
d_output = (delta_result * 1000) / delta_time
pid_output = (P_GAIN * p_output) + (I_GAIN * i_output) + (D_GAIN * d_output)
output = 90 + max(min(int(pid_output), 90), -90)
print_string = "Line Ok - turn %d - line t: %d, r: %d" % (output, line.theta(), line.rho())
else:
print_string = "Line Lost - turn %d" % output
steer_angle = output
steer (steer_angle)
# print(str(measured_angle) + ', ' + str(set_angle) + ', ' + str(steer_angle))
blue_led.off()
The IDE does not generate any error and a line is drawn onto the image when connected with the USB cable. The line is well oriented so the linear regression seems to work.
However the motors are not moving at all. The problem does not seem to be the hardware. Indeed, it works well with the blob detection code.
Does someone have any idea on what should be corrected ?