Пример #1
0
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.
Пример #3
0
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
Пример #4
0
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
Пример #5
0
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")
Пример #6
0
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")
Пример #7
0
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")
Пример #8
0
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
Пример #9
0
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
Пример #10
0
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)
Пример #11
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)
Пример #12
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)
    
Пример #13
0
def task2():
  pd = kin.getPdCurrent()
  motion = kin.getMotion()
  print ("pdr: ",pd[0],"pdl: ",pd[1],"xdot: ", motion[0], "thetaDot: ", motion[1])
Пример #14
0
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)
Пример #15
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])
Пример #16
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)
Пример #17
0
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
Пример #18
0
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
Пример #19
0
        "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
Пример #20
0
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)
Пример #21
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
Пример #22
0
 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
Пример #23
0
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
Пример #24
0
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)