예제 #1
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

        self.port = rospy.get_param('~port', "/dev/ttyACM0")
        rospy.loginfo("Using port: %s" % (self.port))

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR - 1) * 10)
            self.SPEED += ((AageL - 1) * 10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED < 0):
                self.SPEED = 0
            elif (self.SPEED > 330):
                self.SPEED = 330

            self.SPEEDSET = self.SPEED

            ll = (Lft_d * self.DIST)
            rr = (Rgh_t * self.DIST)
            if (rr >= 0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x = int(x)
            y = int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP < self.SPEEDSET):
                self.SPEEDRAMP += (speeddif / 20)
            else:
                self.SPEEDRAMP -= (speeddif / 20)

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0
            elif (self.SPEEDRAMP > 330):
                self.SPEEDRAMP = 330

            if (self.SPEEDSET > 150):
                if (0 < x < 10):
                    x = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < x < 0):
                    x = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150

                if (0 < y < 10):
                    y = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < y < 0):
                    y = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
            else:
                if (0 < x < 5):
                    x = 5
                elif (-5 < x < 0):
                    x = -5

                if (0 < y < 5):
                    y = 5
                elif (-5 < y < 0):
                    y = -5

            #self.xramp = x
            #self.yramp = y

            if (self.xramp < self.lastx):
                self.xramp += 1
            elif (self.xramp == self.lastx):
                pass
            else:
                self.xramp -= 1

            if (self.yramp < self.lasty):
                self.yramp += 1
            elif (self.yramp == self.lasty):
                pass
            else:
                self.yramp -= 1

            if (x == 0 and y == 0):
                self.SPEEDRAMP -= (self.SPEEDSET / 10)
            else:
                if ((abs(self.xramp - x) > 20) or (abs(self.yramp - y) > 20)):
                    self.SPEEDRAMP = 50

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0

            self.lastx = x
            self.lasty = y
            print(self.xramp, x, self.lastx, self.yramp, y, self.lasty,
                  self.SPEEDRAMP, self.SPEEDSET)
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()

            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def optiona(self):
        #forward
        SPEED = 300
        #right
        self.robot.setMotors(-50, 50, SPEED)
        rospy.sleep(1)
        #left
        self.robot.setMotors(100, -100, SPEED)
        rospy.sleep(1)
        self.robot.setMotors(-50, 50, SPEED)
        rospy.sleep(1)

    def optionb(self):
        SPEED = 100
        self.robot.setMotors(-100, -100, SPEED)
        rospy.sleep(1)
        self.robot.setMotors(100, 100, SPEED)
        rospy.sleep(1)

    def optionc(self):
        SPEED = 200
        self.robot.setMotors(-100, -100, SPEED / 2)
        rospy.sleep(1)
        self.robot.setMotors(200, 200, SPEED * (1.5))
        rospy.sleep(1)

    def optiond(self):
        SPEED = 200
        for _ in range(2):
            self.robot.setMotors(-100, -100, SPEED)
            rospy.sleep(1)
            self.robot.setMotors(100, 100, SPEED)
            rospy.sleep(1)

    def joy_handler(self, ps):
        self.Butt = ps.buttons
        self.Axess = ps.axes
