def task2():
myVelocities = np.array([1, 0]) #input your first pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
print("Move forward D1")
myVelocities = np.array([0, 1]) #input your 2nd pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(0.789) # input your duration (s)
print("trun left 90 degrees")
myVelocities = np.array([1, 0]) #input your 3rd pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(5) # input your duration (s)
print("Move forward D2")
myVelocities = np.array([0, 1]) #input your 4th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(0.789) # input your duration (s)
print("trun left 90 degrees")
myVelocities = np.array([1, 0]) #input your 5th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
print("Move forward D1")
myVelocities = np.array([0, -1]) #input your 6th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(0.789) # input your duration (s)
print("trun right 90 degrees")
myVelocities = np.array([1, 0]) #input your 7th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(5) # input your duration (s)
print("Move forward D2")
myVelocities = np.array([0, -1]) #input your 8th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(0.789) # input your duration (s)
print("trun right 90 degrees")
myVelocities = np.array([1, 0]) #input your 9th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
print("Move forward D1")
def task2():
d = -1
a = -3.5
t1 = 3
t2 = 2
target = 0
scale = 100
limit = 2
heading = head.getDegrees()
f = (heading - target)
if (f < -5):
a = abs(f) / 360 * scale
elif (f > 5):
a = -abs(f) / 360 * scale
else:
a = 0
if (abs(a) > limit):
a *= limit / abs(a)
elif (abs(a) < 0.3 and abs(a) != 0):
a *= 0.3 / abs(a)
print(a)
myVelocities = np.array([0, a / t2]) #input your first pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
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 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 manual_nav(): c = inv.getPdTargets() sc.driveOpenLoop(c) a = gamepad.getGP() if(a[7] == 1): servo.move1(.7) else: servo.move1(-1) print(a[7])
def task2(): myVelocities = np.array([0.2, 0]) # go forward for a distance of d1 myPhiDots = inv.convert(myVelocities) sc.driveOpenLoop(myPhiDots) time.sleep(10) # input your duration (s) myVelocities = np.array([0.4, 0.133]) # ARC TURN of a length (L) = d2 myPhiDots = inv.convert(myVelocities) sc.driveOpenLoop(myPhiDots) time.sleep(11.78) # input your duration (s) myVelocities = np.array([0.3, 0]) # go forward for a distance of d1 myPhiDots = inv.convert(myVelocities) sc.driveOpenLoop(myPhiDots) time.sleep(6.6) # input your duration (s) myVelocities = np.array([0, 0.131]) # SHARP 90 degree turn 1 myPhiDots = inv.convert(myVelocities) sc.driveOpenLoop(myPhiDots) time.sleep(6) # input your duration (s) myVelocities = np.array([0.4, 0]) # go forward for a distance of d2 myPhiDots = inv.convert(myVelocities) sc.driveOpenLoop(myPhiDots) time.sleep(7.5) # input your duration (s) myVelocities = np.array([0, 0.196]) # SHARP 90 degree turn 2 myPhiDots = inv.convert(myVelocities) sc.driveOpenLoop(myPhiDots) time.sleep(4) # input your duration (s)
def task2():
myVelocities = np.array([1, 0]) #input your first pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
myVelocities = np.array([math.pi / 4, math.pi / 2]) #input your 2nd pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
myVelocities = np.array([1, 0]) #input your 3rd pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
myVelocities = np.array([0, math.pi / 2]) #input your 4th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
myVelocities = np.array([1, 0]) #input your 5th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
myVelocities = np.array([0, math.pi / 2]) #input your 6th pair
myPhiDots = inv.convert(myVelocities)
sc.driveOpenLoop(myPhiDots)
time.sleep(2) # input your duration (s)
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)
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 go_ball(thetadot):
x_dot = 0
B = np.array((x_dot,thetadot)) # B = [x_dot , theta_dot]
C = inv_kin.convert(B) # C = [phi_dot_L , phi_dot_R]
C = np.clip(C, -9.7, 9.7 )
return(C)
def manual_nav():
c = inv_kin.getPdTargets()
duties = sp_control.openLoop(c[0],c[1])
motor.MotorL(duties[0])
motor.MotorR(duties[1])
import L2_kinematics as kin # calculates chassis parameters from wheels
import L2_log as log # log live data to local files
# 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
def manual_nav(): c = inv.getPdTargets() sc.driveOpenLoop(c)
def step(stepnum):
myPhiDots = invkin.convert(xdotthetadot(stepnum))
sc.driveOpenLoop(myPhiDots)
#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
thetaOffset = 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] # x = 0, y = 1, radius = 2
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
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
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 manual_nav():
c = inv.getPdTargets() # takes the phi dot targets defined in
# L2_inverse & assigns it as c converts the
# xdot & tdot values into pldr & pldl
sc.driveOpenLoop(c) # takes the pldl and pldr converted values
axis0 = gp_data[0] * -1
axis1 = gp_data[1] * -1
rthumb = gp_data[3] # up/down axis of right thumb
horn = gp_data[4] # "y" button
# HORN FUNCTION
# the horn is connected by relay to port 1 pin 0 (relay 1 of 2)
# print("horn button:", horn)
# if horn:
# gpio.write(1, 0, 1) # write HIGH
# time.sleep(0.30) # actuate for just 0.2 seconds
# gpio.write(1, 0, 0) # write LOW
# #print("rthumb axis:", rthumb)
# myString = str(round(axis0*100,1)) + "," + str(round(axis1*100,1))
# log.stringTmpFile(myString,"uFile.txt")
#print("Gamepad, xd: " ,axis1, " td: ", axis0) # print gamepad percents
# DRIVE IN OPEN LOOP
chassisTargets = inv.map_speeds(np.array([axis1,
axis0])) # generate xd, td
pdTargets = inv.convert(chassisTargets) # pd means phi dot (rad/s)
# phiString = str(pdTargets[0]) + "," + str(pdTargets[1])
# print("pdTargets (rad/s): \t" + phiString)
# log.stringTmpFile(phiString,"pdTargets.txt")
#DRIVING
sc.driveOpenLoop(pdTargets) #call driving function
#servo.move1(rthumb) # control the servo for laser
time.sleep(0.05)
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
pdCurrents = kin.pdCurrents # assign the global variable value to a local var
# 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
sc.driveClosedLoop(pdTargets, pdCurrents, de_dt) # call the control system
# sc.driveOpenLoop(pdTargets) # call the control system