# 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
#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)
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"))
solMotorGeri.setPosition(float("inf"))
sağMotorİleri.setPosition(float("inf"))
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
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