from globals import pixelStrip, startFrom, logInitial
from gps import GPS
if __name__ == "__main__":
gps = GPS(logging=False)
while gps.getCoords() == (0,0):
print("Awaiting gps lock.")
#time.sleep(0.2)
gps.getGPS(force=1)
startingPos = gps.getCoords() #so we know to stop 15m afterwards
issueCommands(0,0) #connect to car
distanceAway = 0
while distanceAway < 12: #go forward 15m
distanceAway = distance.distance(gps.getGPS(force=1), startingPos).m
print("Distance away from destination: ", distanceAway)
issueCommands(1,100) #go straight
issueCommands(0,0)
startLeft = issueCommands.car.get_left_pulse() #should be 0
startRight = issueCommands.car.get_right_pulse() #should be 0
startPulse = int((startLeft + startRight)/2)
flip = 1 #goes right first
def main(visual, green, record, replay, loop, rc, cFlip):
"""//Used when stopping is needed.
gps = GPS()
while gps.getGPS(force=1) == (0,0):
print("Awaiting GPS lock.")
time.sleep(1)
"""
ic = ImageCap(False, replay) # ImCapt() #initializes zed object
if not visual and not rc: #initalizes CAN connection
issueCommands(0, 0)
calculateReading.pastCom = 0 # in case we don't see cones straight away
startTime = time.time()
listReadings = []
lapCounter = 0
timeMarker = time.time()
setStart = True
try:
i = 0
while loop: # for amount in range(framesToDo):
time1 = time.time()
logging.warning("\n*********\nFrame: " + str(i))
image, depth = ic.latest(record)
logging.info("Getting latest image and depth.")
t = threading.Thread(target=ImageCap.capture, args=(ic, ))
if not record:
original_image = image
depth_data_ocv = depth[startFrom:startFrom + pixelStrip]
image_ocv = original_image[startFrom:startFrom + pixelStrip]
steering, velocity = imProcessing(t, image_ocv, depth_data_ocv,
visual, original_image,
green, cFlip)
else:
steering, velocity = 0, 0
cv2.imshow("image", image)
cv2.waitKey(1)
if ic.exit: # when we replay tests
raise KeyboardInterrupt
# steering = min(19, max(-19, steering))
#Uncomment to cap steering to a certain boundary
""" //To be used when counting laps
if setStart:
velocity -= 30
if time.time()-startTime > 12:
startingPos = gps.getGPS()
setStart = False
gps.coords = (-1,-1)
if time.time() - timeMarker > 70:
velocity -= 50
if time.time()-timeMarker > 75: #only checks 30s after we've passed the starting point
gps.getGPS(timeBound=2)
if distance.distance(gps.getCoords(), startingPos).m < 7: #5m within the finish line
lapCounter += 1
timeMarker = time.time() #resets the time marker
if lapCounter == 10: #change to 10 in the future
raise KeyboardInterrupt
"""
print("Steering: ", steering)
print("Velocity: ", velocity)
logging.info("Steering: %d, Velocity: %d", steering, velocity)
issueCommands(steering, velocity, False, visual, replay, record,
rc)
listReadings.append(steering)
if not isinstance(loop, bool):
loop -= 1
if loop == 0:
raise KeyboardInterrupt
t.join()
if 'car' in issueCommands.__dict__:
if issueCommands.car.checkEBS():
raise KeyboardInterrupt
i += 1
logging.warning("End of frame.\n\n")
#print(time.time()-time1)
#print("Frames left: ", framesToDo-amount)
except KeyboardInterrupt:
if not replay:
ic.zed.close()
issueCommands(0, 0, False, visual, replay, record, rc)
time.sleep(4)
issueCommands(0, 0, True, visual, replay, record,
rc) #initiates the exit protocol
f1 = open("../test/pastMission.txt", "w")
for element in listReadings:
f1.write(str(element) + ",")
f1.close()
print("Seconds it took: ", time.time() - startTime)
print("Total frames: ", len(listReadings))
print("Actual framerate: ",
len(listReadings) / (time.time() - startTime))
print("\nFINISH")
logging.info("\nTotal seconds: %d", time.time() - startTime)
logging.info("Framerate: %d",
len(listReadings) / (time.time() - startTime))
logging.warning("Mission end.\n\n")
quit()
from cmds import issueCommands
from gps import GPS
import math
if __name__ == "__main__":
gps = GPS(logging=False)
gps.getGPS()
while gps.getCoords() == (0, 0):
print("Awaiting gps lock.")
startingPos = gps.getCoords()
issueCommands(0, 0)
distanceAway = 0
while distanceAway < 0: #go forward 15m
gps.getGPS()
distanceAway = (distance.distance(gps.getCoords(), startingPos)).m
print("Distance away from destination: ", distanceAway)
issueCommands(0, 85)
issueCommands(0, 0) #connect to car
startLeft = issueCommands.car.get_left_pulse() #should be 0
startRight = issueCommands.car.get_right_pulse() #should be 0
carWidth = 1.17 #in meters
fullCircleDistance = 2 * math.pi * carWidth
tyreCirc = math.pi * 0.58
fullCircleRotations = fullCircleDistance / tyreCirc
from cmds import issueCommands
from globals import carVelocity
import time
if __name__ == "__main__":
while True:
try:
steering = int(input("Steering: "))
issueCommands(steering, carVelocity)
except:
issueCommands(0, 0)
time.sleep(4)
issueCommands(0, 0, True)
from cmds import issueCommands
import time
if __name__ == "__main__":
print("Starting inspection.")
for i in range(9):
if i == 0:
issueCommands(0, 0) #connects and sets up CAN
elif i == 1:
issueCommands(24, 0) #sweeping through steering
time.sleep(2)
elif i == 2:
issueCommands(-24, 0) #sweeping through steering
time.sleep(2)
elif i == 3:
issueCommands(0, 0) #straightens up
time.sleep(1)
elif i == 4:
issueCommands(0, 200) #ramps up to 200 within 10s
time.sleep(5)
elif i == 5:
issueCommands(0, 0) #stops within 5 seconds
time.sleep(3)
elif i == 6:
issueCommands(0, 70) #ramp up to 50 for 3s
time.sleep(3)
elif i == 7:
issueCommands.car.deploy_EBS() #deploy EBS
time.sleep(5)
def main(visual, green, record, replay, loop, rc, cFlip):
calculateReading.pastCom = 0 # in case we don't see cones straight away
ic = ImageCap(False, replay) # ImCapt() #initializes zed object
startTime = time.time()
listReadings = []
gps = GPS()
while gps.getGPS() == (0, 0):
print("Waiting on GPS lock.")
time.sleep(1)
issueCommands(0, 0) #makes connection
time.sleep(3) #waits 3 seconds
issueCommands(-24, 0) #sweeps left
time.sleep(2)
issueCommands(24, 0) #sweep right
time.sleep(2)
issueCommands(0, 0)
startingPos = gps.getGPS()
running = True
timeCheck = False
ebs = False
try:
i = 0
while loop: # for amount in range(framesToDo):
logging.warning("\n*********\nFrame: " + str(i))
image, depth = ic.latest(record)
logging.info("Getting latest image and depth.")
t = threading.Thread(target=ImageCap.capture, args=(ic, ))
if not record:
original_image = image
depth_data_ocv = depth[startFrom:startFrom + pixelStrip]
image_ocv = original_image[startFrom:startFrom + pixelStrip]
steering, velocity = imProcessing(t,
image_ocv,
depth_data_ocv,
visual,
original_image,
green,
cFlip,
gates=0)
else: #looks for one gate
steering, velocity = 0, 0
cv2.imshow("image", image)
cv2.waitKey(1)
# works faster here for performance reasons
if ic.exit: # when we replay tests
raise KeyboardInterrupt
steering = min(19, max(-19, steering))
if running:
velocity = 250 #15kph
else:
velocity = 0
if distance.distance(gps.getGPS(), startingPos).m > 10:
running = False
timeCheck = time.time()
issueCommands(0, 0)
time.sleep(5)
issueCommands(0, 250)
time.sleep(2)
issueCommands.car.deploy_ebs()
raise KeyboardInterrupt
if timeCheck:
if time.time() - timeCheck > 5: #after 5 seconds
while gps.getGPS(True) == startingPos:
pass #resets starting position so we go for 10m
startingPos = gps.getGPS(True)
running = True #resets velocity to 15kph
ebs = True #it will deploy ebs once it reaches 10m again
print("Steering: ", steering)
print("Velocity: ", velocity)
logging.info("Steering: %d, Velocity: %d", steering, velocity)
issueCommands(steering, velocity, False, visual, replay, record,
rc)
listReadings.append(steering)
if not isinstance(loop, bool):
loop -= 1
if loop == 0:
raise KeyboardInterrupt
if issueCommands.car.checkEBS():
raise KeyboardInterrupt
i += 1
t.join()
i += 1
logging.warning("End of frame.\n\n")
#print("Frames left: ", framesToDo-amount)
except KeyboardInterrupt:
if not replay:
ic.zed.close()
if not ebs:
issueCommands(0, 0, False, visual, replay, record, rc)
time.sleep(4)
issueCommands(0, 0, True, visual, replay, record,
rc) #initiates the exit protocol
print("Seconds it took: ", time.time() - startTime)
print("Total frames: ", len(listReadings))
print("Actual framerate: ",
len(listReadings) / (time.time() - startTime))
print("\nFINISH")
logging.info("\nTotal seconds: %d", time.time() - startTime)
logging.info("Framerate: %d",
len(listReadings) / (time.time() - startTime))
logging.warning("Mission end.\n\n")
quit()