def task2():
wheels = kin.getPdCurrent()
wheelL = wheels[0] # left phi dot value from getPdCurrent
wheelR = wheels[1] # rigth phi dot value from getPDCurrent
frames = kin.getMotion()
speed = frames[0] # x dot value from getMotion
angle = frames[1] # theta dot value from getMotion
# myAxes = head.scale()
# nswe = head.getHeading()
# xAxes = nswe[0]
# yAxes = nswe[1]
axes = head.getXY()
axesScaled = head.scale(axes) # perform scale function
h = head.getHeading(axesScaled) # compute the heading
headingDegrees = round(h * 180 / np.pi, 2)
log.uniqueFile(wheelR, "phidotR.txt")
log.uniqueFile(wheelL, "phidotL.txt")
print("Left Wheel, Right Wheel: \t", wheelL, wheelR, "\t",
"x dot, theta dot \t", speed, angle, "\t", "Heading: \t",
headingDegrees)
log.uniqueFile(speed, "xdot.txt")
log.uniqueFile(angle, "thetadot.txt")
log.uniqueFile(headingDegrees, "heading.txt")
def task():
a = kin.getPdCurrent() #function returns both phi-dot values from motor
b = kin.getMotion(
) #function returns x-dot and theta-dot values from motor
axes = head.getXY() #return an average heading value for x and y
scaledaxes = head.scale(axes) #scale axes based on np array
h = head.getHeading(scaledaxes) #returns a radian value for the heading
hDegrees = round(h * 180 / np.pi,
2) #converts the heading value into degrees
print('Pdr and Pdl values are:') #prints out values for Pdr and Pdl
print(a)
print('motion values are:') #prints out values for x-dot and theta-dot
print(b)
print('heading is:') #print out the heading value in degrees
print(hDegrees)
log.uniqueFile(a[0],
"task2pdl.txt") #use unique file to save pdl to a txt file
#rather than a temporary folder that would be
#created with tempFile
log.uniqueFile(a[1], "task2pdr.txt") #save unique file for pdr
log.uniqueFile(b[0], "task2xDot.txt") #save unique file for x-dot
log.uniqueFile(b[1], "task2thetaDot.txt") #save unique file for theta-dot
log.uniqueFile(hDegrees,
"heading.txt") #save unique file for heading in degrees
def convertHeading():
axes = getXY() # call xy function
# print("raw values:", axes)
axesScaled = scale(axes) # perform scale function
# print("scaled values:", axesScaled) # print it out
h = getHeading(axesScaled) # compute the heading
headingDegrees = round(h * 180 / np.pi, 2)
print("heading:", headingDegrees)
time.sleep(0.25) # delay 0.25 sec
log.tmpFile(headingDegrees, "headingDegrees.txt")
return headingDegrees
def Lab4task2():
#do some items here
pdees = kin.getPdCurrent()
pdl = pdees[0] #gets pdl
pdr = pdees[1] #gets pdr
move = kin.getMotion()
xdot = move[0] #gets xdot
thetadot = move[1] #gets thetadot
log.tmpFile(pdl, "pdl.txt")
log.tmpFile(pdr, "pdr.txt")
log.tmpFile(xdot, "xdot.txt")
log.tmpFile(thetadot, "thetadot.txt")
def ball_look():
near_vector = bs.get_nearest() #return vector with the smallest distance [d,theta]
min_distance = round(near_vector[0],3)
# print(min_distance)
if min_distance <= 0.365:
min_angle = round(near_vector[1],3)
#print("ball found")
log.uniqueFile_2("Autonomous Mode","state.txt")
global count
count = count + 1
log.uniqueFile(count,"count.txt")
if min_angle < -15 or min_angle > 15 :
autonomous_nav(min_angle)
else:
vac_ON()
motor.MotorL(-.8)
motor.MotorR(-.8)
time.sleep(2)
vac_OFF()
elif min_distance > 0.37:
motor.MotorL(0)#need to be taken out
motor.MotorR(0)
#print("no ball")
log.uniqueFile_2("Manual Mode", "state.txt")
pass
def task2():
wheels = kin.getPdCurrent()
wheelL = wheels[0] # left phi dot value from getPdCurrent
wheelR = wheels[1] # rigth phi dot value from getPDCurrent
frames = kin.getMotion()
speed = frames[0] # x dot value from getMotion
angle = frames[1] # theta dot value from getMotion
log.uniqueFile(wheelR, "phidotR.txt")
log.uniqueFile(wheelL, "phidotL.txt")
print("Left Wheel, Right Wheel: \t", wheelL, wheelR, "x dot, theta dot \t",
speed, angle)
log.uniqueFile(speed, "xdot.txt")
log.uniqueFile(angle, "thetadot.txt")
def task2():
pd = kin.getPdCurrent() # returns [pdl, pdr] in radians/second
print("pd = ", pd)
pdl = pd[0]
pdr = pd[1]
frame = kin.getMotion() # returns a matrix containing [xDot, thetaDot]
print("frame = ", frame)
framex = frame[0]
frametheta = frame[1]
log.tmpFile(pdl, "pdl.txt")
log.tmpFile(pdr, "pdr.txt")
log.tmpFile(framex, "framex.txt")
log.tmpFile(frametheta, "frametheta.txt")
def task():
a = kin.getPdCurrent() #function returns both phi-dot values from motor
b = kin.getMotion(
) #function returns x-dot and theta-dot values from motor
print('Pdr and Pdl values are:') #prints out values for Pdr and Pdl
print(a)
print('motion values are:') #prints out values for x-dot and theta-dot
print(b)
log.uniqueFile(a[0],
"task2pdl.txt") #use unique file to save pdl to a txt file
#rather than a temporary folder that would be
#created with tempFile
log.uniqueFile(a[1], "task2pdr.txt") #save unique file for pdr
log.uniqueFile(b[0], "task2xDot.txt") #save unique file for x-dot
log.uniqueFile(b[1], "task2thetaDot.txt") #save unique file for theta-dot
def task2():
vect = veclab.getNearest()
scan = veclab.lidar.polarScan() # get a reading in meters and degrees
valids = veclab.getValid(scan) # remove the bad readings
vec = veclab.nearest(valids)
dist = vec[0] # left phi dot value from getPdCurrent
ang = vec[1] # rigth phi dot value from getPDCurrent
dy = (dist * np.sin(ang))
dx = (dist * np.cos(ang))
log.uniqueFile(dist, "distance.txt")
log.uniqueFile(ang, "angle.txt")
log.uniqueFile(dx, "dx.txt")
log.uniqueFile(dy, "dy.txt")
print("Distance [m]\t", dist, "\t", "Angle [deg]\t", ang, "\t", "dx [m]\t",
dx, "\t", "dy [m]\t", dy)
def main():
while 1:
mqtt = read_mqtt()
if mqtt == "1":
log.uniqueFile_2("Galileo ON", "on_off.txt")
ball_look()
manual_nav()
elif mqtt == "0":
log.uniqueFile_2("Galileo OFF", "on_off.txt")
log.uniqueFile_2(" ", "state.txt")
time.sleep(1)
pass
def task():
axes = heading.getXY()
axesScaled = heading.scale(axes)
print(axesScaled)
h = heading.getHeading(axesScaled)
headingDegrees = round(h * 180 / np.pi,
2) #converting raw data into degrees
print("heading: ", headingDegrees) #printing value in terminal
log.uniqueFile(headingDegrees,
"headingDegrees.txt") #printing values in nodered
log.uniqueFile(axesScaled[0], "scaledX.txt") #printing values in nodered
log.uniqueFile(axesScaled[1], "scaledY.txt") #printing values in nodered
def loop_drive():
count = 0 # number of loop iterations
# INITIALIZE VARIABLES FOR CONTROL SYSTEM
t0 = 0 # time sample
t1 = 1 # time sample
e00 = 0 # error sample
e0 = 0 # error sample
e1 = 0 # error sample
dt = 0 # delta in time
de_dt = np.zeros(2) # initialize the de_dt
while (1):
count += 1 #count the number of times this loop has run
# THIS CODE IS FOR OPEN AND CLOSED LOOP control
pdTargets = np.array([9.7, 9.7]) # Input requested PhiDots (radians/s)
#pdTargets = inv.getPdTargets() # populates target phi dots from GamePad
kin.getPdCurrent() # capture latest phi dots & update global var
pdCurrents = kin.pdCurrents # assign the global variable value to a local var
# THIS BLOCK UPDATES VARIABLES FOR THE DERIVATIVE CONTROL
t0 = t1 # assign t0
t1 = time.time() # generate current time
dt = t1 - t0 # calculate dt
e00 = e0 # assign previous previous error
e0 = e1 # assign previous error
e1 = pdCurrents - pdTargets # calculate the latest error
de_dt = (e1 - e0) / dt # calculate derivative of error
# CALLS THE CONTROL SYSTEM TO ACTION
#sc.driveOpenLoop(pdTargets) # call on open loop
sc.driveClosedLoop(pdTargets, pdCurrents, de_dt) # call on closed loop
time.sleep(0.05) # this time controls the frequency of the controller
# u = sc.u # calculate the control effort
# u_proportional = sc.u_proportional
# u_integral = sc.u_integral
# u_derivative = sc.u_derivative
# THIS BLOCK OUTPUTS DATA TO A CSV FILE
if count == 1: # check if this is the first iteration of the loop
log.clear_file() # clear old contents from the csv file
log.csv_write([count, pdCurrents[0],
pdTargets[0]]) # log this iteration
elif count > 1 and count <= 400: # only log 400 samples and then quit logging
log.csv_write([count, pdCurrents[0],
pdTargets[0]]) # log this iteration
import L2_log as log
import L2_vector as vec
import time
while (1):
nearest = vec.getNearest()
log.tmpFile(nearest[0], 'distance')
log.tmpFile(nearest[1], 'angle')
nearestxy = vec.polar2cart(nearest[0], nearest[1])
log.tmpFile(nearestxy[0], 'x')
log.tmpFile(nearestxy[1], 'y')
time.sleep(0.2)
if -80 < headingDegrees: #Cardnal direction northeast
if headingDegrees < -10:
(Cardnal) = "Northeast"
if 100 < headingDegrees: #Cardnal direction southwest
if headingDegrees < 170:
(Cardnal) = "Southwest"
if -170 < headingDegrees: #Cardnal direction southeast
if headingDegrees < -100:
(Cardnal) = "Southeast"
# print("x axis:", xAccel, "y axis:", yAccel,"z axis:", zAccel)
# print("Barrel Voltage:",Volt, "Temperature:", TC, "Pressure:", Press, "Altitude:", alt) # print the two values
print("Heading Degree:", headingDegrees, "Direction:", str(Cardnal))
#axes = np.array([xAccel, yAccel) # store just 2 axes in an array
#log.NodeRed2(axes) # send the data to txt files for NodeRed to access.
# log.uniqueFile(xAccel,"xAccel.txt") # another way to log data for NodeRed access
# log.uniqueFile(yAccel,"yAccel.txt")
# log.uniqueFile(zAccel,"zAccel.txt")
# log.uniqueFile(Volt,"BarrelVolt.txt")
# log.uniqueFile(TC,"Temp.txt")
# log.uniqueFile(Press,"Pressure.txt")
# log.uniqueFile(alt,"Altitude.txt")
log.uniqueFile(headingDegrees, "HeadingDeg.txt")
log.stringTmpFile(Cardnal, "Direction.txt")
# log.tmpFile(xAccel,"xAccel.txt") # another way to lof data for NodeRed access
# log.tmpFile(yAccel,"yAccel.txt")
time.sleep(0.2)
print("Voltage is:", dcvoltage)
#log.uniqueFile(dcvoltage,"voltage.txt") ## use this function for the lab activity
accel = mpu.getAccel() # call the function from within L1_mpu.py
(xAccel) = accel[0] # x axis is stored in the first element
(yAccel) = accel[1] # y axis is stored in the second element
(zAccel) = accel[2]
print("x axis:", xAccel, "y axis:", yAccel, "z axis",
zAccel) # print the two values
axes = np.array([xAccel, yAccel, zAccel]) # store just 2 axes in an array
print(axes)
Temp = bmp.temp()
Pres = bmp.pressure()
Alt = bmp.altitude()
print("temperature is:", Temp)
print("altitude is:", Alt)
print("pressure is:", Pres)
log.uniqueFile(xAccel, "xAccel.txt")
log.uniqueFile(yAccel, "yAccel.txt")
log.uniqueFile(zAccel, "zAccel.txt")
log.uniqueFile(dcvoltage, "volt.txt")
log.uniqueFile(Temp, "temp.txt")
log.uniqueFile(Alt, "Alt.txt")
log.uniqueFile(Pres, "Pres.txt")
# send the data to txt files for NodeRed to access.
# log.uniqueFile(xAccel,"xAccel.txt") # another way to log data for NodeRed access
# log.uniqueFile(yAccel,"yAccel.txt")
# log.tmpFile(xAccel,"xAccel.txt") # another way to lof data for NodeRed access
# log.tmpFile(yAccel,"yAccel.txt")
time.sleep(0.2)
def trackingDrive():
width = 240 # Resized image width. This is the image width in pixels.
size_h = 160 # Resized image height. This is the image height in pixels.
tc = 40 # Too Close - Maximum pixel size of object to track
tf = 15 # Too Far - Minimum pixel size of object to track
tp = 25 # Target Pixels - Target size of object to track
band = 25 #range of x considered to be centered
x = 0 # will describe target location left to right
y = 0 # will describe target location bottom to top
radius = 0 # estimates the radius of the detected target
duty_l = 0 # initialize motor with zero duty cycle
duty_r = 0 # initialize motor with zero duty cycle
print("initializing rcpy...")
rcpy.set_state(rcpy.RUNNING) # initialize rcpy
print("finished initializing rcpy.")
try:
while rcpy.get_state() != rcpy.EXITING:
if rcpy.get_state() == rcpy.RUNNING:
scale_t = 1 # a scaling factor for speeds
scale_d = 1.3 # a scaling factor for speeds
motor_r = 2 # Right Motor assigned to #2
motor_l = 1 # Left Motor assigned to #1
x, y, radius = trackTarget.colorTarget(
color_range) # Get properties of circle around shape
if x != None: # If more than 0 closed shapes exist
# handle centered condition
if x > ((width / 2) - (band / 2)) and x < (
(width / 2) +
(band / 2)): # If center point is centered
dir = "driving"
if (radius - tp) >= 1: # Too Close
case = "too close"
duty = -1 * ((radius - tp) / (tc - tp))
dutyF = 0
elif (radius - tp) < -1: # Too Far
case = "too far"
duty = 1 - ((radius - tf) / (tp - tf))
duty = scale_d * duty
dutyF = 0
else:
case = "good"
duty = 0
dutyF = -0.5
duty_r = duty
duty_l = duty
else:
case = "turning"
dutyF = 0
duty_l = round((x - 0.5 * width) / (0.5 * width),
2) # Duty Left
duty_l = duty_l * scale_t
duty_r = round((0.5 * width - x) / (0.5 * width),
2) # Duty Right
duty_r = duty_r * scale_t
# Keep duty cycle within range
if duty_r > 1:
duty_r = 1
elif duty_r < -1:
duty_r = -1
if duty_l > 1:
duty_l = 1
elif duty_l < -1:
duty_l = -1
if duty_r < .3 and duty_r > 0:
duty_r = .3
elif duty_r < 0 and duty_r > -.3:
duty_r = -.3
if duty_l < .3 and duty_l > 0:
duty_l = .3
elif duty_l < 0 and duty_l > -0.3:
duty_l = -0.3
# Round duty cycles
duty_l = round(duty_l, 2)
duty_r = round(duty_r, 2)
print(case, "\tradius: ", round(radius, 1), "\tx: ",
round(x, 0), "\t\tL: ", duty_l, "\tR: ", duty_r)
# Set motor duty cycles
motor.set(motor_l, duty_l)
motor.set(motor_r, duty_r)
motor.set(3, dutyF)
log.stringTmpFile(str(duty_l), "leftMotor.txt")
log.stringTmpFile(str(duty_r), "rightMotor.txt")
log.stringTmpFile(str(radius),
"radiusOfComputerVision.txt")
log.stringTmpFile(str(x), "xValue.txt")
import numpy as np # for handling numpy arrays
import time # for timekeeping
# Import internal programs
import L2_vector as vector
import L2_log as log
tRefresh = 0.120 # target refresh time for the loop (sec)
# Start the program
print("Running Lab8Template.py")
while (1):
t0 = time.time() # sample present time
p2 = vector.getNearest(
) # store the nearest obstacle as point 2 [meters, degrees]
log.tmpFile(p2[0],
"p2_distance.txt") # log the distance for nodeRed pickup
log.tmpFile(np.round(p2[1], 1),
"p2_offset.txt") # log the angle for nodeRed pickup
t1 = time.time() # sample present time
tSweep = t1 - t0 # output sec
if tSweep <= tRefresh: # add a delay if the sweep was less than desired refresh speed
time.sleep(
tRefresh - tSweep
) # Run your loop faster than the NodeRed sampling but slower than 15hz
tSweep = round(tSweep * 1000, 0) # output ms
t2 = time.time() # sample current time
tLoop = round(1000 * (t2 - t0), 0) # output ms
print("SweepTime (ms):", tSweep, "\t", "loopTime (ms):",
tLoop) # print the execution times.
def main():
camera = cv2.VideoCapture(camera_input) # Define camera variable
camera.set(3, size_w) # Set width of images that will be retrived from camera
camera.set(4, size_h) # Set height of images that will be retrived from camera
tc = 300 # Too Close - Maximum pixel size of object to track
tf = 6 # Too Far - Minimum pixel size of object to track
tp = 200 # Target Pixels - Target size of object to track
band = 130 #range of x considered to be centered
x = 0 # will describe target location left to right
y = 0 # will describe target location bottom to top
radius = 0 # estimates the radius of the detected target
duty_l = 0 # initialize motor with zero duty cycle
duty_r = 0 # initialize motor with zero duty cycle
maxCount = 40
currentCount = 0
d = .6
reached = False
print("initializing rcpy...")
rcpy.set_state(rcpy.RUNNING) # initialize rcpy
print("finished initializing rcpy.")
f = 0
InitFiles()
delta_x = 0
delta_y = 0
x_dot = 0
try:
while rcpy.get_state() != rcpy.EXITING:
t = time.time()
r = 0
if rcpy.get_state() == rcpy.RUNNING:
scale_t = .3 # a scaling factor for speeds
scale_d = 1 # a scaling factor for speeds
motor_r = 2 # Right Motor assigned to #2
motor_l = 1 # Left Motor assigned to #1
direction = 0
found = False
#image = cv2.imread("image2.bmp")
ret, image = camera.read() # Get image from camera
cv2.imwrite('image2.jpg',image)
height, width, channels = image.shape # Get size of image
#if not ret:
# break
if filter == 'RGB': # If image mode is RGB switch to RGB mode
frame_to_thresh = image.copy()
else:
frame_to_thresh = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # Otherwise continue reading in HSV
thresh = cv2.inRange(frame_to_thresh, (v1_min, v2_min, v3_min), (v1_max, v2_max, v3_max)) # Find all pixels in color range
time.sleep(0.02)
cv2.imwrite('thresh.jpg',thresh)
kernel = np.ones((5,5),np.uint8) # Set gaussian blur strength.
mask = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel) # Apply gaussian blur
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2] # Find closed shapes in image
center = None # Create variable to store point
if len(cnts) > 0: # If more than 0 closed shapes exist
c = max(cnts, key=cv2.contourArea) # Get the properties of the largest circle
((x, y), radius) = cv2.minEnclosingCircle(c) # Get properties of circle around shape
radius = round(radius, 2) # Round radius value to 2 decimals
r = radius
x = int(x) # Cast x value to an integer
M = cv2.moments(c) # Gets area of circle contour
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) # Get center x,y value of circle
h = heading.getDegrees()
# handle centered condition
if x > ((width/2)-(band/2)) and x < ((width/2)+(band/2)): # If center point is centered
dir = "driving"
found = True
if abs(radius - tp)<10:
reached = True
servo.move1(1)
duty = 0
elif radius < tp: # Too Far
case = "too far"
servo.move1(-1)
duty = 1 - ((radius - tf)/(tp - tf))
duty = scale_d * duty
reached = False
duty_r = duty
duty_l = duty
else:
reached = False
case = "turning"
duty_l = round((x-0.5*width)/(0.5*width),2) # Duty Left
duty_l *= scale_t
duty_r = round((0.5*width-x)/(0.5*width),2) # Duty Right
duty_r *= scale_t
limit = 0.3
if(abs(duty_l)<limit):
duty_l*= limit/duty_l
if(abs(duty_r)<limit):
duty_r*= limit/duty_r
file = open("radius.txt","w")
file.write(str(reached)+","+ str(r)+"\n")
file.close()
# Keep duty cycle within range
if duty_r > 1:
duty_r = 1
elif duty_r < -1:
duty_r = -1
if duty_l > 1:
duty_l = 1
elif duty_l < -1:
duty_l = -1
# Round duty cycles
duty_l = round(duty_l,2)
duty_r = round(duty_r,2)
direction = duty_r - duty_l
case+= " "+str(direction)
print(x_dot," ",case, "\tradius: ", round(radius,1), "\tx: ", round(x,0), "\t\tL: ", duty_l, "\tR: ", duty_r)
# Set motor duty cycles
motor.set(motor_l, duty_l)
motor.set(motor_r, duty_r)
else:
direction = duty_r - duty_l
print(currentCount, " ", direction)
if(currentCount <maxCount):
duty_l = d * scale_t
duty_r = -d * scale_t
currentCount += 1
else:
d*= -1
currentCount = 0
file = open("data.txt","a")
file.write(str(duty_r) +","+ str(duty_l)+"\n")
file.close()
# Set motor duty cycles
motor.set(motor_l, duty_l)
motor.set(motor_r, duty_r)
tend = time.time()
del_t = tend - t
length = x_dot*del_t
delta_x += math.cos(h) * length
delta_y += math.sin(h) * length
log.uniqueFile(tend - t, "time.txt")
x_dot = speed([duty_l, duty_r])[0]
elif rcpy.get_state() == rcpy.PAUSED:
pass
import L2_log as log
# Import External programs
import numpy as np
import time
# Run the main loop
while True:
accel = mpu.getAccel() # call the function from within L1_mpu.py
(xAccel) = accel[0] # x axis is stored in the first element
(yAccel) = accel[1] # y axis is stored in the second element
(zAccel) = accel[2] # z axis is stored in the third element
print("x axis:", xAccel, "y axis:", yAccel, "z axis:",
zAccel) # print the two values
axes = np.array([xAccel, yAccel, zAccel]) # store just 2 axes in an array
log.NodeRed3(axes) # send the data to txt files for NodeRed to access.
log.uniqueFile(xAccel,
"xAccel.txt") # another way to log data for NodeRed access
log.uniqueFile(yAccel, "yAccel.txt")
log.uniqueFile(zAccel, "zAccel.txt")
# log.tmpFile(xAccel,"xAccel.txt") # another way to lof data for NodeRed access
# log.tmpFile(yAccel,"yAccel.txt")
temperature = bmp.temp()
#(temperature) = tempC[0]
print("Temperature:", temperature)
log.uniqueFile(temperature, "temperature.txt")
pressure = bmp.pressure()
# (pressure) = presskpa[0]
print("Pressure:", pressure)
import L2_log as log
import L2_vector as vec
import time
while (1):
nearest = vec.getNearest()
log.tmpFile(nearest[0], 'distance')
log.tmpFile(nearest[1], 'angle')
time.sleep(0.2)
def loop_drive(ID):
# initialize variables for control system
t0 = 0
t1 = 1
e00 = 0
e0 = 0
e1 = 0
dt = 0
de_dt = np.zeros(2) # initialize the de_dt
count = 0
# initialize variables for color tracking
color_range = np.array([[0, 180, 130], [10, 255, 255]])
thetaOffset = 0
# initialize driving mode
mode = 0
while (1):
count += 1
# THIS BLOCK IS FOR DRIVING BY COLOR TRACKING
# colorTarget = ct.colorTarget(color_range) # use color tracking to generate targets
# x = colorTarget[0]
# if x != None:
# thetaOffset = ct.horizLoc(x) #grabs the angle of the target in degrees
# myThetaDot = thetaOffset * 3.14/180 *2 # achieve centering in 0.5 seconds
# print(myThetaDot)
# myXDot = 0 # enable this line to freeze driving, and turn only.
# myXDot =
# A = np.array([ myXDot, myThetaDot ])
# pdTargets = inv.convert(A) # convert [xd, td] to [pdl, pdr]
# print(pdTargets)
# THIS BLOCK IS FOR CONSTANT SPEED DRIVING (NO CONTROL INPUT)
# constantThetaDot = 1.5 # target, rad/s
# constantXDot = 0.2 # target, m/sc
# A = np.array([constantXDot, constantThetaDot])
# myPhiDots = inv.convert(A)
# THIS CODE IS FOR OPEN AND CLOSED LOOP control
#pdTargets = np.array([9.7, 9.7]) #Fixed requested PhiDots; SPECIFICALLY FOR PID LAB
pdTargets = inv.getPdTargets(
) # populates target phi dots from GamePad
kin.getPdCurrent() # capture latest phi dots & update global var
pdCurrents = kin.pdCurrents # assign the global variable value to a local var
# THIS BLOCK UPDATES VARS FOR CONTROL SYSTEM
t0 = t1 # assign t0
t1 = time.time() # generate current time
dt = t1 - t0 # calculate dt
e00 = e0 # assign previous previous error
e0 = e1 # assign previous error
e1 = pdCurrents - pdTargets # calculate the latest error
de_dt = (e1 - e0) / dt # calculate derivative of error
# CALLS THE CONTROL SYSTEM TO ACTION
signals = gp.getGP() # grab gamepad input
if signals[14]: # check if mode button was pressed (Left stick)
mode = not mode
time.sleep(1)
#t2s.say("changing modes.")
if mode == 0:
sc.driveOpenLoop(pdTargets)
else:
sc.driveClosedLoop(pdTargets, pdCurrents,
de_dt) # call the control system
time.sleep(0.05)
u = sc.u
u_proportional = sc.u_proportional
u_integral = sc.u_integral
u_derivative = sc.u_derivative
# THIS BLOCK OUTPUTS DATA TO A CSV FILE
if count == 1:
log.clear_file()
log.csv_write([
count, pdCurrents[0], pdTargets[0], u[0], u_integral[0],
u_derivative[0]
])
#log.csv_write([count,pdCurrents[0], pdTargets[0], u[0]] )
elif count > 1 and count <= 400:
#log.csv_write([count,pdCurrents[0], pdTargets[0], u[0]])
log.csv_write([
count, pdCurrents[0], pdTargets[0], u[0], u_integral[0],
u_derivative[0]
])
#print(count,pdCurrents[0], pdTargets[0])
else:
break
# L3_telemetry.py
# This program grabs data from the onboard sensors and log data in files
# for NodeRed access and integrate into a custom "flow".
# Access nodered at 192.168.8.1:1880 (by default, it's running on the Blue)
# Import Internal Programs
import L1_mpu as mpu
import L2_log as log
# Import External programs
import numpy as np
import time
# Run the main loop
while True:
accel = mpu.getAccel() # call the function from within L1_mpu.py
(xAccel) = accel[0] # x axis is stored in the first element
(yAccel) = accel[1] # y axis is stored in the second element
print("x axis:", xAccel, "y axis:", yAccel) # print the two values
axes = np.array([xAccel, yAccel]) # store just 2 axes in an array
log.NodeRed2(axes) # send the data to txt files for NodeRed to access.
# log.uniqueFile(xAccel,"xAccel.txt") # another way to log data for NodeRed access
# log.uniqueFile(yAccel,"yAccel.txt")
# log.tmpFile(xAccel,"xAccel.txt") # another way to lof data for NodeRed access
# log.tmpFile(yAccel,"yAccel.txt")
time.sleep(0.2)
def main():
camera = cv2.VideoCapture(camera_input) # Define camera variable
camera.set(3,
size_w) # Set width of images that will be retrived from camera
camera.set(
4, size_h) # Set height of images that will be retrived from camera
tc = 70 # Too Close - Maximum pixel size of object to track
tf = 6 # Too Far - Minimum pixel size of object to track
tp = 65 # Target Pixels - Target size of object to track
band = 40 #range of x considered to be centered
x = 0 # will describe target location left to right
y = 0 # will describe target location bottom to top
radius = 0 # estimates the radius of the detected target
duty = 0
duty_l = 0 # initialize motor with zero duty cycle
duty_r = 0 # initialize motor with zero duty cycle
print("initializing rcpy...")
rcpy.set_state(rcpy.RUNNING) # initialize rcpy
print("finished initializing rcpy.")
p = 0 #Boolean to stop until ___
reloading = 0
button = 0
shooting = 0
scale_t = .8 # a scaling factor for speeds
scale_d = .8 # a scaling factor for speeds
motor_S = 3 # Trigger Motor assigned to #3
motor_r = 2 # Right Motor assigned to #2
motor_l = 1 # Left Motor assigned to #1
state = 1
battery = 0
shots = 0
integrate = 0
ki = 0.001
error = 0
try:
while p != 1:
p = gamepad.getGP()
p = p[7]
print("waiting for input")
while rcpy.get_state() != rcpy.EXITING:
if p == 1:
print(state)
battery = bat.getDcJack()
log.tmpFile(state, "state.txt") #States
log.tmpFile(shots, "shots.txt") #Shots left
log.tmpFile(radius,
"radius.txt") #radius (distance from tartget)
log.tmpFile(battery, "battery.txt") #battery voltage
log.tmpFile(duty_l, "dcL.txt") #motor duty cycle L
log.tmpFile(duty_r, "dcR.txt") #motor duty cycle R
if rcpy.get_state() == rcpy.RUNNING:
#Camera code
ret, image = camera.read() # Get image from camera
height, width, channels = image.shape # Get size of image
if not ret:
break
if filter == 'RGB': # If image mode is RGB switch to RGB mode
frame_to_thresh = image.copy()
else:
frame_to_thresh = cv2.cvtColor(
image, cv2.COLOR_BGR2HSV
) # Otherwise continue reading in HSV
thresh = cv2.inRange(
frame_to_thresh, (v1_min, v2_min, v3_min),
(v1_max, v2_max,
v3_max)) # Find all pixels in color range
kernel = np.ones((5, 5),
np.uint8) # Set gaussian blur strength.
mask = cv2.morphologyEx(thresh, cv2.MORPH_OPEN,
kernel) # Apply gaussian blur
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
cnts = cv2.findContours(
mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[
-2] # Find closed shapes in image
center = None # Create variable to store point
#End of camera code
if state == 1 and len(
cnts) > 0: # If more than 0 closed shapes exist
state = 2
if state == 2 and len(cnts) == 0:
state = 1
if shooting == 1 and state == 2:
state = 3
if state == 3 and reloading == 1:
state = 4
if state == 4 and button == 1:
state = 1
if state == 1:
#case = "turning"
duty_l = 1
duty_r = -1
integrate = 0
if state == 2 and len(cnts) > 0:
#Case = Target Aquesition
c = max(cnts, key=cv2.contourArea
) # Get the properties of the largest circle
((x, y), radius) = cv2.minEnclosingCircle(
c) # Get properties of circle around shape
radius = round(radius,
2) # Round radius value to 2 decimals
print(radius)
x = int(x) # Cast x value to an integer
M = cv2.moments(c) # Gets area of circle contour
center = (int(M["m10"] / M["m00"]),
int(M["m01"] / M["m00"])
) # Get center x,y value of circle
# handle centered condition
if x > ((width / 2) - (band / 2)) and x < (
(width / 2) +
(band / 2)): # If center point is centered
if radius > 38: # Too Close
#case = "too close"
case = "Back Up"
duty = -1.2
print("too close")
shooting = 0
duty_l = duty
duty_r = duty
elif radius >= 34 and radius <= 38:
case = "On target"
duty = 0
shooting = 1
print("on target")
duty_l = duty
duty_r = duty
elif radius < 34: # Too Far
case = "Bruh where it at"
duty = 1.2
duty = scale_d * duty
print("too far")
shooting = 0
duty_l = duty
duty_r = duty
else:
#case = "turning"
case = "Looking For Target"
print(x)
"""
if x>50:
duty_l = .2
duty_r = -.2
elif x<50:
duty_l = -.2
duty_r = .2"""
error = 50 - x
integrate = integrate + error
duty_l = round((x - 0.5 * width) / (0.5 * width) +
ki * integrate, 2) # Duty Left
#duty_l = duty_l*scale_t
#duty_r = round((0.5*width-x)/(0.5*width),2) # Duty Right
#duty_r = duty_r*scale_t
print("turning")
shooting = 0
print(duty_l)
if state == 3:
#State = Shooting
motor.set(motor_l, 0)
motor.set(motor_r, 0)
print("Getting ready to shoot")
motor.set(motor_S, -.45)
time.sleep(.3)
motor.set(motor_S, .45)
time.sleep(.3)
motor.set(motor_S, 0)
print("Gotcha Bitch")
reloading = 1
button = 0
if state == 4:
reloading = 0
print("Waiting for reload, press A when done")
button = gamepad.getGP()
button = button[4]
duty_l = 0
duty_r = 0
shots = shots + 1
# Keep duty cycle within range
if duty_r > 1:
duty_r = 1
elif duty_r < -1:
duty_r = -1
if duty_l > 1:
duty_l = 1
elif duty_l < -1:
duty_l = -1
duty_l = duty_l * scale_t
duty_r = duty_r * scale_t
# Round duty cycles
duty_l = round(duty_l, 2)
duty_r = round(duty_r, 2)
print(duty_l)
print(duty_r)
# Set motor duty cycles
motor.set(motor_l, duty_l)
motor.set(motor_r, duty_r)
import numpy as np
import time
import pysicktim as lidar
import L2_log as log
import L1_gamepad as gp
import L2_speed_control as sp_control
import L2_inverse_kinematics as inv_kin
import L1_motors as motor
import ball_seeker as bs
import gpio_galileo as gpio
count = 0
log.uniqueFile(count,"count.txt")
def vac_ON():
gpio.vac_on()
def vac_OFF():
gpio.vac_off()
def get_min(sweep_dis): #input array from 0.4m sweep
min_sweep = sweep_dis.min() #get the smallest value of the array
return(min_sweep) #return smallest value
def get_distance(): #returns array
lidar.scan() # Requests and returns list of LiDAR
# distance data in meters
scan_points = np.asarray(lidar.scan.distances) # assign all points to scan_points
sweep_dis = scan_points[300:500:1] # a = points on the array from 338 - 474
return sweep_dis #sweep_dis = 0.4 m sweep (distances)
def stop_mov():
def Lab5task2():
axes = heading.getXY()
axesScaled = heading.scale(axes)
h = heading.getHeading(axesScaled)
headingDegrees = round(h * 180 / np.pi, 2)
log.tmpFile(headingDegrees, "headingDegrees.txt")
de_dt = np.zeros(2) # initialize the de_dt
count = 0
# initialize variables for color tracking
color_range = np.array([[0, 185, 100], [175, 220,
175]]) # Input your HSV choices
thetaOffset = 0 # the measured offset of target (degrees)
x = 0 # target center location (pixels)
while (1):
ts0 = time.time() # check time
colorTarget = ct.colorTarget(
color_range) # use color tracking to generate target (x,y,radius)
#print(colorTarget)
ts1 = time.time() - ts0 # check image processing time
log.tmpFile(ts1, "cvSpeed.txt")
if colorTarget != None:
x = colorTarget[0] # assign the x pixel location of the target
if x != None:
thetaOffset = ct.horizLoc(
x) # grabs the angle of the target in degrees
#joint.one(thetaOffset) # request pointer to point to the target.
myThetaDot = thetaOffset * 3.14 / 180 * 2 # attempt centering in 0.5 seconds
print("x, pixels:", x, "T-offset:", thetaOffset, "processing (s):",
round(ts1, 4), "myTD (rad/s):", round(myThetaDot, 3))
myXDot = 0 # freeze the forward driving
# BUILD SPEED TARGETS ARRAY
A = np.array([myXDot, myThetaDot])
pdTargets = inv.convert(A) # convert from [xd, td] to [pdl, pdr]
kin.getPdCurrent() # capture latest phi dots & update global var
except:
print('Warning (I2C): Could not read encoder ' + channel)
angle_raw = 0 # set to zero, avoid sending wrong value
return angle_raw # the returned value must be scaled by ( 359deg / 2^14 )
# The read() function returns both encoder values (RAW).
# Call this function from external programs.
def read():
encLeft = readEnc('L') # call for left enc value
encRight = readEnc('R') # call for right enc value
encoders = np.array([encLeft, encRight]) # form array from left and right
return encoders
if __name__ == "__main__":
file = open('excel_data.csv', "w")
file.close()
while True:
encoders = read()
encoders = np.round((encoders * (360 / 2**14)),
2) # scale values to get degrees
print("encoders: ", encoders) # print the values
# SECTION FOR LOGGING --------------------------
log.csv_write(encoders)
# log.uniqueFile(encoders[1], "encR.txt")
time.sleep(0.10)
# Import pysicktim library as "lidar"
import pysicktim as lidar
import numpy as np
import L2_log as log
# Repeat code nested in for loop 10 times
for x in range(1):
# while True:
lidar.scan() # Requests and returns list of LiDAR
# distance data in meters
scan_points = np.asarray(lidar.scan.distances)
a = scan_points[338:474:1]
log.csv_write(a)
print("sum is", a.sum())
print("std is", np.std(a))
# print distance list
def main():
# INITIALIZE VARIABLES FOR CONTROL SYSTEM
status = "Preping for pickup"
log.stringTmpFile(status, "Final_status.txt")
t0 = 0 # time sample
t1 = 1 # time sample
e00 = 0 # error sample
e0 = 0 # error sample
e1 = 0 # error sample
dt = 0 # delta in time
de_dt = np.zeros(2) # initialize the de_dt
stored_items = 0
r = 0.041 # radius of wheel
l = 0.201 # distance to wheel
target_pixel = np.array([None, None, 0, None, None])
x = 0
#y = 0
#rcpy.set_state(rcpy.RUNNING)
while stored_items < 3:
# GET NEAREST OBJECT
nearest = obs.nearest_point() # L2_obstacle nearest obstacle
obstacleX = nearest[0] # obstacle distance x
obstacleY = nearest[1] - 0.089 # obstacle distance y with lidar offset
# print("Dx: ", obstacleX)
# print("Dy: ", obstacleY)
status = "Searching"
log.stringTmpFile(status, "Final_status.txt")
if obstacleX > 0 and obstacleX < 0.30: # If approaching object
status = "Obstacle Detected...Avoiding"
log.stringTmpFile(status, "Final_status.txt")
# PDTARGET FROM RANDOM ANGLE
randDeg = random.randrange(5, 26,
1) # random degree turn from 5 to 25
print("Deg: ", randDeg)
randRad = randDeg * (math.pi / 180) # convert to radians
rotateTime = (1 *
randRad) / 1.57 # rotation time based on turn amount
# WILL ROTATE LEFT UNLESS OBJECT IS TO LEFT
if (obstacleY > 0): # object to the left
randRad = randRad * -1 # turn right
# SOLVE FOR PHIDOTS FROM THETADOT AND XDOT
A = np.array([[r / 2, r / 2], [-r / (2 * l), r / (2 * l)]])
B = np.array([0, randRad / rotateTime])
pdTargets = np.linalg.solve(A, B) # turning phidots (radians/s)
startTime = time.time() # set starting time
currentTime = 0 # initialize current time
while currentTime < rotateTime: # turn for a certain amount of time
kin.getPdCurrent(
) # capture latest phi dots & update global var
pdCurrents = kin.pdCurrents # assign the global variable value to a local var
# THIS BLOCK UPDATES VARIABLES FOR THE DERIVATIVE CONTROL
t0 = t1 # assign t0
t1 = time.time() # generate current time
dt = t1 - t0 # calculate dt
e00 = e0 # assign previous previous error
e0 = e1 # assign previous error
e1 = pdCurrents - pdTargets # calculate the latest error
de_dt = (e1 - e0) / dt # calculate derivative of error
currentTime = time.time() - startTime # track time elapsed
# CALLS THE CONTROL SYSTEM TO ACTION
sc.driveClosedLoop(pdTargets, pdCurrents, de_dt)
time.sleep(
0.05) # this time controls the frequency of the controller
else:
image = cam.newImage()
height, width, channels = image.shape
target_pixel = target.colorTarget()
radius = target_pixel[2]
x = target_pixel[0]
if radius > 0 and x != -1:
status = "Item Detected"
log.stringTmpFile(status, "Final_status.txt")
#time.sleep(0.5)
print("Radius: \t", radius)
magnet.em_on()
status = "Picking Up item(s)"
log.stringTmpFile(status, "Final_status.txt")
time.sleep(0.5)
status = "Item has been picked up"
log.stringTmpFile(status, "Final_status.txt")
print("Resuming search")
m.MotorL(0)
m.MotorR(0)
store.storing()
time.sleep(0.55)
store.checking()
cell.setup()
status = "storing items...checking tray"
log.stringTmpFile(status, "Final_status.txt")
print("checking tray")
buff1 = cell.loadRead()
time.sleep(1)
print("dropping now")
# turn magnet off
magnet.em_off()
time.sleep(5)
cell.setup()
buff2 = cell.loadRead()
print("checking tray")
print("buff2 \t", buff2, "buff1 \t", buff1, "difference \t",
buff2 - buff1)
if (buff2 - buff1) > 5000:
print("buff2 \t", buff2, "buff1 \t", buff1,
"difference \t", buff2 - buff1)
stored_items = stored_items + 1
store.complete()
status = "retract tray...Storage Complete"
time.sleep(0.55)
store.checking()
log.stringTmpFile(status, "Final_status.txt")
#time.sleep(2)
else:
# keep magnet on
#magnet.em_on
store.complete()
time.sleep(0.55)
#status = "retract tray...Failed Storage"
#log.stringTmpFile(status, "Final_status.txt")
store.checking()
status = "Retract Tray...Failed to store"
log.stringTmpFile(status, "Final_status.txt")
time.sleep(2)
# halt servos to check tray
store.checking()
status = "Resuming Search"
log.stringTmpFile(status, "Final_status.txt")
time.sleep(2)
pdTargets = np.array([5, 5]) # Input requested PhiDots (radians/s)
kin.getPdCurrent() # capture latest phi dots & update global var
pdCurrents = kin.pdCurrents # assign the global variable value to a local var
t0 = t1 # assign t0
t1 = time.time() # generate current time
dt = t1 - t0 # calculate dt
e00 = e0 # assign previous previous error
e0 = e1 # assign previous error
e1 = pdCurrents - pdTargets # calculate the latest error
de_dt = (e1 - e0) / dt # calculate derivative of error
sc.driveClosedLoop(pdTargets, pdCurrents, de_dt)
time.sleep(
0.05) # this time controls the frequency of the controller
log.tmpFile(pdCurrents[0], "Final_PDLEFT.txt")
log.tmpFile(pdCurrents[1], "Final_PDRIGHT.txt")
log.tmpFile(stored_items, "Final_Stored.txt")
m.MotorR(0)
m.MotorL(0)
status = "Mission Complete"
log.stringTmpFile(status, "Final_status.txt")
time.sleep(5)
status = "Please Give Us A Good Grade"
log.stringTmpFile(status, "Final_status.txt")
time.sleep(2)