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