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 launch(self): # needs to run in while loop every .005 seconds
# could insert an obstacle avoidance function call here
oba.callObstacle() # pulls in open loop obstacle avoidance code
currentHeading = heading.whereismyHead(
) # gets the direction the robot is facing
print(currentHeading)
thetaOffset = self.targetBearing - currentHeading # calculates theta offset (the difference between the heading and the direction of intention)
print('offset: ', thetaOffset)
myThetaDot = thetaOffset * 3.14 / 180 * 4 # attempt centering in 0.5 seconds
print(myThetaDot)
myXDot = 0.5 # travel half a meter per second in forward speed
# BUILD SPEED TARGETS ARRAY
A = np.array(
[myXDot, myThetaDot]
) # combines xdot and myThetaDot into one array to be passed into inverse kin
pdTargets = inv.convert(A) # convert from [xd, td] to [pdl, pdr]
kin.getPdCurrent() # capture latest phi dots & update global var
pdCurrents = kin.pdCurrents # assign the global variable value to a local var
print(pdTargets)
# UPDATES VARS FOR CONTROL SYSTEM
self.t0 = self.t1 # assign t0
self.t1 = time.time() # generate current time
self.dt = self.t1 - self.t0 # calculate dt
self.e00 = self.e0 # assign previous previous error
self.e0 = self.e1 # assign previous error
self.e1 = pdCurrents - pdTargets # calculate the latest error
self.de_dt = (self.e1 -
self.e0) / self.dt # calculate derivative of error
# CALLS THE CONTROL SYSTEM TO ACTION
sc.driveClosedLoop(pdTargets, pdCurrents,
self.de_dt) # call the control system
time.sleep(0.005) # very small delay.
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 pickupPath():
# 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
driveTime = 1 # rotation time based on turn amount
startTime = time.time() # set starting time
currentTime = 0 # initialize current time
while currentTime < driveTime: # drive for a certain amount of time
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
# 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
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 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 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
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 straightdockvpc():
colorTarget = ct.colorTarget(color_range)
xdot = 0.2 #do not change this for just turning in a circle, leave as 0
tdot = 0 #this updates below, for turning left or right but is initialized to zero
#NOTE: +tdot is ccw, -tdot is cw
#st = sleep time (how long to run)
myVelocities = np.array([xdot, tdot]) #input your first pair
myPhiDots = inv.convert(myVelocities)
# 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 5 <= colorTarget[2] <= 45:
#CLOSED LOOP DRIVE START
myVelocities = np.array([xdot, tdot]) #input your first pair
myPhiDots = inv.convert(myVelocities)
# THIS CODE IS FOR OPEN AND CLOSED LOOP control
pdTargets = np.array([myPhiDots[0], myPhiDots[1]
]) # 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
# 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.driveClosedLoop(pdTargets, pdCurrents, de_dt) # call on closed loop
#CLOSED LOOP DRIVE END
colorTarget = ct.colorTarget(color_range)
if ((colorTarget[0] < 87.5 or colorTarget[0] > 167.5)
and (colorTarget[2] < 36)):
CloseThetaOffset()
m.MotorL(0)
m.MotorR(0)
def DriveDP(xdot, tdot, st):
#NOTE: +tdot is ccw, -tdot is cw
#st = sleep time (how long to run)
myVelocities = np.array([xdot, tdot]) #input your first pair
myPhiDots = inv.convert(myVelocities)
# 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
timestamp = time.time()
endtime = timestamp + st
while (time.time() < endtime):
# THIS CODE IS FOR OPEN AND CLOSED LOOP control
pdTargets = np.array([myPhiDots[0], myPhiDots[1]
]) # 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
# 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.driveClosedLoop(pdTargets, pdCurrents, de_dt) # call on closed loop
time.sleep(0.05) # this time controls the frequency of the controller
MotorL(0)
MotorR(0)
# Initialize variables for control system
t0 = 0 # time 0
t1 = 1 # time 1
e00 = 0 # previous previous error
e0 = 0 # previous error
e1 = 0 # error
dt = 0 # delta-time
de_dt = np.zeros(2) # initialize the de_dt
count = 0
while(1):
# THIS CODE IS FOR CLOSED LOOP control
pdTargets = inv.getPdTargets() # populates target phi dots from GamePad
pdTargets = np.array([1, -1]) # override the gp data
print("targets:", pdTargets)
kin.getPdCurrent() # capture latest phi dots & update global variable
pdCurrents = kin.pdCurrents # assign the global variable value to a local var
# THIS BLOCK UPDATES VARS FOR CONTROL SYSTEM
t0 = t1 # assign time0
t1 = time.time() # capture current time
dt = t1 - t0 # calculate delta-t
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.driveClosedLoop(pdTargets, pdCurrents, de_dt) # call the control system
time.sleep(0.05)
def task2():
pd = kin.getPdCurrent()
motion = kin.getMotion()
print ("pdr: ",pd[0],"pdl: ",pd[1],"xdot: ", motion[0], "thetaDot: ", motion[1])
def driveStraight():
start_time = time.time()
# 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, 103, 137], [43, 178, 213]]) #defines the color range
color_range = np.array([[24, 67, 52], [52, 255,
255]]) #defines the color range
colorTarget = ct.colorTarget(
color_range) #grab an x and radius value from color target
thetaOffset = 0
while (5 <= colorTarget[2] <=
45): #perform the loop until the ball is close
count += 1
# THIS BLOCK IS FOR DRIVING BY COLOR TRACKING
colorTarget = ct.colorTarget(
color_range) # use color tracking to generate targets
x = colorTarget[0] # x = 0, y = 1, radius = 2
if x != None:
thetaOffset = ct.horizLoc(
x) #grabs the angle of the target in degrees
myScalar = .25
myThetaDot = thetaOffset * 3.14 / 180 * 2 * myScalar # achieve centering in 0.5 seconds
myXDot = .30
#print(myThetaDot)cd
A = np.array([myXDot, myThetaDot])
pdTargets = inv.convert(
A
) # convert [xd, td] to [pdl, pdr] TODO: how is pdTargets calculating pdTargets?
print("X: ", colorTarget[0], "\t", "Y: ", colorTarget[1], "\t",
"Radius: ", colorTarget[2], "\t")
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
print(time.time() - start_time, "loop")
sc.driveClosedLoop(pdTargets, pdCurrents,
de_dt) # call the control system
print("The SCUTTLE has docked.")
m.MotorR(0)
m.MotorL(0)
def CloseThetaOffset():
colorTarget = ct.colorTarget(color_range)
xdot = 0 #do not change this for just turning in a circle, leave as 0
tdot = 0.25
#NOTE: +tdot is ccw, -tdot is cw
# 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 (colorTarget[0] < 122.5 or colorTarget[0] > 132.5):
if (colorTarget[0] < 122.5):
print(" Turn Left")
# CALLS THE CONTROL SYSTEM TO ACTION
tdot = 0.15
myVelocities = np.array([xdot, tdot]) #input your first pair
myPhiDots = inv.convert(myVelocities)
pdTargets = np.array([myPhiDots[0], myPhiDots[1]
]) # 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
# 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
sc.driveClosedLoop(pdTargets, pdCurrents,
de_dt) # call on closed loop
elif (colorTarget[0] > 132.5):
print(" Turn Right")
# CALLS THE CONTROL SYSTEM TO ACTION
tdot = -0.15
myVelocities = np.array([xdot, tdot]) #input your first pair
myPhiDots = inv.convert(myVelocities)
pdTargets = np.array([myPhiDots[0], myPhiDots[1]
]) # 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
# 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
sc.driveClosedLoop(pdTargets, pdCurrents,
de_dt) # call on closed loop
colorTarget = ct.colorTarget(color_range)
print(" X: ", colorTarget[0])
m.MotorL(0)
m.MotorR(0)
colorTarget = ct.colorTarget(color_range)
print(" Closed Theta Offset")
print(" X: ", colorTarget[0])
def FindStation():
error = 0
timestamp = time.time()
#PWM = ScalePWM()
colorTarget = ct.colorTarget(color_range)
xdot = 0 #do not change this for just turning in a circle, leave as 0
tdot = 0.25
#NOTE: +tdot is ccw, -tdot is cw
#st = sleep time (how long to run)
myVelocities = np.array([xdot, tdot]) #input your first pair
myPhiDots = inv.convert(myVelocities)
# 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 (colorTarget[0] == None or colorTarget[1] == None
or colorTarget[0] < 110 or colorTarget[1] < 85
or colorTarget[1] > 126 or colorTarget[2] < 5
or colorTarget[2] > 36):
#Turn in circle
#CLOSED LOOP DRIVE START
# THIS CODE IS FOR OPEN AND CLOSED LOOP control
pdTargets = np.array([myPhiDots[0], myPhiDots[1]
]) # 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
# 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.driveClosedLoop(pdTargets, pdCurrents, de_dt) # call on closed loop
#CLOSED LOOP DRIVE END
#m.MotorL(-PWM)
#m.MotorR(PWM)
colorTarget = ct.colorTarget(color_range)
print("X: ", colorTarget[0], "\t", "Y: ", colorTarget[1], "\t",
"Radius: ", colorTarget[2], "\t")
print(" Turning!")
if (time.time() > timestamp + 40):
print("Time expired")
error = 1 #Target not found within time limit
break
m.MotorL(0)
m.MotorR(0)
print(" Found target...")
print(" X: ", colorTarget[0])
return (error)
def main():
# INITIALIZE VARIABLES FOR CONTROL SYSTEM
print("Preping for pickup")
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
r = 0.041 # radius of wheel
l = 0.201 # distance to wheel
target_pixel = np.array([None, None, 0, None, None])
band = 50
tc = 90
tf = 6
tp = 65
x = 0
y = 0
duty_l = 0
duty_r = 0
scale_t = 1.2
scale_d = 0.8
motor_r = 2
motor_l = 1
found_obj = False
first_time = True
#rcpy.set_state(rcpy.RUNNING)
while 1:
# GET NEAREST OBJECT
first_time = True
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)
print("Searching")
if obstacleX > 0 and obstacleX < 0.30: # If approaching object
# 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: # continue straight unless
image = cam.newImage()
height, width, channels = image.shape
target_pixel = target.colorTarget()
radius = target_pixel[2]
x = target_pixel[0]
print("Radius: \t", radius)
if radius > 0 and x != -1:
#and target_pixel[0] is not None
if first_time:
m.MotorL(0)
m.MotorR(0)
first_time = False
time.sleep(0.5)
print("Radius: \t", radius)
found_obj = True
x = int(x)
if x > ((width/2) - (band/2)) and x < ((width/2) + (band/2)):
time.sleep(1)
magnet.em_on()
print("Item Centered")
duty = -1 *((radius-tp)/(tc-tp))
print("slowly approach")
# go to centered item
if radius >= tp:
duty = ((radius-tp)/(tc-tp)) # back up
print("slowly approach")
elif radius < tp:
duty = 1 - ((radius - tf)/(tp-tf))
duty = scale_d * duty
print("approaching item")
duty_r = duty
duty_l = duty
print("duty:\t", duty)
print("Picking Up item(s)")
print("Item has been picked up")
print("Resuming search")
elif x < ((width/2)-(band/2)): #turning right
print("Turning Right")
duty_r = round((x-0.5*width)/(0.5*width),2)
duty_r = duty_r * scale_t
duty_l = round((0.5*width-x)/(0.5*width),2)
print("DutyL:", duty_l, "DutyR: ", duty_r)
duty_l = duty_l * scale_t
elif x > ((width/2)-(band/2)): # turning left
print("Turning Left")
duty_l = round((x-0.5*width)/(0.5*width),2)
duty_l = duty_l * scale_t
duty_r = round((0.5*width-x)/(0.5*width),2)
duty_r = duty_r * scale_t
print("DutyL:", duty_l, "DutyR: ", duty_r)
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 = round(duty_l,2)
duty_r = round(duty_r,2)
m.MotorL(duty_l)
m.MotorR(duty_r)
else:
found_obj = False
if found_obj == False:
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
def main():
# INITIALIZE VARIABLES FOR CONTROL SYSTEM
print("Preping for pickup")
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
r = 0.041 # radius of wheel
l = 0.201 # distance to wheel
x = 0
y = 0
global first_time = True
while 1:
# GET NEAREST OBJECT
#first_time = True now know where to place
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)
print("Searching")
if obstacleX > 0 and obstacleX < 0.30: # If approaching object
first_time = True
# 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: # continue straight unless
image = cam.newImage()
height, width, channels = image.shape
target_pixel = target.colorTarget()
radius = target_pixel[2]
x = target_pixel[0]
print("Radius: \t", radius)
if radius > 0 and x != -1:
#and target_pixel[0] is not None
x = int(x)
CR.center_robot(x,width,first_time)
else:
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
"targ_phi_L": 1,
"targ_phi_R": 2,
"curr_phi_L": 3,
"curr_phi_R": 4,
"x": [10, 10, 10, 10, 10, 10, 10, 10, 10, 10],
"y": [1, 2, 3, 4, 5, 6, 7, 8, 9, 9.7],
"phi_dots_L": None,
"phi_dots_R": None
}
######################################
# UPDATE DATA HERE
# Example:
PhiDots = kin.getPhiDots()
data["phi_dots_L"] = PhiDots[0]
data["phi_dots_R"] = PhiDots[1]
######################################
try:
request, ip = socket.recvfrom(1024) # Wait until data is received
request = json.loads(
request.decode('utf-8')) # Converts data back from bytes to string
log("Received Request from", ip[0], "for:", request) # Log to console
packet = [] # Create empty list to construct packet
for item in request: # Iterate through requested items and
# assemble packet in order requested
if item in data: # Check requested item exists in data dictionary
import csv
print("f")
# IMPORT INTERNAL ITEMS
import L2_speed_control as sc # closed loop control. Import speed_control for open-loop
import L2_inverse_kinematics as inv #calculates wheel parameters from chassis
import L2_kinematics as kin # calculates chassis parameters from wheels
import L2_log as log # log live data to local files
if __name__ == "__main__":
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:
pdCurrents = kin.getPdCurrent()
pdTargets = np.array([9.7, 9.7])
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)
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
def getWheelIncrements(self): # (NO READING) get wheel increment
s1 = self.shaft1 # first you must update shaft2 via updateShaftPositions()
s2 = self.shaft2
self.wheelTravel = kin.phiTravels(s1, s2)
return self.wheelTravel # return wheel travels between datapoints
def randomPath():
# 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
r = 0.041 # radius of wheel
l = 0.201 # distance to wheel
while 1:
# 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)
if obstacleX > 0 and obstacleX < 0.30: # If approaching object
# 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: # continue straight
pdTargets = np.array([9, 9]) # 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
# 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.driveClosedLoop(pdTargets, pdCurrents, de_dt)
time.sleep(
0.05) # this time controls the frequency of the controller
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)