def init():
GPIO.setup(RIGHT_MOTOR_DIR, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(LEFT_MOTOR_DIR, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(RIGHT_MOTOR, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(LEFT_MOTOR, GPIO.OUT, initial=GPIO.LOW)
pwm = [GPIO.PWM(RIGHT_MOTOR, 50), GPIO.PWM(LEFT_MOTOR, 50)]
pwm[0].start(0)
pwm[1].start(0)
def __init__(self, pwm_pin, ctrl_pin1, ctrl_pin2, **kwargs):
self.pwm_pin = pwm_pin
self.ctrl_pin1 = ctrl_pin1
self.ctrl_pin2 = ctrl_pin2
# Motor calibration
self.alpha = kwargs.get('alpha', 1.0)
self.beta = kwargs.get('beta', 0.0)
# GPIO setup
GPIO.setup(self.pwm_pin, GPIO.OUT)
self.pwm = GPIO.PWM(self.pwm_pin, 50) # (channel, frequency)
self.pwm.start(0)
self.speed = 0
GPIO.setup(self.ctrl_pin1, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(self.ctrl_pin2, GPIO.OUT, initial=GPIO.LOW)
def __init__(self, EN, IN1, IN2):
self.speed = EN
self.forward = IN1
self.backward = IN2
self.pwm = None
# pins setup
GPIO.setup(self.speed, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(self.forward, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(self.backward, GPIO.OUT, initial=GPIO.LOW)
# pwm setup
self.pwm = GPIO.PWM(self.speed, 50) # frequency of pwm signal: 50Hz
self.pwm.start(0)
def main():
# Pin Setup:
# Board pin-numbering scheme
GPIO.setmode(GPIO.BOARD)
# set pin as an output pin with optional initial state of HIGH
GPIO.setup(output_pin, GPIO.OUT, initial=GPIO.LOW)
p = GPIO.PWM(output_pin, 100)
p.start(50)
print("PWM running. Press CTRL+C to exit.")
try:
while True:
time.sleep(.1)
finally:
p.stop()
GPIO.cleanup()
def __init__(self):
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(DC_ENA, GPIO.OUT)
GPIO.setup(DC_IN1, GPIO.OUT)
GPIO.setup(DC_IN2, GPIO.OUT)
GPIO.setup(SERVO, GPIO.OUT)
GPIO.setup(HEADLIGHT, GPIO.OUT)
GPIO.setup(BACKLIGHT, GPIO.OUT)
GPIO.setup(BREAKLIGHT, GPIO.OUT)
GPIO.setup(WAVE_TRIG, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(WAVE_ECHO, GPIO.IN)
self.servo = GPIO.PWM(SERVO, 50)
self.servo.start(8)
def main():
# Pin Setup:
# Board pin-numbering scheme
GPIO.setmode(GPIO.BOARD)
# set pin as an output pin with optional initial state of HIGH
GPIO.setup(output_pin, GPIO.OUT, initial=GPIO.HIGH)
pwm = GPIO.PWM(output_pin, 50)
val = 25
incr = 5
pwm.start(val)
print("PWM running. Press CTRL+C to exit.")
try:
while True:
time.sleep(0.25)
if val >= 100:
incr = -incr
if val <= 0:
incr = -incr
val += incr
pwm.ChangeDutyCycle(val)
finally:
pwm.stop()
GPIO.cleanup()
def setup(self):
GPIO.setup(self.pwm_pin, GPIO.OUT)
self.pwm = GPIO.PWM(self.pwm_pin, self.pwm_frequency)
self.pwm.start(self._clip_value(self.pwm_init_duty_cycle))
import time import Jetson.GPIO as GPIO # MOTORS SETUP # GPIO.cleanup() #clean trash from ports GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) # Motor left GPIO.setup(18, GPIO.OUT, initial=GPIO.LOW) # IN1 GPIO.setup(17, GPIO.OUT, initial=GPIO.LOW) # IN2 # Motor right GPIO.setup(23, GPIO.OUT, initial=GPIO.LOW) # IN3 GPIO.setup(22, GPIO.OUT, initial=GPIO.LOW) # IN4 #PWM Setup GPIO.setup(13, GPIO.OUT) GPIO.setup(12, GPIO.OUT) M1=GPIO.PWM(13, 50) M2=GPIO.PWM(12, 50) M1.start(0) # define color detection boundaries lowerBound = np.array([29, 85, 6]) upperBound = np.array([64, 255, 255]) # camera setup dispW=640 dispH=480 flip=2 camSet='nvarguscamerasrc ! video/x-raw(memory:NVMM), width=1980, height=1080, format=NV12, framerate=59/1 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(dispW)+', height='+str(dispH)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink' cam = cv2.VideoCapture(camSet) # setting up kernel properties with matrices kernelOpen = np.ones((5, 5)) kernelClose = np.ones((21, 21))
def main():
pinList = []
GPIO.setmode(GPIO.BOARD)
for key in pins.keys():
pinList.append(pins[key])
GPIO.setup(pinList, GPIO.OUT, initial=GPIO.LOW)
APWM = GPIO.PWM(pins["ENA"], 50) # Frequency 100 cycles per second
BPWM = GPIO.PWM(pins["ENB"], 50) # Frequency 100 cycles per second
APWM.start(100) # Duty Cycle
BPWM.start(100)
# Straight Floor = 4.9
# 360 100-0 Diff Floor = 16
# Straight Carpet = 3.3
# 360 100-0 Diff Carpet = 12.5
try:
while True:
# GPIO.output(pins["IN1"],GPIO.HIGH)
# GPIO.output(pins["IN2"],GPIO.LOW)
# GPIO.output(pins["IN3"],GPIO.LOW)
# GPIO.output(pins["IN4"],GPIO.HIGH)
# time.sleep(1)
# stopCar()
# Lower duty cycle means more torque?
# APWM.ChangeDutyCycle(100)
# BPWM.ChangeDutyCycle(0)
# time.sleep(0.5) # Stop time after motion
# Differntial RTurn
GPIO.output(pins["IN1"], GPIO.HIGH)
GPIO.output(pins["IN2"], GPIO.LOW)
GPIO.output(pins["IN3"], GPIO.LOW)
GPIO.output(pins["IN4"], GPIO.HIGH)
# time.sleep(5)
print(time.clock())
# stopCar()
# time.sleep(0.5)
# GPIO.output(pins["IN1"],GPIO.LOW)
# GPIO.output(pins["IN2"],GPIO.LOW)
# GPIO.output(pins["IN3"],GPIO.HIGH)
# GPIO.output(pins["IN4"],GPIO.LOW)
# Right Turn
# GPIO.output(pins["IN1"],GPIO.LOW)
# GPIO.output(pins["IN2"],GPIO.HIGH)
# GPIO.output(pins["IN3"],GPIO.LOW)
# GPIO.output(pins["IN4"],GPIO.HIGH)
# Left turn
# GPIO.output(pins["IN1"],GPIO.HIGH)
# GPIO.output(pins["IN2"],GPIO.LOW)
# GPIO.output(pins["IN3"],GPIO.HIGH)
# GPIO.output(pins["IN4"],GPIO.LOW)
# time.sleep(3) # how long to hold the turn
stopCar()
# moveCar(60)
# time.sleep(7)
# break
# time.sleep(1)
# stopCar()
# time.sleep(1)
# getCarAngleTo(270, 180, 3)
# time.sleep(1)
finally:
print("Cleanup")
GPIO.cleanup()
def autonomousDriving(width, ROWS, mpQueue):
win = pygame.display.set_mode((SCREENWIDTH, SCREENWIDTH))
pygame.display.set_caption("Path Planning")
midCoords = MIDCOORDS
grid = populate_grid(ROWS, width)
# GPIO Setup
pinList = []
GPIO.setmode(GPIO.BOARD)
for key in pins.keys():
pinList.append(pins[key])
GPIO.setup(pinList, GPIO.OUT, initial=GPIO.LOW) # Init with motors stopped
APWM = GPIO.PWM(pins["ENA"], 50) # Frequency 50 cycles per second
BPWM = GPIO.PWM(pins["ENB"], 50)
APWM.start(100) # Duty Cycle
BPWM.start(100)
# Initial declarations
xList, yList = [], []
numDetections, loopIter, mainIter = 0, 0, 0
run, buttonClickedToStart, firstPassStartFindPath = True, False, True
experimentalZeroTurnTime = 12 # experimental
speed = 0.27 # 100% motor speed/duty cycle in meters/second
curAngle = 180 # default for the car pointing in the forward (left, -x) direction
timePerTile = 0.2
# Navigation goal
endRow, endCol = 1, 26
grid[midCoords][midCoords].make_start()
grid[endRow][endCol].make_end()
start = grid[midCoords][midCoords]
end = grid[endRow][endCol]
# Main functionality loop - car is now driving
while run:
if grid[midCoords][midCoords].get_state() == OBSTACLESTATE:
print("You crashed.")
grid[midCoords][midCoords].make_start()
draw(win, grid, ROWS, width)
for event in pygame.event.get():
if event.type == pygame.KEYDOWN: buttonClickedToStart = True
if event.type == pygame.QUIT: run = False
# Get detections from perception // interprocess communication
if buttonClickedToStart:
if not mpQueue.empty() or firstPassStartFindPath:
stopCar(pins)
while not mpQueue.empty():
detection = mpQueue.get(
) # new detections only (filtered in camera process)
print("In multiprocessing queue --> not empty!")
timePerTile = detection[3]
boundaryCreator(
detection[0], detection[1], detection[2], 0, win, grid,
ROWS, width
) # diameter, x, y, cleanup, win, grid, rows, width
for row in grid:
for node in row:
node.update_neighbors(grid)
draw(win, grid, ROWS, width)
xList, yList = [], []
xList, yList = algorithm(
lambda: draw(win, grid, ROWS, width), grid, start, end,
xList, yList) # row, col of each node in came_from
# Spline generation
x, y = np.array(xList), np.array(yList)
xe, ye = end.get_pos()
xs, ys = start.get_pos()
dist = math.sqrt(abs(xs - xe)**2 + abs(ys - ye)**2)
if dist < 15:
thresh = (int)(
ROWS * 0.15
) # too many points to try and map to the path with not much movement means choppy turns - so restrict it.
else:
thresh = (int)(ROWS * 0.25)
tck, u = interpolate.splprep([x, y], k=2,
s=0) # returns tuple and array
u = np.linspace(
0, 1, num=thresh, endpoint=True
) # num is the number of entries the spline will try and map onto the graph, if you have more rows, you want more points when interpolating to make the model better fit
out = interpolate.splev(
u, tck
) # 'out' is the interpolated array of new row,col positions out[0] = rows, out[1] = cols
print("Interpolated original array size: ", len(out[0]))
localChangeList = [
] # list of tuples, [0][0] is local X changes, [0][1] is local Y changes
for i in range(len(out[0]) - 1):
# out[0][0] is the last interpolated value (farthest from ego position)
# if out x val(rows) is decreasing: car is going right, call: movement(right) --> boundary moves left though
# Note, the order below will allow O(N) pop() later on for the first ego position.
rowDiff, colDiff = out[0][i] - out[0][
i + 1], out[1][i] - out[1][i + 1]
localChangeList.append((rowDiff, colDiff))
# visualize and clear lists containing path for next iteration
plt.figure()
plt.plot(y, x, 'ro', out[1], out[0], 'b')
plt.legend(['Points', 'Interpolated B-spline', 'True'],
loc='best')
plt.axis([min(y) - 1, max(y) + 1, max(x) + 1, min(x) - 1])
plt.title('Interpolated Planned Path')
plt.show(block=False)
plt.pause(1)
mainIter = len(out[0]) - 1
loopIter += 1
firstPassStartFindPath = False
else:
continue
"""
After running algorithm, you have the most up to date data about boundaries, path (smoothed),
all node status. If no new detection occurs, the while run loop will jump to here!
The incremental movements of the car happen in separate while loop iterations, the while run
happens after every incremental movement this means if we have a new detection we can change the path on the fly!
"""
iteration = len(
xList
) - 1 # xList[iteration] would give: the closest to egopose from came_from[]
if loopIter == 0: # algorithm hasn't run yet.
continue
elif len(
localChangeList
) == 0: # deplete localChangeList which is the spline point to point updates - may still be left overs in xList because of the shotcaller - nextAddition, so just go through the remaining
while iteration > 0:
# this part is just for the visual - filling in the remaining movement() for the end points and boundaries
nodeX, nodeY = xList[iteration], yList[iteration]
nextNodeX, nextNodeY = xList[iteration - 1], yList[iteration -
1]
if nextNodeX - nodeX > 0: dir = 'left'
elif nextNodeX - nodeX < 0: dir = 'right'
elif nextNodeY - nodeY > 0: dir = 'back'
elif nextNodeY - nodeY < 0: dir = 'straight'
else: dir = ''
end = movement(dir, end, win, grid, ROWS, width, 1)
print("end pos:", end.get_pos())
xList.pop()
yList.pop()
iteration -= 1
xList.pop()
yList.pop()
print("You made it!")
break
infoTuple = localChangeList[len(localChangeList) -
1] # gives closest one to the ego vehicle
rowDiffClosestToEgo = infoTuple[0]
colDiffClosestToEgo = infoTuple[1]
# computed for each subset of spline that is a subpath
distance = math.sqrt(rowDiffClosestToEgo**2 + colDiffClosestToEgo**2)
desiredAngle = (int)(
(math.atan2(colDiffClosestToEgo, rowDiffClosestToEgo) * 180 /
math.pi) +
360) % 360 # get angle between 0-360 instead of (-pi to pi)
nextAddition = abs(xList[iteration] - xList[iteration - 1]) + abs(
yList[iteration] - yList[iteration - 1]
) # difference between the two next came_from points
shotCaller = abs(rowDiffClosestToEgo) + abs(rowDiffClosestToEgo)
dir = ''
if abs(curAngle - desiredAngle) > 10:
# only then stop motors and readjust
print("Stop and get car to angle - big angle change", curAngle,
desiredAngle)
stopCar(pins)
time.sleep(0.2)
getCarAngleTo(
curAngle, desiredAngle, experimentalZeroTurnTime, pins
) # map difference to a time: (curCarAngle - desiredAngle) # have experimental time for full turn
# Else don't stop the motors, the trajectory is fine
# this is here because one straight line might be split into multiple splines
# update anyways to protect for incremental error
curAngle = desiredAngle
travelTime = distance * timePerTile # tiles*time/tile
print("TRAVELTIME", travelTime)
moveCar(pins)
timer = time.clock()
# shotCaller is a larger number than nextAddition, nextAdditions are smaller movements that make up shotCaller
while shotCaller - nextAddition > 0 and iteration > 0: # distance away from 0
# incase car goes over 'travelTime' inside loop.
if (time.clock() - timer) > travelTime:
stopCar(pins)
print("Stopped, caught in while loop", (time.clock() - timer))
shotCaller -= nextAddition
nodeX, nodeY = xList[iteration], yList[iteration]
nextNodeX, nextNodeY = xList[iteration - 1], yList[iteration - 1]
if nextNodeX - nodeX > 0: dir = 'left'
elif nextNodeX - nodeX < 0: dir = 'right'
elif nextNodeY - nodeY > 0: dir = 'back'
elif nextNodeY - nodeY < 0: dir = 'straight'
else: dir = ''
# algorithm is written such that came_from list will contain the immediately adjacent tile (meaning 1 tile move in 1 direction only)
end = movement(dir, end, win, grid, ROWS, width, 1)
print("End pos:", end.get_pos())
xList.pop()
yList.pop()
iteration = iteration - 1
if iteration > 0:
nextAddition = abs(
xList[iteration] - xList[iteration - 1]) + abs(
yList[iteration] -
yList[iteration - 1]) # always going to be 1
# time.sleep(0.5) # MAP TO VELOCITY OF CAR
print("Visualization Mapping Time:", time.clock() - timer)
while (time.clock() - timer) < travelTime:
print("Still Moving", time.clock() - timer)
continue
print("DIST JUST TRAVELLED:", distance, "@DESIRED ANGLE", desiredAngle,
"\n")
# Do this only everytime car moves one full distance - distance is between n number of spline points and so is out[0] gets done
mainIter -= 1
localChangeList.pop()
pygame.quit()
cv2.createTrackbar(
'hue2-low', 'Trackbars', 0, 179, nothing
) # need 6 trackbars. Low, high values for hue, saturation, and value.
cv2.createTrackbar(
'hue2-high', 'Trackbars', 14, 179,
nothing) # name, window to put in, initial value, max value, callback fn
cv2.createTrackbar('sat-low', 'Trackbars', 140, 255, nothing)
cv2.createTrackbar('sat-high', 'Trackbars', 255, 255, nothing)
cv2.createTrackbar('val-low', 'Trackbars', 111, 255, nothing)
cv2.createTrackbar('val-high', 'Trackbars', 255, 255, nothing)
GPIO.setmode(GPIO.BOARD)
GPIO.setup((32, 33), GPIO.OUT)
pwm_pan = GPIO.PWM(32, 50) # 50hz
pwm_tilt = GPIO.PWM(33, 50) # 50hz
pwm_pan.start(0)
pwm_tilt.start(0)
disp_w = 320 * 2
disp_h = 240 * 2
flip = 2 # a param on rpi cam. Else comes out upside down
pan = 0 # center
tilt = 0 # center
margin = 15
def set_angle(angle: int, pwm):
# Update Theta
def Theta_Update(theta_ik, theta_jk, learning_rate, reward_i, reward_j, phi_ikplus1, phi_ik, phi_jkplus1, phi_jk):
return theta_ik + (learning_rate * ((reward_i + reward_j) + ((np.matmul(np.transpose(phi_ikplus1), theta_ik)) + (np.matmul(np.transpose(phi_jkplus1), theta_jk))) - ((np.matmul(np.transpose(phi_ik), theta_ik)) + (np.matmul(np.transpose(phi_ik),theta_ik)))) * phi_ik) # Eq (30)
# Apply Action in Simulation
def Apply_Leader_Agent_Action(x_k, u, dt):
x_k1 = np.array([x_k[0] + dt * u, u]) # Integrator Model
return x_k1
### GPIO SETUP
GPIO.setmode(GPIO.BOARD)
# Motor Pin Setup
GPIO.setup(MOTOR_FORWARD_PWM, GPIO.OUT) # Set GPIO pin 32 to output mode.
pwm_forward_pin = GPIO.PWM(MOTOR_FORWARD_PWM, 100) # Initialize PWM on Forward Pin at 100Hz frequency
GPIO.setup(MOTOR_BACKWARD_PWM, GPIO.OUT) # Set GPIO pin 33 to output mode.
pwm_backward_pin = GPIO.PWM(MOTOR_BACKWARD_PWM, 100) # Initialize PWM on Backward Pin at 100Hz frequency
# Start PWM with 0% duty cycle
pwm_forward_pin.start(0)
pwm_backward_pin.start(0)
# Ultrasonic Sensor Pin Setup
GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
### XBEE SETUP
local_device = XBeeDevice("COM6", 9600) # Orange XBee
local_device.open()
remote_device = RemoteXBeeDevice(local_device, XBee64BitAddress.from_hex_string("0013A20041C80821")) # Blue XBee
GPIO.setup(GPIO_ECHO1, GPIO.IN)
GPIO.setup(GPIO_TRIGGER2, GPIO.OUT)
GPIO.setup(GPIO_ECHO2, GPIO.IN)
GPIO.setup(GPIO_TRIGGER3, GPIO.OUT)
GPIO.setup(GPIO_ECHO3, GPIO.IN)
GPIO.setup(GPIO_TRIGGER4, GPIO.OUT)
GPIO.setup(GPIO_ECHO4, GPIO.IN)
GPIO.setup(GPIO_TRIGGER5, GPIO.OUT)
GPIO.setup(GPIO_ECHO5, GPIO.IN)
GPIO.setup(GPIO_PWM, GPIO.OUT)
GPIO.setup(GPIO_IN1, GPIO.OUT)
GPIO.setup(GPIO_IN2, GPIO.OUT)
GPIO.setup(GPIO_SERVO, GPIO.OUT)
# PWM Setup for motor and the steering
pwm = GPIO.PWM(GPIO_PWM, 10000) # Initialize PWM on pwmPin 100Hz frequency
pwm_steer = GPIO.PWM(GPIO_SERVO, 50)
# main loop of program
# Print blank line before and after message.
dc = 0.1 # set dc variable to 0 for 0%
pwm.start(dc) # Start PWM with 0% duty cycle
pwm_steer.start(0)
def distance(GPIO_TRIGGER, GPIO_ECHO, sensor_number):
# set Trigger to HIGH
GPIO.output(GPIO_TRIGGER, True)
# set Trigger after 0.01ms to LOW
time.sleep(0.00001)
def setup(self):
global pwm
GPIO.setmode(GPIO.BOARD)
GPIO.setup(P_SERVO, GPIO.OUT)
pwm = GPIO.PWM(P_SERVO, fPWM)
pwm.start(0)
import Jetson.GPIO as GPIO
from time import sleep
en = 33
in1 = 35
in2 = 37
temp1=1
GPIO.setmode(GPIO.BOARD)
GPIO.setup(in1,GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(in2,GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(en,GPIO.OUT, initial=GPIO.LOW)
p=GPIO.PWM(en,1000)
p.start(25)
print("\n")
print("The default speed & direction of motor is LOW & Forward.....")
print("r-run s-stop f-forward b-backward l-low m-medium h-high e-exit")
print("\n")
while(1):
x=raw_input()
if x=='r':
print("run")
if(temp1==1):
GPIO.output(in1,GPIO.HIGH)
def main(width, ROWS, mpQueue):
time.sleep(0.5)
win = pygame.display.set_mode((SCREENWIDTH, SCREENWIDTH))
pygame.display.set_caption("Path Planning")
midCoords = MIDCOORDS # the middle block: ceil(NUMROWS/2) - 1 because 0th index
grid = populate_grid(ROWS, width)
endRow = 1
endCol = 26
pins = {
"ENA": 32, #PWM
"IN1": 35,
"IN2": 37,
"ENB": 33,
"IN3": 31,
"IN4": 29 #PWM
}
pinList = []
GPIO.setmode(GPIO.BOARD)
for key in pins.keys():
pinList.append(pins[key])
GPIO.setup(pinList, GPIO.OUT, initial=GPIO.LOW) #INIT WITH MOTORS STOPPED
APWM = GPIO.PWM(pins["ENA"], 50) # Frequency 100 cycles per second
BPWM = GPIO.PWM(pins["ENB"], 50)
APWM.start(100) # Duty Cycle
BPWM.start(100)
grid[midCoords][midCoords].make_start()
grid[endRow][endCol].make_end()
start = grid[midCoords][midCoords]
end = grid[endRow][endCol]
xList = []
yList = []
temp = []
for row in grid:
for node in row:
temp.append(node.state)
temp = []
# each detection is relative to the previous detection
# v = d/t --> get a set velocity and now you can always get the time to travel any distance (t = d/v)
# map the actual ruler distance to the first reference obstacle to the mapsize
# 60 inches obstacle, IRL maxsize = 100 -->
# distance to the reference car that we'll put at some specified (20,36) boundary coordinate
# EXAMPLE: Beginning reference car is x45, y5 inches away, we will compute x,y distance from start point 19,19
# to boundary hardcoded - selected by the user and will relate that to either x OR y. say 45x Inches =
# irlX, irlY, irlRadius, Size = input("ENTER HERE").split() # take as input, boundaries location, +-(x) +-(y) in meters, also the entire SQUARE that you are trying to fill 2meter by 2meter?
# irlX = float(irlX)
# irlY = float(irlY)
# irlRadius = float(irlRadius)
# Size = float(Size)
# TEST CASES:
# boundaryCreator(9, 11, 22, 0, win, grid, ROWS, width) #(diameter, row, col, cleanup, win, grid, rows, width)
# boundaryCreator(7, 3, 30, 0, win, grid, ROWS, width)
# Keep in mind: 'RADIUS' and MEASURED FROM THE MIDCOORDS --> 0.63 up (in the 180 degree direction) from the midcoords.
# Test case for 9,11,22
# irlX = 0.15
# irlY = 0.4
# irlRadius = 1
# Size = 9
# OG: zeroturntime = 7, speed = 0.6
# realObstacles = [(0.15, 0.4, 1, 9),(-0.45, 0.63, 1, 5), (0.55, 0.8,1,7)] # irlX, irlY, irlRadius, Size
# realObstacles = [(0.3, 0.8, 2, 9),(-0.9, 1.26, 2, 5), (1.1, 1.6,2,7)] # irlX, irlY, irlRadius, Size, use this
# Testing on red table
# realObstacles = [(0.3, 0.4, 0.5, 1, 5), (0.3, 0.2, 0.5, 1, 7)] # irlX, irlY, irlRadiusX, irlRadiusY, Size, use this
# for obstacle in realObstacles:
# realRatioX = obstacle[0]/obstacle[2] # irlX/irlRadiusX
# realRatioY = obstacle[1]/obstacle[3] # irlY/irlRadiusY --> Approximate row length of the longest sides of the actual human sized car grid.
# gridXEquivalent = abs((ROWS/2)*realRatioX) # how many tiles would equate to the location of other cars/boundaries
# gridYEquivalent = abs((ROWS/2)*realRatioY)
# if obstacle[0] < 0:
# boundCol = midCoords - gridXEquivalent # columns would decrease
# elif obstacle[0] > 0: # columns increase
# boundCol = midCoords + gridXEquivalent
# if obstacle[1] < 0:
# boundRow = midCoords + gridYEquivalent # rows would increase
# elif obstacle[1] > 0: # rows decrease
# boundRow = midCoords - gridYEquivalent
# boundaryCreator(obstacle[4], int(boundRow), int(boundCol), 0, win, grid, ROWS, width) #(diameter, x, y, cleanup, win, grid, rows, width)
# metersPerTile = abs(realObstacles[len(realObstacles)-1][0])/gridXEquivalent # abs(irlX)/gridXEquivalent, irlX of last boundary because gridXEquivalent will be of the last boundary
# timePerMeter = 1/speed
# timePerTile = timePerMeter*metersPerTile
# print("TIME PER TILE", timePerTile)
# print("METERS PER TILE", metersPerTile)
# print("TIME PER METER", timePerMeter)
numDetections = 0
run = True
loopIter = 0
mainIter = 0
buttonClickedToStart = False
firstPassStartFindPath = True
experimentalZeroTurnTime = 12 # experimental
speed = 0.27 # 100% motor speed/duty cycle in meters/second
curAngle = 180 # default for the car pointing in the forward (left, -x) direction
timePerTile = 0.2
while run:
if grid[midCoords][midCoords].get_state() == OBSTACLESTATE:
print("YOU CRASHED.")
grid[midCoords][midCoords].make_start()
draw(win, grid, ROWS, width)
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
buttonClickedToStart = True
if event.type == pygame.QUIT:
run = False
# Get detections from perception
if buttonClickedToStart:
if not mpQueue.empty() or firstPassStartFindPath:
stopCar(pins)
while not mpQueue.empty():
detection = mpQueue.get(
) # only new detections (filtered in camera process)
print("IN MULTIPROCESSING QUEUE MEANING NOT EMPTY!")
timePerTile = detection[3]
boundaryCreator(
detection[0], detection[1], detection[2], 0, win, grid,
ROWS, width
) #(diameter, x, y, cleanup, win, grid, rows, width)
print("LOOPED DETECTIONS IN mpQueue.get()")
# for event in pygame.event.get():
# if event.type == pygame.QUIT:
# run = False
# if event.type == pygame.KEYDOWN: # https://www.programcreek.com/python/example/5891/pygame.K_LEFT
# if event.key == pygame.K_UP:
# boundaryCreator(3, 11, 15, 0, win, grid, ROWS, width) #(diameter, x, y, cleanup, win, grid, rows, width)
# print("TRYING TO CREATE BOUNDARY")
# if event.key == pygame.K_LEFT:
# end = movement('straight', end, win, grid, ROWS, width, 1)
# if event.key == pygame.K_DOWN:
# end = movement('left', end, win, grid, ROWS, width, 1)
# if event.key == pygame.K_RIGHT:
# end = movement('back', end, win, grid, ROWS, width, 1)
for row in grid:
for node in row:
node.update_neighbors(grid)
draw(win, grid, ROWS, width)
xList = []
yList = []
# row, col of each node in came_from
xList, yList = algorithm(lambda: draw(win, grid, ROWS, width),
grid, start, end, xList, yList)
# if event.key == pygame.K_c:
# grid = populate_grid(ROWS, width)
# ONLY IF ALGORITHM RUNS --> MEANING THERE WAS A NEW BOUNDARY DETECTION: RUN SPLINE SMOOTHED INTERPOLATED PATH
x = np.array(xList)
y = np.array(yList)
xe, ye = end.get_pos()
xs, ys = start.get_pos()
dist = math.sqrt(abs(xs - xe)**2 + abs(ys - ye)**2)
if dist < 15:
thresh = (int)(
ROWS * 0.15
) # too many points to try and map to the path with not much movement means choppy turns - so restrict it.
else:
thresh = (int)(ROWS * 0.25)
tck, u = interpolate.splprep([x, y], k=2,
s=0) # returns tuple and array
u = np.linspace(
0, 1, num=thresh, endpoint=True
) # num is the number of entries the spline will try and map onto the graph, if you have more rows, you want more points when interpolating to make the model better fit
out = interpolate.splev(
u, tck
) # 'out' is the interpolated array of new row,col positions out[0] = rows, out[1] = cols
print("INTERPOLATED ORIGINAL ARRAY SIZE: ", len(out[0]))
localChangeList = [
] # list of tuples, [0][0] is local X changes, [0][1] is local Y changes
for i in range(len(out[0]) - 1):
# out[0][0] is the last interpolated value (farthest from ego position)
# if out x val(rows) is decreasing: car is going right, call: movement(right) --> boundary moves left though
rowDiff = out[0][i] - out[0][
i +
1] # Note, this order will allow O(N) pop() later on for the first ego position.
colDiff = out[1][i] - out[1][i + 1]
localChangeList.append((rowDiff, colDiff))
# visualize and clear lists containing path for next iteration
plt.figure()
plt.plot(y, x, 'ro', out[1], out[0], 'b')
plt.legend(['Points', 'Interpolated B-spline', 'True'],
loc='best')
plt.axis([min(y) - 1, max(y) + 1, max(x) + 1, min(x) - 1])
plt.title('Interpolated Planned Path')
plt.show(block=False)
plt.pause(1)
mainIter = len(out[0]) - 1
loopIter += 1
firstPassStartFindPath = False
else:
# print("Press any button to start autonomous driving")
continue
# AFTER RUNNING ALGORITHM, YOU HAVE THE MOST UP TO DATE DATA ABOUT BOUNDARIES, PATH (SMOOTHED), ALL NODE STATUS
# IF NO NEW DETECTION OCCURS, THE WHILE RUN LOOP WILL JUMP TO HERE!
# NOTE THE INCREMENTAL MOVEMENTS OF THE CAR HAPPEN IN SEPARATE WHILE LOOP ITERATIONS, THE WHILE RUN HAPPENS AFTER EVERY INCREMENTAL MOVEMENT
# THIS MEANS IF WE HAVE A NEW DETECTION WE CAN CHANGE THE PATH ON THE FLY!
iteration = len(
xList
) - 1 # xList[iteration] would give: the closest to egopose from came_from[]
if loopIter == 0: # algorithm hasn't run yet.
continue
elif len(
localChangeList
) == 0: # deplete localChangeList which is the spline point to point updates - may still be left overs in xList because of the shotcaller - nextAddition, so just go through the remaining
while iteration > 0:
# this part is just for the visual - filling in the remaining movement() for the end points and boundaries
nodeX, nodeY = xList[iteration], yList[iteration]
nextNodeX, nextNodeY = xList[iteration - 1], yList[iteration -
1]
if nextNodeX - nodeX > 0:
dir = 'left'
elif nextNodeX - nodeX < 0:
dir = 'right'
elif nextNodeY - nodeY > 0:
dir = 'back'
elif nextNodeY - nodeY < 0:
dir = 'straight'
else:
dir = ''
end = movement(dir, end, win, grid, ROWS, width, 1)
print("end pos:", end.get_pos())
xList.pop()
yList.pop()
iteration -= 1
xList.pop()
yList.pop()
print("You made it!")
break
# need to see how much is overlapped from xList and yList to the movement happening in spline.
# max of rowDiff and colDiff takes precedence - shot caller
# see how far max of ^ would take us in the xList,yList (camefrom subset) and just pop() until you get there.
# ex. max()--> 2.392, if max was col --> look in yList, else look in xList, pop() and subtract till max is almost reached
# if you subtract and pop one more time, will it be a greater distance from 0 than if you left it? --> if yes, leave it--> that's where end is.
# xList[0] has the row position of start tile
# difference between consecutive xList vals
# goal: does shotcaller benefit from adding another xList val or does it get further awawy from 0?
# infoTuple indexing: X,Y,MAG,ANGLE
infoTuple = localChangeList[len(localChangeList) -
1] # gives closest one to the ego vehicle
# for efficiency, do the computations here because you might not need the whole list if algorithm runs again
rowDiffClosestToEgo = infoTuple[0]
colDiffClosestToEgo = infoTuple[1]
# computed for each subset of spline that is a subpath
distance = math.sqrt(rowDiffClosestToEgo**2 + colDiffClosestToEgo**2)
desiredAngle = (int)(
(math.atan2(colDiffClosestToEgo, rowDiffClosestToEgo) * 180 /
math.pi) +
360) % 360 # get angle between 0-360 instead of (-pi to pi)
nextAddition = abs(xList[iteration] - xList[iteration - 1]) + abs(
yList[iteration] - yList[iteration - 1]
) # difference between the two next came_from points
dir = ''
shotCaller = abs(rowDiffClosestToEgo) + abs(rowDiffClosestToEgo)
if abs(curAngle - desiredAngle) > 10:
# only then stop motors and readjust
print("STOP AND GET CAR TO ANGLE - BIG ANGLE CHANGE", curAngle,
desiredAngle)
stopCar(pins)
time.sleep(0.2)
getCarAngleTo(
curAngle, desiredAngle, experimentalZeroTurnTime, pins
) # map difference to a time: (curCarAngle - desiredAngle) # have experimental time for full turn
# Else don't stop the motors, the trajectory is fine - this is here because one straighht line might be split into multiple splines
curAngle = desiredAngle # update anyways to protect for incremental error
travelTime = distance * timePerTile # tiles*time/tile
print("TRAVELTIME", travelTime)
moveCar(pins)
timer = time.clock()
# shotCaller is a larger number than nextAddition, nextAdditions are smaller movements that make up shotCaller
while shotCaller - nextAddition > 0 and iteration > 0: # distance away from 0
# incase car goes over 'travelTime' inside loop.
if (time.clock() - timer) > travelTime:
stopCar(pins)
print("STOPPED CAUGHT IN WHILE LOOP", (time.clock() - timer))
# don't count this part as taking much time - just set teh motors going in desired direction and time.sleep(calcTime) after.
shotCaller -= nextAddition
nodeX, nodeY = xList[iteration], yList[iteration]
nextNodeX, nextNodeY = xList[iteration - 1], yList[iteration - 1]
if nextNodeX - nodeX > 0:
dir = 'left'
elif nextNodeX - nodeX < 0:
dir = 'right'
elif nextNodeY - nodeY > 0:
dir = 'back'
elif nextNodeY - nodeY < 0:
dir = 'straight'
else:
dir = ''
# the way the algorithm is written, the came_from list will contain the immediately adjacent tile (meaning 1 tile move in 1 direction only)
end = movement(dir, end, win, grid, ROWS, width, 1)
print("End pos:", end.get_pos())
xList.pop()
yList.pop()
iteration = iteration - 1
if iteration > 0:
nextAddition = abs(
xList[iteration] - xList[iteration - 1]) + abs(
yList[iteration] -
yList[iteration - 1]) #always going to be 1
# time.sleep(0.5) # MAP TO VELOCITY OF CAR
print("Visualization Mapping Time:", time.clock() - timer)
while (time.clock() - timer) < travelTime:
# print("Still Moving", time.clock()-timer)
continue
print("DIST JUST TRAVELLED:", distance, "@DESIRED ANGLE", desiredAngle,
"\n")
# Do this only everytime car moves one full distance - distance is between n number of spline points and so is out[0] gets done
mainIter -= 1
localChangeList.pop()
# finally:
# print("CLEANUP GPIO")
# GPIO.cleanup()
pygame.quit()
import Jetson.GPIO as GPIO
import time
import os
import drone_options as drone
GPIO.setwarnings(False)
# Prime GPIO to read input.
GPIO.setmode(GPIO.BOARD)
GPIO.setup(33, GPIO.OUT)
GPIO.setup(32, GPIO.OUT)
# Instantiate PWM variables.
color_pwm = GPIO.PWM(32, 50) # PWM3
record_pwm = GPIO.PWM(33, 50) # PWM4
## Initialize color depending on value from text file ##
current = drone.read_file()
mode_value = drone.mode_dict.get(drone.get_mode_value(current))
if mode_value == 1:
color_pwm.start(5) # White Hot View
if mode_value == 2:
color_pwm.start(7.5) # Green Hot View
if mode_value == 3:
color_pwm.start(10) # Fusion View
else:
color_pwm.start(0) # Don't Care
# Start Recording
record_pwm.start(10)
