Hi OpenMV support
I’m preparing for a DIY robocar and facing trouble recording the rover camera (15s):
I get RED LED blinking twice then the wheels spin like crazy!
The script runs without problems when connected by USB to IDE.
That’s why I’m suspecting disk issue.
NB: I’m using SDHC card 4GB on Ubuntu and have no problem accessing the card itself.
Code below, sorry if it’s a bit long
### Imports
import sensor, pyb, math, time, mjpeg
from pyb import LED
from pyb import Pin, Timer
### Constants
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 = 30 # 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
thd = (240, 255) #threshold for white lanes
MAG_THRESHOLD = 4 #magnitude of line in hough transform
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
blue_led = LED(3)
record_time=15000
####Helper functions
def constrain(value, min, max):
if value < min :
return min
if value > max :
return max
else:
return value
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)
# Generate a 1KHz square wave on TIM4 with each channel
ch1.pulse_width_percent(left)
ch2.pulse_width_percent(right)
def update_pid():
global old_time, old_error, measured_angle, set_angle
global p_term, i_term, d_term
now = pyb.millis()
dt = now - old_time
error = set_angle - measured_angle
de = error - old_error
p_term = kp * error
i_term += ki * error
i_term = constrain(i_term, 0, 100)
d_term = (de / dt) * kd
old_error = error
output = steering_direction * (p_term + i_term + d_term)
output = constrain(output, -50, 50)
return output
###Init: Motor pins
pinADir0 = pyb.Pin('P0', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
pinADir1 = pyb.Pin('P1', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
pinBDir0 = pyb.Pin('P2', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
pinBDir1 = pyb.Pin('P3', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
pinADir0.value(1)
pinADir1.value(0)
pinBDir0.value(0)
pinBDir1.value(1)
tim = Timer(4, freq=1000) # Frequency in Hz
ch1 = tim.channel(1, pyb.Timer.PWM, pin=pyb.Pin("P7"))
ch2 = tim.channel(2, pyb.Timer.PWM, pin=pyb.Pin("P8"))
###Init: Camera
clock = time.clock() # Tracks FPS.
start = pyb.millis()
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)
###Init: Recording file
fname1='gs_'+str(pyb.millis())+'.mjpeg'
m_1 = mjpeg.Mjpeg(fname1)
###Init: Binary mask
imbin = sensor.alloc_extra_fb(sensor.width(), sensor.height(), sensor.GRAYSCALE)
imbin.replace(sensor.snapshot())
imbin.clear()
imbin.draw_rectangle(1,20,79,40,fill=True)
start = pyb.millis()
while(pyb.elapsed_millis(start) < record_time):
clock.tick()
######1. Perception
img = sensor.snapshot().histeq()
img.binary([thd],mask=imbin)
img.erode(1)
line = img.get_regression([thd],roi=(1,20,79,40), robust=True)
img.draw_rectangle(1,20,79,40)
######2. Localization
deflection_angle = 0
cruise_speed = 15 # how fast should the car drive, range from 0 to 100
if line and (line.magnitude() >= MAG_THRESHOLD):
img.draw_line(line.line(), color=127)
lane_center=40
if abs(line.x1()-line.x2())<5:
lane_center=int((line.x1()+line.x2())/2)
elif abs(line.y1()-line.y2())<5:
continue
else:
a=(line.y2()-line.y1())/(line.x2()-line.x1())
b=line.y2()-a*line.x2()
lane_center=(20-b)/a
lane_center=constrain(lane_center,0,80)
img.draw_cross(int(lane_center),20,255)
######3. Trajectory generation
deflection_angle = -math.atan((lane_center-40)/40)
deflection_angle = math.degrees(deflection_angle)
######4. Control
now = pyb.millis()
if now > old_time + 0.02 : # time has passed since last measurement; do the PID at 50hz
blue_led.on()
measured_angle = deflection_angle + 90
steer_angle = update_pid()
old_time = now
steer(steer_angle)
blue_led.off()
m_1.add_frame(img)
pinADir0.value(0)
pinADir1.value(0)
pinBDir0.value(0)
pinBDir1.value(0)
m_1.close(clock.fps())