psiVec = []
psidotVec = []
FVec = []
minDist = []
passMiddlePoint = False
nearGoal = False
finish = False
while driver.step() != -1:
# Call control update method
X, Y, xdot, ydot, psi, psidot, F, delta = \
customController.update(timestep)
# Set control update output
driver.setThrottle(clamp(F / throttleConversion, 0, 1))
driver.setSteeringAngle(-clamp(delta, np.radians(-30), np.radians(30)))
# Check for halfway point/completion
disError, nearIdx = closestNode(X, Y, trajectory)
consoleObject.consoleUpdate(disError, nearIdx)
speedometerObject.speedometerUpdate(speedometerGraphic, xdot * msToKmh)
stepToMiddle = nearIdx - len(trajectory) / 2.0
if abs(stepToMiddle) < 100.0 and passMiddlePoint == False:
passMiddlePoint = True
if passMiddlePoint == True:
console.drawText("Middle point passed.", 5, 60)
from vehicle import Driver
import os
import pickle
curr_dir = os.getcwd()
par_dir = curr_dir[:curr_dir.rfind('/')]
driver = Driver()
driver.setSteeringAngle(0.0)
driver.setGear(1)
driver.setThrottle(1)
driver.setCruisingSpeed(100)
camera = driver.getCamera('camera')
camera.enable(1)
brake_coeff = 9.0
counter = 0
braking = False
braking_threshold = 0.3
friction_acc = 0.5
ignore_smart_intersection = True # Set this to True if you want to ignore smart intersection
while driver.step() != -1:
counter = counter + 1
speed = driver.getCurrentSpeed() * (5. / 18.)
while True:
try:
