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