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