def run(self): while True: # Read the supervisor order. if self.receiver.getQueueLength() > 0: message = self.receiver.getData().decode('utf-8') self.receiver.nextPacket() print('I should ' + message + '!') #if message == 'avoid obstacles': #self.mode = self.Mode.AVOIDOBSTACLES #elif message == 'move forward': # self.mode = self.Mode.MOVE_FORWARD # elif message == 'stop': #self.mode = self.Mode.STOP #elif message == 'turn': #self.mode = self.Mode.TURN delta = self.distanceSensors[0].getValue() - self.distanceSensors[1].getValue() speeds = [0.0, 0.0] gps = GPS("ambu") gps.enable(self.timeStep) gpsvalues = [] gpsvalues.extend(gps.getValues()) gps1stvalue = gpsvalues[0] gps11stvalue = round(gps1stvalue, 2) gps2ndvalue = gpsvalues[1] gps2ndvalue = round(gps2ndvalue, 2) gps3rdvalue = gpsvalues[2] gps3rdvalue =round(gps3rdvalue, 2) new_gst_values = np.arange(0.5,0.9,0.01) #position = [] #position = gps.getValues() #latitude = GPS.convertToDegreesMinutesSeconds(position[0]) #altitude = position[2] #longitude = GPS.convertToDegreesMinutesSeconds(position[1]) #total_speed = gps.getSpeed(gps) #print(latitude) #print(gpsvalues) #lightsensor = LightSensor("light") #lightsensor = self.getLightSensor ('light') #ligfhtsensor.enable(self.timestep) # Send actuators commands according to the mode. if self.mode == self.Mode.AVOIDOBSTACLES: speeds[0] = self.boundSpeed(self.maxSpeed / 2 + 0.1 * delta) speeds[1] = self.boundSpeed(self.maxSpeed / 2 - 0.1 * delta) elif self.mode == self.Mode.MOVE_FORWARD: speeds[0] = self.maxSpeed speeds[1] = self.maxSpeed elif self.mode == self.Mode.TURN: speeds[0] = self.maxSpeed / 2 speeds[1] = -self.maxSpeed / 2 self.motors[0].setVelocity(speeds[0]) self.motors[1].setVelocity(speeds[1]) # Perform a simulation step, quit the loop when # Webots is about to quit. if self.step(self.timeStep) == -1: break
class BaseController(): """ The base controller class. """ def __init__(self, robot, lossOfThrust = 0): """ Base controller __init__ method. Initialize drone parameters here. Args: robot (webots controller object): Controller for the drone. lossOfThrust (float): percent lost of thrust. """ # Initialize variables self.robot = robot self.timestep = 0 # intializa percent loss of thrust self.lossOfThrust = lossOfThrust # Define robot parameter self.m = 0.4 self.d1x = 0.1122 self.d1y = 0.1515 self.d2x = 0.11709 self.d2y = 0.128 self.Ix = 0.000913855 self.Iy = 0.00236242 self.Iz = 0.00279965 # define constants self.g = 9.81 self.ct = 0.00026 self.ctau = 5.2e-06 self.U1_max = 10 self.pi = 3.1415926535 # define H matrix for conversion from control input U to motor speeds self.H_inv = self.ct*np.array([[1, 1, 1, 1], [self.d1y, -self.d1y, self.d2y, -self.d2y], [-self.d1x, -self.d1x, self.d2x, self.d2x], [-self.ctau/self.ct, self.ctau/self.ct, self.ctau/self.ct, -self.ctau/self.ct] ]) self.H = np.linalg.inv(self.H_inv) # define variables for speed calculations self.xGPS_old = 0 self.yGPS_old = 0 self.zGPS_old = 0.099019 def startSensors(self, timestep): """ Start sensors. Instantiate objects and start up GPS, Gyro, IMU sensors. For more details, refer to the Webots documentation. Args: timestep (int): time step of the current world. """ self.gps = GPS("gps") self.gps.enable(timestep) self.gyro = Gyro("gyro") self.gyro.enable(timestep) self.imu = InertialUnit("inertial unit") self.imu.enable(timestep) self.timestep = timestep def getStates(self): """ Get drone state. The state of drone is 16 dimensional: xGPS, yGPS, zGPS, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_rate, pitch_rate, yaw_rate Returns: np.array: x_t. information of 12 states. """ # Timestep returned by Webots is in ms, so we convert delT = 1e-3*self.timestep # Extract (X, Y, Z) coordinate from GPS xGPS = self.gps.getValues()[0] yGPS = -self.gps.getValues()[2] zGPS = self.gps.getValues()[1] # Find the rate of change in each axis, and store the current value of (X, Y, Z) # as previous (X, Y, Z) which will be used in the next call x_vel = (xGPS - self.xGPS_old)/delT y_vel = (yGPS - self.yGPS_old)/delT z_vel = (zGPS - self.zGPS_old)/delT self.xGPS_old = xGPS self.yGPS_old = yGPS self.zGPS_old = zGPS # Extract (roll, pitch, yaw) angle from imu roll = self.imu.getRollPitchYaw()[0] pitch = -self.imu.getRollPitchYaw()[1] yaw = self.imu.getRollPitchYaw()[2] # Extract (roll rate, pitch rate, yaw rate) angular velocity from imu roll_rate = self.gyro.getValues()[0] pitch_rate = -self.gyro.getValues()[2] yaw_rate = self.gyro.getValues()[1] x_t = np.array([xGPS, yGPS, zGPS, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_rate, pitch_rate, yaw_rate]).reshape(-1,1) return x_t def getMotorAll(self): """ Get each motors' controller. Returns: list: Each motor's controller. """ frontLeftMotor = self.robot.getMotor('front left propeller') frontRightMotor = self.robot.getMotor('front right propeller') backLeftMotor = self.robot.getMotor('rear left propeller') backRightMotor = self.robot.getMotor('rear right propeller') return [frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] def initializeMotors(self): """ Initialisze all motors speed to 0. """ [frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] = self.getMotorAll() frontLeftMotor.setPosition(float('inf')) frontRightMotor.setPosition(float('inf')) backLeftMotor.setPosition(float('inf')) backRightMotor.setPosition(float('inf')) self.motorsSpeed(0, 0, 0, 0) def motorsSpeed(self, v1, v2, v3, v4): """ Set each motors' speed. Args: v1, v2, v3, v4 (int): desired speed for each motor. """ [frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] = self.getMotorAll() frontLeftMotor.setVelocity(v1) frontRightMotor.setVelocity(v2) backLeftMotor.setVelocity(v3) backRightMotor.setVelocity(v4) def convertUtoMotorSpeed(self, U): """ Convert control input to motor speed. Args: U (np.array): desired control input. Returns: np.array: rotorspeed. Desired rotor speed. """ w_squre = np.clip(np.matmul(self.H, U), 0, 576**2) rotorspeed = np.sqrt(w_squre.flatten()) return rotorspeed def setMotorsSpeed(self, motorspeed, motor_failure=0): """ Set motor speed. Args: motorspeed (np.array): desired motor speed. motor_failure (bool): True for motor failure, False otherwise. """ if motor_failure : # print("--- Motor Failure ---") factor = np.sqrt(1 - self.lossOfThrust) self.motorsSpeed(float(motorspeed[0]) * factor, float(-motorspeed[1]), float(-motorspeed[2]), float(motorspeed[3])) else: self.motorsSpeed(float(motorspeed[0]), float(-motorspeed[1]), float(-motorspeed[2]), float(motorspeed[3]))
# You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor from controller import Robot, GPS # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # You should insert a getDevice-like function in order to get the # instance of a device of the robot. Something like: # motor = robot.getMotor('motorname') # ds = robot.getDistanceSensor('dsname') # ds.enable(timestep) gp = robot.getGPS("gps") GPS.enable(gp, 64) # Main loop: # - perform simulation steps until Webots is stopping the controller while robot.step(timestep) != -1: # Read the sensors: # Enter here functions to read sensor data, like: # val = ds.getValue() print(GPS.getValues(gp)) # Process sensor data here. # Enter here functions to send actuator commands, like: # motor.setPosition(10.0) pass # Enter here exit cleanup code.
# from controller import Robot, Motor, DistanceSensor import sys from controller import Robot, Motor, Keyboard from controller import GPS import time import numpy as np import matplotlib.pyplot as plt import math TIME_STEP = 64 #timestep should be the same as in the WorldInfo # create the Robot, Keyboard, GPS instance. robot = Robot() keyboard = Keyboard() keyboard.enable(TIME_STEP) gps = GPS('global') gps.enable(TIME_STEP) # assign the motors to wheels. wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4'] for i in range(4): wheels.append(robot.getMotor(wheelsNames[i])) wheels[i].setPosition( float('inf')) #set position infinity not to restrict motion wheels[i].setVelocity(5.0) #set initial speed as 5.0 rad/s print('Click the simulation window before the keyboard command!') print('Press [D] or [d] to give right input') print('Press [A] or [a] to give left input') time = 0 leftSpeed = 0.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")) solMotorGeri.setPosition(float("inf")) sağMotorİleri.setPosition(float("inf")) sağMotorGeri.setPosition(float("inf")) #camera motorlarını çağıralım
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