def startSensors(self, timestep):
# Instantiate objects and start up GPS, Gyro, and Compass sensors
# For more details, refer to the Webots documentation
self.gps = GPS("gps")
self.gps.enable(timestep)
self.gyro = Gyro("gyro")
self.gyro.enable(timestep)
self.compass = Compass("compass")
self.compass.enable(timestep)
def bearing(compass: controller.Compass) -> float:
"""Get bearing from compass ([0, 360] version)
Args:
compass (controller.Compass): Compass instance.
Returns:
float: Bearing in degrees, [0, 360]
"""
values = compass.getValues()
theta = np.arctan2(values[0], values[2])
theta = np.rad2deg(theta) - 90
# Unlike C/C++, Python's modulus operator returns a number with the same sign as the divisor
return theta % 360
# Data: 04/07/2020
# **************************************************************
from controller import Robot
from controller import Motor
from controller import Compass
import math
TIME_STEP = 64
MAX_SPEED = 1.2
robot = Robot()
# bussola
mag = robot.getCompass("compass")
Compass.enable(mag, TIME_STEP)
leftMotor = robot.getMotor('left wheel')
rightMotor = robot.getMotor('right wheel')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
while robot.step(TIME_STEP) != -1:
# lê o sensor
north = Compass.getValues(mag)
phi = math.atan2(north[0], north[2]) * (180 / math.pi)
# converter para faixa de 0 a 360 graus
if phi < 0:
phi = phi + 360
class Vehicle:
stage = 0
MAX_SPEED = 2
time_tmp = 0
robot = Robot()
motors = []
camera = Camera('camera')
compass = Compass('compass')
ds = robot.getDistanceSensor('distance sensor')
#tmp
picked = False
def __init__(self):
#get motors, camera and initialise them.
motorNames = ['left motor', 'right motor', 'tower rotational motor']
for i in range(3):
self.motors.append(self.robot.getMotor(motorNames[i]))
self.motors[i].setPosition(float('inf'))
if i <= 1:
self.motors[i].setVelocity(0)
else:
self.motors[i].setVelocity(0.2)
self.motors[i].setPosition(0)
self.camera.enable(int(self.robot.getBasicTimeStep()))
self.compass.enable(int(self.robot.getBasicTimeStep()))
self.ds.enable(int(self.robot.getBasicTimeStep()))
def getStage(self):
return self.stage
def setStage(self, stage_num):
self.stage = stage_num
def getCompass(self):
return np.angle(complex(self.compass.getValues()[0], self.compass.getValues()[2]))
def getDistanceValue(self):
return self.ds.getValue()
def towerSeeLeft(self):
self.motors[2].setPosition(np.pi / 2)
def towerSeeRight(self):
self.motors[2].setPosition(-np.pi / 2)
def towerRestore(self):
self.motors[2].setPosition(0)
def releaseFood(self):
#release food
pass
#Speed setting functions
def setSpeed(self, left, right):
#set speed for four tracks
self.motors[0].setVelocity(left)
self.motors[1].setVelocity(right)
def turnRound(self, diff):
#set speed for turning left or right
global error_integral
global previous_diff#set variable as global will accelerate the speed
error_integral += diff * ts;
error_derivative = (previous_diff - diff) / ts;
Vc = 0.5* diff + 0.00 * error_derivative + 0.05* error_integral ;
#set as 0.35/0.001/0.02 for mass=40kg
#set as 0.5/0/0.05 respectively for mass=400kg
if Vc > 1:
Vc = 1
if abs(diff) < 0.001:
self.setSpeed(0, 0)
return False
else:
self.setSpeed(-Vc, Vc)
previous_diff = diff
return True
def linePatrol(self):
#get camera image, process it and set speed based on line patrolling algorithm
#return False if there is no line
pass
def boxPatrol(self):
#get camera image and find orange box, then adjust the speed to go to the box
#return False if there is no box
pass
def bridgePatrol(self):
#get camera image and find bridge, then adjust the speed to go to the bridge
#return False if there is no bridge
pass
def archPatrol(self):
#get camera image and find arch, then adjust the speed to go to the arch
#return False if there is no arch
pass
def colourPatrol(self):
#for task 5
pass
#Nicholas Dzomba and Michael Resller
#CSCI 455 Final Project Controller
from controller import Robot, Compass
import math, sys
from math import pi as M_PI
TIME_STEP = 64
robot = Robot()
compass = Compass("compass")
compass.enable(TIME_STEP)
ds = []
dsNames = ['ds_right', 'ds_left']
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
avoidObstacleCounter = 0
finstate = 0
start_state = 1
turnCounter = 0.0
for i in range(2):
ds.append(robot.getDistanceSensor(dsNames[i]))
ds[i].enable(TIME_STEP)
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
from controller import Robot
from controller import Compass
TIME_STEP = 64
compass = Compass()
compass.enable(TIME_STEP)
robot = Robot()
ds = []
dsNames = ['ds_right', 'ds_left', 'compass']
for i in range(3):
ds.append(robot.getDistanceSensor(dsNames[i]))
ds[i].enable(TIME_STEP)
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
avoidObstacleCounter = 0
while robot.step(TIME_STEP) != -1:
leftSpeed = 1.0
rightSpeed = 1.0
print(compass.getValues)
if avoidObstacleCounter > 0:
avoidObstacleCounter -= 1
leftSpeed = 1.0
rightSpeed = -1.0
class Vehicle:
stage = 0
MAX_SPEED = 2
robot = Robot()
motors = []
armMotors = []
armSensors = []
armPs = []
camera = Camera('camera')
compass = Compass('compass')
ds = robot.getDistanceSensor('distance sensor')
ds2 = robot.getDistanceSensor('distance sensor2')
HEIGHT = camera.getHeight()
WIDTH = camera.getWidth()
foodStage = 0
p0 = WIDTH / 2
leftSpeed = 0.0
rightSpeed = 0.0
leftSum = 0
rightSum = 0
HALF = int(WIDTH / 2)
frame = np.zeros((HEIGHT, WIDTH))
blur_im = np.zeros((HEIGHT, WIDTH))
position = WIDTH / 2
def __init__(self):
#get motors, camera and initialise them.
motorNames = ['left motor', 'right motor',
'tower rotational motor'] #, 'trigger motor']
for i in range(3):
self.motors.append(self.robot.getMotor(motorNames[i]))
self.motors[i].setPosition(float('inf'))
if i <= 1:
self.motors[i].setVelocity(0)
else:
self.motors[i].setVelocity(0.8)
self.motors[i].setPosition(0)
armMotorNames = [
"arm_motor_1", "arm_motor_2", "arm_motor_3", "arm_motor_4",
"arm_motor_5", "arm_motor_6", "arm_motor_7", "arm_motor_8"
]
for i in range(len(armMotorNames)):
self.armMotors.append(self.robot.getMotor(armMotorNames[i]))
self.armMotors[i].setPosition(0)
self.armMotors[i].setVelocity(0.3)
armSensorNames = [
"arm_position_sensor_1", "arm_position_sensor_2",
"arm_position_sensor_3", "arm_position_sensor_4",
"arm_position_sensor_5", "arm_position_sensor_6",
"arm_position_sensor_7", "arm_position_sensor_8"
]
for i in range(len(armSensorNames)):
self.armSensors.append(
self.robot.getPositionSensor(armSensorNames[i]))
self.armSensors[i].enable(int(self.robot.getBasicTimeStep()))
self.camera.enable(int(self.robot.getBasicTimeStep()))
self.compass.enable(int(self.robot.getBasicTimeStep()))
self.ds.enable(int(self.robot.getBasicTimeStep()))
self.ds2.enable(int(self.robot.getBasicTimeStep()))
def getStage(self):
return self.stage
def setStage(self, stage_num):
self.stage = stage_num
def getCompass(self):
return np.angle(
complex(self.compass.getValues()[0],
self.compass.getValues()[2]))
def getDistanceValue(self, input_ds=1):
if input_ds == 1:
return self.ds.getValue()
if input_ds == 2:
return self.ds2.getValue()
def towerSeeLeft(self):
self.motors[2].setPosition(np.pi / 2)
@staticmethod
def towerSeeRight():
Vehicle.motors[2].setPosition(-np.pi / 2)
def towerRestore(self):
self.motors[2].setPosition(0)
def pickFood(self):
if self.foodStage == 0:
self.armMotors[0].setPosition(0.039) #upper-arm extend
self.armMotors[1].setPosition(0.93) #upper-arm rotate
if (self.armSensors[1].getValue() > 0.92):
self.armMotors[2].setPosition(0.038) #mid-arm extend
#print(sensor3.getValue())
if (self.armSensors[2].getValue() > 0.0379):
self.armMotors[4].setPosition(0.005) #grab the box
self.armMotors[5].setPosition(0.005)
if ((self.armSensors[4].getValue() >= 0.00269)
or (self.armSensors[5].getValue() >= 0.0031)):
self.foodStage = 1
elif self.foodStage == 1:
self.armMotors[2].setPosition(0) #raise the box
self.armMotors[2].setVelocity(.1)
self.armMotors[1].setPosition(0)
self.armMotors[6].setPosition(0.9)
if (self.armSensors[1].getValue() < 0.002):
self.armMotors[0].setPosition(0)
self.armMotors[0].setVelocity(0.01) #grabbing task finished
if (self.armSensors[0].getValue() < 0.003):
return True
return False
def releaseFood(self):
if (self.armSensors[6].getValue() > 0.899):
self.armMotors[3].setPosition(0.06)
self.armMotors[7].setPosition(0.06)
if ((self.armSensors[3].getValue() > 0.059)
and (self.armSensors[7].getValue() > 0.059)):
self.armMotors[4].setPosition(0)
self.armMotors[5].setPosition(0)
if ((self.armSensors[4].getValue() <= 0.00269)
or (self.armSensors[5].getValue() >= 0.0031)):
self.armMotors[3].setPosition(0)
self.armMotors[7].setPosition(0)
self.armMotors[6].setPosition(0)
return True
#Img Processing related
@staticmethod
def get_frame():
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
camera = Vehicle.camera
cameraData = camera.getImage()
# get image and process it
frame = np.zeros((HEIGHT, WIDTH))
for x in range(0, Vehicle.WIDTH):
for y in range(0, HEIGHT):
gray = int(camera.imageGetGray(cameraData, WIDTH, x, y))
frame[y][x] = gray
return frame
@staticmethod
def edge_detect(frame,
threshold=254,
maxVal=255,
kernel_1=20,
kernel_2=15):
# threshold
# Edge_LineTracking_T1:254; Edge_Bridge_detection_T3:180; Edge_Arch_detection_T4:100;Edge_Color_detect_T5:254
# maxVal
# Edge_LineTracking_T1:255; Edge_Bridge_detection_T3:255; Edge_Arch_detection_T4:255;Edge_Color_detect_T5:255
# kernel_1
# Edge_LineTracking_T1:20; Edge_Bridge_detection_T3:20; Edge_Arch_detection_T4:20;Edge_Color_detect_T5:20
# kernel_2
# Edge_LineTracking_T1:15; Edge_Bridge_detection_T3:15; Edge_Arch_detection_T4:15;Edge_Color_detect_T5:15
# edge_detect Sobel
x = cv2.Sobel(frame, cv2.CV_16S, 1, 0)
y = cv2.Sobel(frame, cv2.CV_16S, 0, 1)
absx = cv2.convertScaleAbs(x)
absy = cv2.convertScaleAbs(y)
dst = cv2.addWeighted(absx, 0.5, absy, 0.5, 0)
#cv2.imwrite("dst.jpg", dst)
# to binary
ret, binary = cv2.threshold(dst, threshold, maxVal, cv2.THRESH_BINARY)
#cv2.imwrite("binary.jpg", binary)
# Smooth
kernel1 = np.ones((kernel_1, kernel_1), np.float) / 25
smooth = cv2.filter2D(binary, -1, kernel1)
# blur
blur_im = cv2.boxFilter(smooth, -1, (kernel_2, kernel_2), normalize=1)
#cv2.imwrite("blur.jpg", blur_im)
return blur_im
@staticmethod
def positioning(blur_im,
type_select=0,
height_per_1=0.7,
height_per_2=0.8):
# height_per_1
# Edge_LineTracking_T1:0.7; Edge_Bridge_detection_T3:0.3; Edge_Arch_detection_T4:0.3;Edge_Color_detect_T5:0.6
# height_per_2
# Edge_LineTracking_T1:0.8; Edge_Bridge_detection_T3:0.6; Edge_Arch_detection_T4:0.8;Edge_Color_detect_T5:0.7
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
HALF = Vehicle.HALF
axis = 0
num = 0
position = 0
axis_l = 0
axis_r = 0
num_l = 0
num_r = 0
position_l = 0
position_r = WIDTH - 1
if type_select == 0:
for axis_v in range(int(HEIGHT * height_per_1),
int(HEIGHT * height_per_2)):
for axis_h in range(WIDTH * 0, WIDTH):
if blur_im[axis_v][axis_h] != 0:
axis = axis + axis_h
num = num + 1
if num:
position = axis / num + 1
elif type_select == 1:
for axis_v in range(int(HEIGHT * height_per_1),
int(HEIGHT * height_per_2)):
for axis_h in range(WIDTH * 0, HALF):
if blur_im[axis_v][axis_h] != 0:
axis_l = axis_l + axis_h
num_l = num_l + 1
if blur_im[axis_v][axis_h + HALF] != 0:
axis_r = axis_r + axis_h + HALF
num_r = num_r + 1
if num_l:
position_l = axis_l / num_l + 1
if num_r:
position_r = axis_r / num_r + 1
position = (position_l + position_r) / 2
else:
print('INPUT ERROR!:positioning:type_select')
return position
@staticmethod
def steering(position,
type_select=0,
rectify_pixel=0,
base_speed=3.0,
straight_speed=2.0):
# rectify_pixel
# Edge_LineTracking_T1:7; Edge_Bridge_detection_T3:5; Edge_Arch_detection_T4:5;Edge_Color_detect_T5:3
# base_speed
# Edge_LineTracking_T1:3; Edge_Bridge_detection_T3:5; Edge_Arch_detection_T4:5;Edge_Color_detect_T5:20
# straight_speed
# Edge_LineTracking_T1:2; Edge_Bridge_detection_T3:3; Edge_Arch_detection_T4:2;Edge_Color_detect_T5:2
WIDTH = Vehicle.WIDTH
if type_select == 0:
if abs(position - WIDTH / 2) > rectify_pixel:
leftSpeed = (position / WIDTH) * base_speed
rightSpeed = (1 - position / WIDTH) * base_speed
else:
leftSpeed = straight_speed
rightSpeed = straight_speed
elif type_select == 1:
if abs(position - WIDTH / 2) > rectify_pixel:
leftSpeed = (position / WIDTH - 0.5) * base_speed
rightSpeed = (0.5 - position / WIDTH) * base_speed
else:
leftSpeed = straight_speed
rightSpeed = straight_speed
else:
print('INPUT ERROR!:steering:type_select')
return leftSpeed, rightSpeed
#Speed setting functions
def setSpeed(self, left, right):
#set speed for four tracks
self.motors[0].setVelocity(left)
self.motors[1].setVelocity(right)
def turnRound(self, diff):
#set speed for turning left or right
if abs(diff) < 0.001:
self.setSpeed(0, 0)
return False
else:
self.setSpeed(-0.3 * diff, 0.3 * diff)
return True
def linePatrol(self):
#get camera image, process it and set speed based on line patrolling algorithm
#return False if there is no line
image = self.camera.getImage()
leftSum = 0
rightSum = 0
leftSpeed = 0
rightSpeed = 0
cameraData = self.camera.getImage()
HEIGHT = self.camera.getHeight()
WIDTH = self.camera.getWidth()
frame = np.zeros((HEIGHT, WIDTH))
for x in range(0, HEIGHT):
for y in range(0, WIDTH):
frame[y][x] = int(
self.camera.imageGetGray(cameraData, WIDTH, x, y))
absX = cv2.convertScaleAbs(cv2.Sobel(frame, cv2.CV_16S, 1, 0))
absY = cv2.convertScaleAbs(cv2.Sobel(frame, cv2.CV_16S, 0, 1))
# to binary
ret, binary = cv2.threshold(cv2.addWeighted(absX, 0.5, absY, 0.5, 0),
254, 255, cv2.THRESH_BINARY)
binaryImg = cv2.boxFilter(cv2.filter2D(
binary, -1,
np.ones((20, 20), np.float) / 25),
-1, (15, 15),
normalize=1)
positionSum = 0
positionCount = 0
for i in range(int(HEIGHT * 0.75), HEIGHT):
for j in range(WIDTH * 0, WIDTH):
if binaryImg[i][j] != 0:
positionSum += j
positionCount += 1
farpositionSum = 0
farpositionCount = 0
for i in range(int(HEIGHT * 0.32), int(HEIGHT * 0.44)):
for j in range(WIDTH * 0, WIDTH):
if binaryImg[i][j] != 0:
farpositionSum += j
farpositionCount += 1
if farpositionCount == 0:
farcenter = 0
else:
farcenter = farpositionSum / farpositionCount
if abs(farcenter - WIDTH / 2) < 0.1:
self.motors[0].setAcceleration(0.3)
self.motors[1].setAcceleration(0.3)
#("straight")
else:
self.motors[0].setAcceleration(20)
self.motors[1].setAcceleration(20)
#print("not straight")
if positionCount or farpositionCount:
if positionCount == 0:
center = 80
else:
center = positionSum / positionCount
diff = 4 * (0.5 - center / WIDTH)
if diff > 0.01:
leftSpeed = (1 - 0.6 * diff) * self.MAX_SPEED
rightSpeed = (1 - 0.3 * diff) * self.MAX_SPEED
elif diff < -0.01:
leftSpeed = (1 + 0.3 * diff) * self.MAX_SPEED
rightSpeed = (1 + 0.6 * diff) * self.MAX_SPEED
else:
leftSpeed = self.MAX_SPEED
rightSpeed = self.MAX_SPEED
self.setSpeed(leftSpeed, rightSpeed)
return True
else:
return False
@staticmethod
def linePatrol2():
# Task 1
# get_frame
frame = Vehicle.get_frame()
# edge_detect
blur_im = Vehicle.edge_detect(frame, 100, 255, 20, 15)
# positioning
position = Vehicle.positioning(blur_im, 0, 0.7, 0.8)
# steering
leftSpeed, rightSpeed = Vehicle.steering(position, 0, 4, 0.8, 0.8)
return leftSpeed, rightSpeed
@staticmethod
def box_found():
# Task 2
HALF = Vehicle.HALF
frame = Vehicle.get_frame()
x = cv2.Sobel(frame, cv2.CV_16S, 1, 0)
y = cv2.Sobel(frame, cv2.CV_16S, 0, 1)
absx = cv2.convertScaleAbs(x)
absy = cv2.convertScaleAbs(y)
dst = cv2.addWeighted(absx, 0.5, absy, 0.5, 0)
cv2.imwrite("dst.jpg", dst)
# to binary
ret, filtered = cv2.threshold(dst, 50, 255, cv2.THRESH_TOZERO)
cv2.imwrite("binary.jpg", filtered)
position_i = Vehicle.positioning(filtered, 0, 0.32, 0.35)
if abs(position_i - HALF) < 10:
position = Vehicle.positioning(filtered, 0, 0.25, 0.3)
if abs(
position - HALF
) < 10: # this is a predict letting it stop exactly at the box center
return True
else:
pass
@staticmethod
def bridge_found():
# Task 3
HALF = Vehicle.HALF
WIDTH = Vehicle.WIDTH
HEIGHT = Vehicle.HEIGHT
count = 0
num1 = 0
num = 0
axis = WIDTH
position_i = 0
frame = Vehicle.get_frame()
x = cv2.Sobel(frame, cv2.CV_16S, 1, 0)
y = cv2.Sobel(frame, cv2.CV_16S, 0, 1)
absx = cv2.convertScaleAbs(x)
absy = cv2.convertScaleAbs(y)
dst = cv2.addWeighted(absx, 0.5, absy, 0.5, 0)
# cv2.imwrite("dst.jpg", dst)
# to binary
ret, filtered = cv2.threshold(dst, 50, 255, cv2.THRESH_TOZERO)
# cv2.imwrite("filtered.jpg", filtered)
for axis_v in range(int(HEIGHT * 0.8), int(HEIGHT * 0.87)):
for axis_h in range(HALF, WIDTH):
if filtered[axis_v][axis_h] != 0:
axis = axis + axis_h
num1 = num1 + 1
if num1:
position_i = axis / num1 + 1
if abs(position_i - 1.5 * HALF) < 10:
# print('pi = ', position_i)
for axis_v in range(int(HEIGHT * 0.82), int(HEIGHT * 0.85)):
for axis_h in range(WIDTH * 0, int(WIDTH * 1)):
if filtered[axis_v][axis_h] != 0:
axis_i = axis_h
axis = min(axis_i, axis)
# print('axis = ', axis)
if abs(axis - 0.5 * WIDTH) < 5:
return True
else:
return False
def arch_found(self, a, b):
# Task 4
global count_arch
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
camera = Vehicle.camera
cameraData = camera.getImage()
frame = np.zeros((HEIGHT, WIDTH))
Q = 0
position = 0
for x in range(0, WIDTH):
for y in range(0, HEIGHT):
gray = int(camera.imageGetGray(cameraData, WIDTH, x, y))
if 110 < gray < 130:
frame[y][x] = 255
Q = Q + 1
else:
pass
#print(i)
# cv2.imwrite('frame.jpg', frame)
#blur_im = self.edge_detect(frame, 254, 255)
#cv2.imwrite('blur.jpg', blur_im)
if Q > 50:
position = self.positioning(frame, 0, a, b)
#print(i, position)
if count_arch == 0:
if int(position) < WIDTH / 2:
count_arch = 1
for i in range(2):
self.motors[i].setVelocity(0)
return 1
else:
if int(position) > WIDTH / 2:
for i in range(2):
self.motors[i].setVelocity(0)
return 1
else:
return 0
else:
return 0
def count(self):
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
camera = Vehicle.camera
cameraData = camera.getImage()
i = 0
for x in range(0, WIDTH):
for y in range(0, HEIGHT):
gray = int(camera.imageGetGray(cameraData, WIDTH, x, y))
if 110 < gray < 130:
i = i + 1
else:
pass
#print(i)
return i
@staticmethod
def colourPatrol():
# Task 5
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
camera = Vehicle.camera
cameraData = camera.getImage()
frame = np.zeros((HEIGHT, WIDTH))
color_kernel = np.zeros((3, 3))
compass = Vehicle.getCompass(Vehicle)
position = HEIGHT / 2
leftSpeed = 0
rightSpeed = 0
global flag
if (flag == -1) and (3.12 < abs(compass) < 3.15):
for x in range(0, 3):
for y in range(0, 3):
gray = int(
camera.imageGetGray(cameraData, WIDTH,
x + int(0.5 * WIDTH),
y + int(0.80 * HEIGHT)))
color_kernel[y][x] = gray
gray_average = np.mean(color_kernel)
# color classification
if 100 < gray_average < 130:
flag = 0 # red
elif 175 < gray_average < 200:
flag = 1 # yellow
elif 140 < gray_average < 170:
flag = 2 # purple
else:
pass
# print('color flag=', flag)
leftSpeed = 2.0
rightSpeed = 2.0
elif flag == 0 or flag == 1 or flag == 2:
for x in range(0, 3):
for y in range(0, 3):
gray = int(
camera.imageGetGray(cameraData, WIDTH,
x + int(0.5 * WIDTH),
y + int(0.95 * HEIGHT)))
color_kernel[y][x] = gray
gray_average = np.mean(color_kernel)
if 90 < gray_average < 100:
flag = 3 # finished
for y in range(0, HEIGHT):
for x in range(0, WIDTH):
gray = int(camera.imageGetGray(cameraData, WIDTH, x, y))
# color classification
if flag == 0:
if 100 < gray < 130:
frame[y][x] = 255
else:
pass
elif flag == 1:
if 175 < gray < 200:
frame[y][x] = 255
else:
pass
elif flag == 2:
if 140 < gray < 170:
frame[y][x] = 255
else:
pass
# edge_detect Sobel
x = cv2.Sobel(frame, cv2.CV_16S, 1, 0)
y = cv2.Sobel(frame, cv2.CV_16S, 0, 1)
absX = cv2.convertScaleAbs(x)
absY = cv2.convertScaleAbs(y)
dst = cv2.addWeighted(absX, 0.5, absY, 0.5, 0)
# to binary
ret, binary = cv2.threshold(dst, 254, 255, cv2.THRESH_BINARY)
# Smooth
kernel1 = np.ones((20, 20), np.float) / 25
smooth = cv2.filter2D(binary, -1, kernel1)
# blur
blur_im1 = cv2.boxFilter(smooth, -1, (15, 15), normalize=1)
# cv2.imwrite('smooth_blur.jpg', blur_im1)
# cv2.imwrite('gray.jpg', frame)
# cv2.imwrite('frame.jpg', frame)
# video.write(blur_im1)
# Locate
axis = 0
num = 0
for axis_v in range(int(HEIGHT * 0.8), int(HEIGHT * 0.9)):
for axis_h in range(WIDTH * 0, WIDTH):
if blur_im1[axis_v][axis_h] != 0:
axis = axis + axis_h
num = num + 1
if num:
position = axis / num + 1
# Steering
if 5 <= abs(position - WIDTH / 2):
leftSpeed = (position / WIDTH) * 1.5
rightSpeed = (1 - position / WIDTH) * 1.5
elif 2.5 < abs(position - WIDTH / 2) < 5:
leftSpeed = (position / WIDTH) * 0.9
rightSpeed = (1 - position / WIDTH) * 0.9
# if lines are losted in the view of camera, try adjust parameters here
else:
leftSpeed = 1
rightSpeed = 1
else:
pass
return leftSpeed, rightSpeed
lms291_yatayda = Lidar.getHorizontalResolution(lms291)
#print(lms291_yatayda)
#yatay=lms291_yatayda/2
#max_range=Lidar.getMaxRange(lms291)
#num_points=Lidar.getNumberOfPoints(lms291)
print("Lidar Başladı")
#araç üzeirnden gyro çekme
gyro = robot.getGyro("gyro")
Gyro.enable(gyro, timestep)
#araç üzerinden pususla çağırma
pusula = robot.getCompass("compass")
Compass.enable(pusula, timestep)
# motorların tagını getirir
#motorları getirir
solMotorİleri = robot.getMotor("front left wheel")
sağMotorİleri = robot.getMotor("front right wheel")
sağMotorGeri = robot.getMotor("back right wheel")
solMotorGeri = robot.getMotor("back left wheel")
#motorları hareket etirir
solMotorİleri.setPosition(float("inf"))
solMotorGeri.setPosition(float("inf"))
sağMotorİleri.setPosition(float("inf"))
sağMotorGeri.setPosition(float("inf"))
sayici = 0
robot = Robot()
M_PI=np.pi
k_pitch_p=30.0
k_roll_p=50.0
k_vertical_p=3.0
k_vertical_thrust=68.5
k_vertical_offset=0.6
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
camera=robot.getCamera("camera")
Camera.enable(camera,timestep)
imu=InertialUnit("inertial unit")
imu.enable(timestep)
pusula=Compass("compass")
gyro=Gyro("gyro")
pusula.enable(timestep)
gyro.enable(timestep)
gps=GPS("gps")
gps.enable(timestep)
# motorların tagını getirir
#motorları getirir
solMotorİleri=robot.getMotor("front left propeller")
sağMotorİleri=robot.getMotor("front right propeller")
sağMotorGeri=robot.getMotor("rear right propeller")
solMotorGeri=robot.getMotor("rear left propeller")
#motorları hareket etirir
solMotorİleri.setPosition(float("inf"))
class BaseController():
def __init__(self, trajectory):
# Initialize variables
self.trajectory = trajectory
self.previousX = 0
self.previousY = 0
self.previousZ = 0
self.previousPsi = 0
self.previousXdotError = 0
self.integralXdotError = 0
def startSensors(self, timestep):
# Instantiate objects and start up GPS, Gyro, and Compass sensors
# For more details, refer to the Webots documentation
self.gps = GPS("gps")
self.gps.enable(timestep)
self.gyro = Gyro("gyro")
self.gyro.enable(timestep)
self.compass = Compass("compass")
self.compass.enable(timestep)
def getStates(self, timestep):
# Timestep returned by Webots is in ms, so we convert
delT = 0.001 * timestep
# Extract (X, Y) coordinate from GPS
position = self.gps.getValues()
X = position[0]
Y = position[1]
# Find the rate of change in each axis, and store the current value of (X, Y)
# as previous (X, Y) which will be used in the next call
Xdot = (X - self.previousX) / (delT + 1e-9)
self.previousX = X
Ydot = (Y - self.previousY) / (delT + 1e-9)
self.previousY = Y
XYdot = np.array([[Xdot], [Ydot]])
# Get heading angle and angular velocity
psi = wrapToPi(self.getBearingInRad())
angularVelocity = self.gyro.getValues()
psidot = angularVelocity[2]
# Get the rotation matrix (2x2) to convert velocities to the vehicle frame
rotation_mat = np.array([[np.cos(psi), -np.sin(psi)],
[np.sin(psi), np.cos(psi)]])
xdot = (np.linalg.inv(rotation_mat) @ XYdot)[0, 0]
ydot = (np.linalg.inv(rotation_mat) @ XYdot)[1, 0]
# Clamp xdot above 0 so we don't have singular matrices
xdot = clamp(xdot, 1e-5, np.inf)
return delT, X, Y, xdot, ydot, psi, psidot
def getBearingInRad(self):
# Get compass relative north vector
north = self.compass.getValues()
# Calculate vehicle's heading angle from north
rad = np.arctan2(north[1], north[0])
# Convert to vehicle's heading angle from x-axis
bearing = np.pi / 2.0 - rad
return bearing