예제 #2
0
class NeatoNode:

    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

        self.port = rospy.get_param('~port1 ', "/dev/ttyACM1")
        rospy.loginfo("Using port: %s"%(self.port))

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber("/cbon04", Int8, self.cbon04, queue_size=10)
        rospy.Subscriber('/touches04', Int8, self.touch_handler, queue_size=10)
        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False

    def spin(self):        
        
        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR-1)*10)
            self.SPEED += ((AageL-1)*10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED<0):
                self.SPEED=0
            elif (self.SPEED>330):
                self.SPEED=330
            
            self.SPEEDSET = self.SPEED

            ll = (Lft_d*self.DIST)
            rr = (Rgh_t*self.DIST)
            if (rr>=0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr) 

            x=int(x)
            y=int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP<self.SPEEDSET):
                self.SPEEDRAMP += (speeddif/20)
            else:
                self.SPEEDRAMP -= (speeddif/20)

            if (self.SPEEDRAMP<0):
                self.SPEEDRAMP=0
            elif (self.SPEEDRAMP>330):
                self.SPEEDRAMP=330

            if (self.SPEEDSET > 150):
                if (0<x<10):
                    x=10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
                elif (-10<x<0):
                    x=-10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150

                if (0<y<10):
                    y=10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
                elif (-10<y<0):
                    y=-10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
            else:
                if (0<x<5):
                    x=5
                elif (-5<x<0):
                    x=-5

                if (0<y<5):
                    y=5
                elif (-5<y<0):
                    y=-5
            
            #self.xramp = x 
            #self.yramp = y

            if (self.xramp < self.lastx):
                self.xramp += 1
            elif (self.xramp == self.lastx):
                pass
            else:
                self.xramp -= 1

            if (self.yramp < self.lasty):
                self.yramp += 1
            elif (self.yramp == self.lasty):
                pass
            else:
                self.yramp -= 1


            if (x==0 and y==0):
                self.SPEEDRAMP -= (self.SPEEDSET/10)
            else:
                if ((abs(self.xramp-x)>20) or (abs(self.yramp-y)>20)):
                    self.SPEEDRAMP = 50
            
            if (self.SPEEDRAMP<0):
                self.SPEEDRAMP=0

            self.lastx = x
            self.lasty = y
            print (self.xramp, x, self.lastx, self.yramp, y, self.lasty, self.SPEEDRAMP, self.SPEEDSET)
            # if (self.touch6):
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()

            if (self.touch6):
                pub_LED.publish(1)
                if (self.touch0 or self.touch1):
                    self.left()
                if (self.touch3 or self.touch2):
                    self.right()
                if (self.touch4):
                    self.back()
                if (self.touch5):
                    self.fwd()
            elif (self.touch7):
                pub_LED.publish(1)
                if (self.touch0 or self.touch1):
                    self.leftFast()
                if (self.touch3 or self.touch2):
                    self.rightFast()
                if (self.touch4):
                    self.backFast()
                if (self.touch5):
                    self.fwdFast()
            else:
                pub_LED.publish(0)

            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off") 

    # SQUARE
    def optiona(self):
        SPEED = 125

        self.robot.setMotors(-420,420,SPEED)
        rospy.sleep(3.5)

        self.robot.setMotors(50,50,SPEED)
        rospy.sleep(1)

    # TRIANGLE
    def optionb(self):  
        SPEED=100
        self.robot.setMotors(-100,-100,SPEED)
        rospy.sleep(1)
        self.robot.setMotors(100,100,SPEED)
        rospy.sleep(1)

    # CIRCLE
    def optionc(self):  
        SPEED=200
        self.robot.setMotors(-100,-100,SPEED/2)
        rospy.sleep(1)
        self.robot.setMotors(200,200,SPEED*(1.5))
        rospy.sleep(1)

    # X
    def optiond(self):  
        SPEED=200
        for _ in range(2):
            self.robot.setMotors(-100,-100,SPEED)
            rospy.sleep(1)
            self.robot.setMotors(100,100,SPEED)
            rospy.sleep(1)

    def fwdFast(self):
        SPEED=300
        self.robot.setMotors(-300,-300,SPEED)
        rospy.sleep(1)

    def backFast(self):
        SPEED=200
        self.robot.setMotors(200,200,SPEED)
        rospy.sleep(1)

    def rightFast(self):
        SPEED=150
        self.robot.setMotors(150,-150,SPEED)
        rospy.sleep(1)

    def leftFast(self):
        SPEED=150
        self.robot.setMotors(-150,150,SPEED)
        rospy.sleep(1)

    def fwd(self):
        SPEED=100
        self.robot.setMotors(-100,-100,SPEED)
        rospy.sleep(1)

    def back(self):
        SPEED=50
        self.robot.setMotors(50,50,SPEED)
        rospy.sleep(1) 

    def right(self):
        SPEED=50
        self.robot.setMotors(50,-50,SPEED)
        rospy.sleep(1)

    def left(self):
        SPEED=50
        self.robot.setMotors(-50,50,SPEED)
        rospy.sleep(1)

    def turnRight(self):
        SPEED=100
        self.robot.setMotors(220,-220,150)
        rospy.sleep(2.25)

    def turnLeft(self):
        SPEED=100
        self.robot.setMotors(-210,210,SPEED)
        rospy.sleep(2.25)

    def stop(self):
        SPEED=00
        self.robot.setMotors(00,00,SPEED)
        rospy.sleep(1)
        self.robot.setMotors(00,00,SPEED)

    def joy_handler(self, ps):
        self.Butt =  ps.buttons
        self.Axess = ps.axes

    def cbon04(self, on):
        pub_LED.publish(on.data)
        print(on.data)
        if on.data == 1:
            self.touch6=True
        elif on.data == 0:
            self.touch6=False

    def touch_handler(self, msg):
        self.touch_number = msg.data
        print self.touch_number
        if self.touch_number == 0:
            self.touch0 = True
        if self.touch_number == 1:
            self.touch1 = True
        if self.touch_number == 2:
            self.touch2 = True
        if self.touch_number == 3:
            self.touch3 = True
        if self.touch_number == 4:
            self.touch4 = True
        if self.touch_number == 5:
            self.touch5 = True
        if self.touch_number == 6:
            self.touch6 = not self.touch6
            if self.touch7 == True:
                self.touch7 = False
        if self.touch_number == 7:
            self.touch7 = not self.touch7
            if self.touch6 == True:
                self.touch6 = False
        if self.touch_number == 8:
            self.touch8 = True
        if self.touch_number == 12:
            self.touch0 = False
        if self.touch_number == 13:
            self.touch1 = False
        if self.touch_number == 14:
            self.touch2 = False
        if self.touch_number == 15:
            self.touch3 = False
        if self.touch_number == 16:
            self.touch4 = False
        if self.touch_number == 17:
            self.touch5 = False
        if self.touch_number == 20:
            self.touch8 = False
