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 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