예제 #3
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop03', anonymous=True)

        self.port = rospy.get_param('~port', "/dev/ttyACM0")
        rospy.loginfo("Using port: %s" % (self.port))

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy03", Joy, self.joy_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        '''
	def optiona(self):
	  #forward
	  self.SPEED = 300
	  #right
	  self.robot.setMotors(-50,50,self.SPEED)
	  rospy.sleep(1)
	  #left
	  self.robot.setMotors(100,-100,self.SPEED)
	  rospy.sleep(1)
	  self.robot.setMotors(-50,50,self.SPEED)
	  rospy.sleep(1)
	def optionb(self):
	  self.SPEED=100
	  self.robot.setMotors(-100,-100,self.SPEED)
	  rospy.sleep(1)
	  self.robot.setMotors(100,100,self.SPEED)
	  rospy.sleep(1)
	def optionc(self):
	  self.SPEED=200
	  self.robot.setMotors(-100,-100,self.SPEED/2)
	  rospy.sleep(1)
	  self.robot.setMotors(200,200,self.SPEED*(1.5))
	  rospy.sleep(1)
	def optiond(self):
	  self.SPEED=200
	  for _ in range(2):
	    self.robot.setMotors(-100,-100,self.SPEED)
	    rospy.sleep(1)
	    self.robot.setMotors(100,100,self.SPEED)
	    rospy.sleep(1)
	'''

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR - 1) * 10)
            self.SPEED += ((AageL - 1) * 10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED < 0):
                self.SPEED = 0
            elif (self.SPEED > 330):
                self.SPEED = 330
            ll = (Lft_d * self.DIST)
            rr = (Rgh_t * self.DIST)
            if (rr >= 0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x = int(x)
            y = int(y)

            print(x, y, self.SPEED)
            self.robot.setMotors(x, y, self.SPEED)
            self.robot.flushing()
            '''
            if tr == 1:
    			self.optiona()
			if ci == 1:
				self.optionb()
			if xx == 1:
				self.optionc()
			if sq == 1:
				self.optiond()
            '''
            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    def joy_handler(self, ps):
        self.Butt = ps.buttons
        self.Axess = ps.axes
예제 #4
0
class NeatoNode:

    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

        self.port = rospy.get_param('~port1 ', "/dev/ttyACM1")
        rospy.loginfo("Using port: %s"%(self.port))

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber('/touches04', Int8, self.touch_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 150
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            leftButt = self.Butt[4]
            rightButt = self.Butt[5]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR-1)*10)
            self.SPEED += ((AageL-1)*10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED<0):
                self.SPEED=0
            elif (self.SPEED>330):
                self.SPEED=330

            self.SPEEDSET = self.SPEED

            ll = (Lft_d*self.DIST)
            rr = (Rgh_t*self.DIST)
            if (rr>=0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x=int(x)
            y=int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP<self.SPEEDSET):
            	self.SPEEDRAMP += (speeddif/20)
            else:
            	self.SPEEDRAMP -= (speeddif/20)

            if (self.SPEEDRAMP<0):
                self.SPEEDRAMP=0
            elif (self.SPEEDRAMP>330):
                self.SPEEDRAMP=330

            if (self.SPEEDSET > 150):
                if (0<x<10):
                    x=10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
                elif (-10<x<0):
                    x=-10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150

                if (0<y<10):
                    y=10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
                elif (-10<y<0):
                    y=-10
                    if (self.SPEEDRAMP>150):
                        self.SPEEDRAMP = 150
            else:
                if (0<x<5):
                    x=5
                elif (-5<x<0):
                    x=-5

                if (0<y<5):
                    y=5
                elif (-5<y<0):
                    y=-5

            #self.xramp = x
            #self.yramp = y

            if (self.xramp < self.lastx):
            	self.xramp += 1
            elif (self.xramp == self.lastx):
            	pass
            else:
            	self.xramp -= 1

            if (self.yramp < self.lasty):
            	self.yramp += 1
            elif (self.yramp == self.lasty):
            	pass
            else:
            	self.yramp -= 1


            if (x==0 and y==0):
            	self.SPEEDRAMP -= (self.SPEEDSET/10)
            else:
                if ((abs(self.xramp-x)>20) or (abs(self.yramp-y)>20)):
                    self.SPEEDRAMP = 50

            if (self.SPEEDRAMP<0):
                self.SPEEDRAMP=0

            self.lastx = x
            self.lasty = y
            print (self.xramp, x, self.lastx, self.yramp, y, self.lasty, self.SPEEDRAMP, self.SPEEDSET)
            # if (self.touch6):
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()
            if leftButt == 1:
                self.optione()
            if rightButt == 1:
                self.optionf()

            if (self.touch6):
                pub_LED.publish(1)
            else:
                pub_LED.publish(0)
            if (self.touch0):
                self.right()
            if (self.touch1):
                self.fwd()
            if (self.touch2):
                self.left()
            if (self.touch3):
                self.back()

            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    # TRIANGLE  -   GREET
    def optionb(self):
        self.lightup()
        self.turn360Left()
        rospy.sleep(0.5)
        self.turn180Left()

    # CIRCLE    -   BRING TO TABLE 1
    def optionc(self):
        self.walk10Forward()
        self.walk3Forward()
        self.walk1Backward()
        self.turn45Left()
        self.turn90Right()
        self.turn45Left()
        self.turn180Right()

    # CROSS     -   BRING TO TABLE 2
    def optiond(self):
        self.turn45Right()
        self.walk10Forward()
        self.walk10Forward()
        self.walk1Backward()
        self.turn45Left()
        self.turn90Right()
        self.turn45Left()
        self.turn180Right()

    # SQUARE    -   BRING TO TABLE 1 / OFFER UP
    def optiona(self):
        self.walk10Forward()
        self.walk3Forward()
        self.walk1Backward()
        self.turn45Left()
        self.turn90Right()
        self.turn45Left()
        self.turn270Right()
        self.walk3Backward()

    # LEFT BUTTON   -   RETURN TO STATION FROM TABLE 1
    def optione(self):
        self.walk10Forward()
        self.walk3Forward()

    # RIGHT BUTTON  -   RETURN TO STATION FROM TABLE 2
    def optionf(self):
        self.walk10Forward()
        self.walk10Forward()
        self.turn45Left()

    # WALK FORWARD
    def walk1Forward(self):
        SPEED=200
        self.robot.setMotors(-100,-100,SPEED)
        rospy.sleep(1)

    def walk3Forward(self):
        SPEED=200
        self.robot.setMotors(-300,-300,SPEED)
        rospy.sleep(2)
        
    def walk5Forward(self):
        SPEED=200
        self.robot.setMotors(-500,-500,SPEED)
        rospy.sleep(3)

    def walk10Forward(self):
        SPEED=200
        self.robot.setMotors(-1200,-1200,SPEED)
        rospy.sleep(6)

    # WALK BACKWARD
    def walk1Backward(self):
        SPEED=200
        self.robot.setMotors(100,100,SPEED)
        rospy.sleep(1)

    def walk3Backward(self):
        SPEED=200
        self.robot.setMotors(300,300,SPEED)
        rospy.sleep(2)
        
    def walk5Backward(self):
        SPEED=200
        self.robot.setMotors(500,500,SPEED)
        rospy.sleep(3)

    def walk10Backward(self):
        SPEED=200
        self.robot.setMotors(1200,1200,SPEED)
        rospy.sleep(6)

    # TURN LEFT
    def turn45Left(self):
        SPEED=200
        self.robot.setMotors(-110,110,SPEED)
        rospy.sleep(1)

    def turn90Left(self):
        SPEED=200
        self.robot.setMotors(-200,200,SPEED)
        rospy.sleep(1.60)

    def turn180Left(self):
        SPEED=200
        self.robot.setMotors(-400,400,SPEED)
        rospy.sleep(3)

    def turn270Left(self):
        SPEED=200
        self.robot.setMotors(-605,605,SPEED)
        rospy.sleep(4.25)

    def turn360Left(self):
        SPEED=200
        self.robot.setMotors(-805,805,SPEED)
        rospy.sleep(5)

    def turn450Left(self):
        SPEED=200
        self.robot.setMotors(-1005,1005,SPEED)
        rospy.sleep(6.5)

    # TURN RIGHT
    def turn45Right(self):
        SPEED=200
        self.robot.setMotors(120,-120,SPEED)
        rospy.sleep(1)

    def turn90Right(self):
        SPEED=200
        self.robot.setMotors(225,-225,SPEED)
        rospy.sleep(1.75)

    def turn180Right(self):
        SPEED=200
        self.robot.setMotors(425,-425,SPEED)
        rospy.sleep(3)

    def turn270Right(self):
        SPEED=200
        self.robot.setMotors(625,-625,SPEED)
        rospy.sleep(4.25)

    def turn360Right(self):
        SPEED=200
        self.robot.setMotors(825,-825,SPEED)
        rospy.sleep(5)

    def turn450Right(self):
        SPEED=200
        self.robot.setMotors(1025,-1025,SPEED)
        rospy.sleep(6.5)



    def shakeNo(self):
        SPEED=275
        self.robot.setMotors(-120,120,SPEED)
        rospy.sleep(1.15)
        self.robot.setMotors(235,-235,SPEED)
        rospy.sleep(1.50)
        self.robot.setMotors(-210,210,SPEED)
        rospy.sleep(1.25)
        self.robot.setMotors(120,-120,SPEED)
        rospy.sleep(1.15)

    def lightup(self):
        pub_LED.publish(1)
        rospy.sleep(0.5)
        pub_LED.publish(0)
        rospy.sleep(0.5)
        pub_LED.publish(1)
        rospy.sleep(0.5)
        pub_LED.publish(0)
        rospy.sleep(0.5)

    # TOUCH SENSOR FUNCTIONS
    def fwd(self):
        SPEED=150
        self.robot.setMotors(-500,-500,SPEED)
        rospy.sleep(1.75)

    def back(self):
        SPEED=150
        self.robot.setMotors(500,500,SPEED)
        rospy.sleep(1.75)

    def right(self):
        SPEED=150
        self.robot.setMotors(200,-200,SPEED)
        rospy.sleep(1)

    def left(self):
        SPEED=150
        self.robot.setMotors(-200,200,SPEED)
        rospy.sleep(1)

    def joy_handler(self, ps):
        self.Butt =  ps.buttons
        self.Axess = ps.axes

    def touch_handler(self, msg):
        self.touch_number = msg.data
        if self.touch_number == 0:
            self.touch0 = True
        if self.touch_number == 1:
            self.touch1 = True
        if self.touch_number == 2:
            self.touch2 = True
        if self.touch_number == 3:
            self.touch3 = True
        if self.touch_number == 4:
            self.touch4 = True
        if self.touch_number == 5:
            self.touch5 = True
        if self.touch_number == 6:
            self.touch6 = not self.touch6
        if self.touch_number == 7:
            self.touch7 = True
        if self.touch_number == 8:
            self.touch8 = True

        if self.touch_number == 12:
            self.touch0 = False
        if self.touch_number == 13:
            self.touch1 = False
        if self.touch_number == 14:
            self.touch2 = False
        if self.touch_number == 15:
            self.touch3 = False
        if self.touch_number == 16:
            self.touch4 = False
        if self.touch_number == 17:
            self.touch5 = False
        if self.touch_number == 19:
            self.touch7 = False
        if self.touch_number == 20:
            self.touch8 = False
예제 #5
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

        self.port = rospy.get_param('~port1 ', "/dev/ttyACM1")
        rospy.loginfo("Using port: %s" % (self.port))

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber("/cbon04", Int8, self.cbon04, queue_size=10)
        rospy.Subscriber('/touches04', Int8, self.touch_handler, queue_size=10)

        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 150
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False
        self.touch9 = False
        self.touch10 = False
        self.touch11 = False

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            leftButt = self.Butt[4]
            rightButt = self.Butt[5]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR - 1) * 10)
            self.SPEED += ((AageL - 1) * 10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED < 0):
                self.SPEED = 0
            elif (self.SPEED > 330):
                self.SPEED = 330

            self.SPEEDSET = self.SPEED

            ll = (Lft_d * self.DIST)
            rr = (Rgh_t * self.DIST)
            if (rr >= 0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x = int(x)
            y = int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP < self.SPEEDSET):
                self.SPEEDRAMP += (speeddif / 20)
            else:
                self.SPEEDRAMP -= (speeddif / 20)

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0
            elif (self.SPEEDRAMP > 330):
                self.SPEEDRAMP = 330

            if (self.SPEEDSET > 150):
                if (0 < x < 10):
                    x = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < x < 0):
                    x = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150

                if (0 < y < 10):
                    y = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < y < 0):
                    y = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
            else:
                if (0 < x < 5):
                    x = 5
                elif (-5 < x < 0):
                    x = -5

                if (0 < y < 5):
                    y = 5
                elif (-5 < y < 0):
                    y = -5

            #self.xramp = x
            #self.yramp = y

            if (self.xramp < self.lastx):
                self.xramp += 1
            elif (self.xramp == self.lastx):
                pass
            else:
                self.xramp -= 1

            if (self.yramp < self.lasty):
                self.yramp += 1
            elif (self.yramp == self.lasty):
                pass
            else:
                self.yramp -= 1

            if (x == 0 and y == 0):
                self.SPEEDRAMP -= (self.SPEEDSET / 10)
            else:
                if ((abs(self.xramp - x) > 20) or (abs(self.yramp - y) > 20)):
                    self.SPEEDRAMP = 50

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0

            self.lastx = x
            self.lasty = y
            print(self.xramp, x, self.lastx, self.yramp, y, self.lasty,
                  self.SPEEDRAMP, self.SPEEDSET)
            # if (self.touch6):
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            # CHECK IF CONTROLLER BUTTONS ARE PRESSED
            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()
            if leftButt == 1:
                self.optione()
            if rightButt == 1:
                self.optionf()

            # CHECK IF TOUCH SENSORS ARE PRESSED
            if (self.touch0 or self.touch1):
                pass
            else:
                if (self.touch2 or self.touch3):
                    self.fwd()
                if (self.touch4 or self.touch5):
                    self.back()
                if (self.touch6 or self.touch7):
                    self.right()
                if (self.touch8 or self.touch9):
                    self.left()
                if (self.touch10 or self.touch11):
                    self.slowFwd()

            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    # SQUARE
    def optiona(self):
        self.walk1Forward()

    # TRIANGLE
    def optionb(self):
        self.walk1Forward()

    # CIRCLE
    def optionc(self):
        self.walk1Forward()

    # CROSS
    def optiond(self):
        self.walk1Forward()

    # LEFT BUTTON
    def optione(self):
        self.walk1Forward()

    # RIGHT BUTTON
    def optionf(self):
        self.walk1Forward()

    # PROGRAMMED FUNCTIONS

    # TOUCH SENSOR FUNCTIONS
    def fwd(self):
        SPEED = 150
        self.robot.setMotors(-500, -500, SPEED)
        rospy.sleep(1.75)

    def slowFwd(self):
        SPEED = 75
        self.robot.setMotors(-100, -100, SPEED)
        rospy.sleep(1)

    def back(self):
        SPEED = 150
        self.robot.setMotors(500, 500, SPEED)
        rospy.sleep(1.75)

    def right(self):
        SPEED = 150
        self.robot.setMotors(200, -200, SPEED)
        rospy.sleep(1)

    def left(self):
        SPEED = 150
        self.robot.setMotors(-200, 200, SPEED)
        rospy.sleep(1)

    def joy_handler(self, ps):
        self.Butt = ps.buttons
        self.Axess = ps.axes

    def cbon04(self, on):
        if on.data == 1:
            pass
        elif on.data == 0:
            pass

    def touch_handler(self, msg):
        self.touch_number = msg.data
        if self.touch_number == 0:
            self.touch0 = True
        if self.touch_number == 1:
            self.touch1 = True
        if self.touch_number == 2:
            self.touch2 = True
        if self.touch_number == 3:
            self.touch3 = True
        if self.touch_number == 4:
            self.touch4 = True
        if self.touch_number == 5:
            self.touch5 = True
        if self.touch_number == 6:
            self.touch6 = True
        if self.touch_number == 7:
            self.touch7 = True
        if self.touch_number == 8:
            self.touch8 = True
        if self.touch_number == 9:
            self.touch9 = True
        if self.touch_number == 10:
            self.touch10 = True
        if self.touch_number == 11:
            self.touch11 = True

        if self.touch_number == 12:
            self.touch0 = False
        if self.touch_number == 13:
            self.touch1 = False
        if self.touch_number == 14:
            self.touch2 = False
        if self.touch_number == 15:
            self.touch3 = False
        if self.touch_number == 16:
            self.touch4 = False
        if self.touch_number == 17:
            self.touch5 = False
        if self.touch_number == 18:
            self.touch5 = False
        if self.touch_number == 19:
            self.touch7 = False
        if self.touch_number == 20:
            self.touch8 = False
        if self.touch_number == 21:
            self.touch9 = False
        if self.touch_number == 22:
            self.touch10 = False
        if self.touch_number == 23:
            self.touch11 = False
예제 #6
0
class NeatoNode:
    def __init__(self):
        """ Start up connection to the Neato Robot. """
        rospy.init_node('teleop04', anonymous=True)

        self.port = rospy.get_param('~port1 ', "/dev/ttyACM1")
        rospy.loginfo("Using port: %s" % (self.port))

        self.robot = Botvac(self.port)

        rospy.Subscriber("/joy04", Joy, self.joy_handler, queue_size=10)
        rospy.Subscriber("/cbon04", Int8, self.cbon04, queue_size=10)
        # rospy.Subscriber('/touches02', Int8, self.touch_handler, queue_size=10)
        self.Axess = (-0.0, -0.0, 1.0, -0.0, -0.0, 1.0, -0.0, -0.0)
        self.Butt = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
        self.SPEED = 0
        self.DIST = 20
        self.SPEEDSET = 0
        self.SPEEDRAMP = 0
        self.lastx = 0
        self.lasty = 0
        self.xramp = 0
        self.yramp = 0
        self.touch_number = -1
        self.touch0 = False
        self.touch1 = False
        self.touch2 = False
        self.touch3 = False
        self.touch4 = False
        self.touch5 = False
        self.touch6 = False
        self.touch7 = False
        self.touch8 = False

    def spin(self):

        # main loop of driver
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            Lft_t = self.Axess[0]
            Lft_d = self.Axess[1]
            Rgh_t = self.Axess[3]
            Rgh_d = self.Axess[4]
            AageL = self.Axess[2]
            AageR = self.Axess[5]
            L_R = self.Axess[6]
            F_B = self.Axess[7]
            sq = self.Butt[0]
            xx = self.Butt[1]
            ci = self.Butt[2]
            tr = self.Butt[3]
            self.SPEED_s = self.Butt[4]
            self.SPEED_f = self.Butt[5]
            AageL_Button = self.Butt[6]
            AageR_Button = self.Butt[7]
            share = self.Butt[8]
            options = self.Butt[9]
            pressL = self.Butt[10]
            pressR = self.Butt[11]
            power = self.Butt[12]
            self.SPEED -= ((AageR - 1) * 10)
            self.SPEED += ((AageL - 1) * 10)
            self.SPEED = int(self.SPEED)
            if (self.SPEED < 0):
                self.SPEED = 0
            elif (self.SPEED > 330):
                self.SPEED = 330

            self.SPEEDSET = self.SPEED

            ll = (Lft_d * self.DIST)
            rr = (Rgh_t * self.DIST)
            if (rr >= 0):
                x = (-ll - rr)
                y = (-ll + rr)
            else:
                x = (-ll - rr)
                y = (-ll + rr)

            x = int(x)
            y = int(y)

            speeddif = abs(self.SPEEDRAMP - self.SPEEDSET)

            if (self.SPEEDRAMP < self.SPEEDSET):
                self.SPEEDRAMP += (speeddif / 20)
            else:
                self.SPEEDRAMP -= (speeddif / 20)

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0
            elif (self.SPEEDRAMP > 330):
                self.SPEEDRAMP = 330

            if (self.SPEEDSET > 150):
                if (0 < x < 10):
                    x = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < x < 0):
                    x = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150

                if (0 < y < 10):
                    y = 10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
                elif (-10 < y < 0):
                    y = -10
                    if (self.SPEEDRAMP > 150):
                        self.SPEEDRAMP = 150
            else:
                if (0 < x < 5):
                    x = 5
                elif (-5 < x < 0):
                    x = -5

                if (0 < y < 5):
                    y = 5
                elif (-5 < y < 0):
                    y = -5

            #self.xramp = x
            #self.yramp = y

            if (self.xramp < self.lastx):
                self.xramp += 1
            elif (self.xramp == self.lastx):
                pass
            else:
                self.xramp -= 1

            if (self.yramp < self.lasty):
                self.yramp += 1
            elif (self.yramp == self.lasty):
                pass
            else:
                self.yramp -= 1

            if (x == 0 and y == 0):
                self.SPEEDRAMP -= (self.SPEEDSET / 10)
            else:
                if ((abs(self.xramp - x) > 20) or (abs(self.yramp - y) > 20)):
                    self.SPEEDRAMP = 50

            if (self.SPEEDRAMP < 0):
                self.SPEEDRAMP = 0

            self.lastx = x
            self.lasty = y
            print(self.xramp, x, self.lastx, self.yramp, y, self.lasty,
                  self.SPEEDRAMP, self.SPEEDSET)
            # if (self.touch6):
            self.robot.setMotors(self.xramp, self.yramp, self.SPEEDRAMP)
            self.robot.flushing()

            if tr == 1:
                self.optiona()
            if ci == 1:
                self.optionb()
            if xx == 1:
                self.optionc()
            if sq == 1:
                self.optiond()

            # if (self.touch6):
            # 	pub_LED.publish(1)
            # 	if (self.touch0 or self.touch1):
            # 		self.left()
            # 	if (self.touch3 or self.touch2):
            # 		self.right()
            # 	if (self.touch4):
            # 		self.back()
            # 	if (self.touch5):
            # 		self.fwd()
            # elif (self.touch7):
            # 	pub_LED.publish(1)
            # 	if (self.touch0 or self.touch1):
            # 		self.leftFast()
            # 	if (self.touch3 or self.touch2):
            # 		self.rightFast()
            # 	if (self.touch4):
            # 		self.backFast()
            # 	if (self.touch5):
            # 		self.fwdFast()
            # else:
            # 	pub_LED.publish(0)

            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setLDS("off")
        self.robot.setTestMode("off")

    # SQUARE
    def optiona(self):
        self.lightup(1, 3)
        self.lightup(0, 1)
        self.lightup(1, 1)
        self.lightup(0, 1)
        self.lightup(1, 1)
        self.lightup(0, 1)
        self.lightup(1, 1)

        self.lightup(0, 1)

    # TRIANGLE
    def optionb(self):
        # self.right(325,300)# almost 120 degree
        # self.fwd()
        BEET = .98
        #Enter

        self.fwd(1000, 100, BEET)
        self.right(300, 130, BEET)
        self.fwd(1000, 100, BEET)
        self.right(300, 130, BEET)

        #03 forward
        self.fwd(0, 0, 7 * BEET)
        self.fwd(0, 0, BEET)

        # shake
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)

        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.left(100, 50, 0.5 * BEET)
        self.right(100, 50, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        #Turn pause, turn pause
        self.left(400, 220, 3 * BEET)
        self.fwd(0, 0, 0.05 * BEET)
        self.left(600, 250, BEET)
        self.fwd(0, 0, 0.1 * BEET)

        # self.lightup(1,BEET)
        # self.lightup(0,0.5*BEET)
        # self.lightup(1,BEET)

        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        self.right(300, 120, 3 * BEET)
        self.fwd(0, 0, BEET)

        self.right(300, 150, 0.55 * BEET)
        self.left(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)
        self.left(300, 150, 0.55 * BEET)
        self.right(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)

        self.right(300, 150, 0.55 * BEET)
        self.left(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)
        self.left(300, 150, 0.55 * BEET)
        self.right(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)

        self.fwd(0, 0, 0.5 * BEET)

        self.back(1000, 100, 0.5 * BEET)
        self.back(0, 0, 0.5 * BEET)
        self.back(1000, 100, 0.5 * BEET)
        self.back(0, 0, 0.5 * BEET)
        self.back(1000, 100, 0.5 * BEET)
        self.back(0, 0, 0.5 * BEET)
        self.back(1000, 100, 0.5 * BEET)
        self.back(0, 0, 0.5 * BEET)
        self.left(300, 100, 2 * BEET)

        self.left(600, 250, BEET)
        self.fwd(0, 0, BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        self.right(600, 250, 2 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        self.left(2200, 150, 12 * BEET)
        self.fwd(0, 0, 2 * BEET)

        #Thriller
        self.right(300, 150, 0.55 * BEET)
        self.left(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)
        self.left(300, 150, 0.55 * BEET)
        self.right(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)

        self.left(600, 250, BEET)
        self.fwd(0, 0, BEET)
        self.right(50, 50, 0.5 * BEET)
        self.left(50, 50, 0.5 * BEET)
        self.right(50, 50, 0.5 * BEET)
        self.left(50, 50, 0.5 * BEET)

        #Thriller
        self.right(300, 150, 0.55 * BEET)
        self.left(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)
        self.left(300, 150, 0.55 * BEET)
        self.right(300, 150, 0.55 * BEET)
        self.fwd(1000, 100, BEET)

        self.right(100, 200, 0.5 * BEET)

        self.left(600, 250, BEET)
        self.fwd(0, 0, 2 * BEET)

        self.right(50, 50, 0.5 * BEET)
        self.left(50, 50, 0.5 * BEET)

        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

        self.fwd(0, 0, 2 * BEET)
        self.left(600, 250, BEET)

        self.right(50, 50, 0.5 * BEET)
        self.left(50, 50, 0.5 * BEET)

        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)
        self.fwd(1000, 100, 0.5 * BEET)
        self.fwd(0, 0, 0.5 * BEET)

    # CIRCLE IS FOR TESTING
    def optionc(self):
        BEET = .98
        #Enter
        self.fwd(1000, 100, BEET)
        self.right(300, 130, BEET)
        self.fwd(1000, 100, BEET)
        self.right(300, 130, BEET)

    # X
    def optiond(self):
        self.lightup()

    def fwd(self, DISTANCE, SPEED, SLEEP):
        self.robot.setMotors(-DISTANCE, -DISTANCE, SPEED)
        rospy.sleep(SLEEP)

    def back(self, DISTANCE, SPEED, SLEEP):
        self.robot.setMotors(DISTANCE, DISTANCE, SPEED)
        rospy.sleep(SLEEP)

    def right(self, ANGLE, SPEED, SLEEP):
        self.robot.setMotors(ANGLE, -ANGLE, SPEED)
        rospy.sleep(SLEEP)

    def left(self, ANGLE, SPEED, SLEEP):
        self.robot.setMotors(-ANGLE, ANGLE, SPEED)
        rospy.sleep(SLEEP)

    def stop(self):
        SPEED = 00
        self.robot.setMotors(00, 00, SPEED)
        rospy.sleep(1)
        self.robot.setMotors(00, 00, SPEED)

    def lightup(self, SWITCH, BEET):
        pub_LED.publish(SWITCH)
        rospy.sleep(BEET)

    def joy_handler(self, ps):
        self.Butt = ps.buttons
        self.Axess = ps.axes

    def cbon04(self, on):
        pub_LED.publish(on.data)
        print(on.data)
        if on.data == 1:
            self.touch6 = True
        elif on.data == 0:
            self.touch6 = False