Esempio n. 1
0
    def __init__(self, dontInclude=[], wait_services=True):

        self.tf_listener = tf_server.TFClient()

        if 'head' not in dontInclude:
            self.head = head.Head()
        if 'base' not in dontInclude:
            self.base = ros_navstack_base.Base(self.tf_listener,
                                               wait_service=wait_services)
        if 'spindle' not in dontInclude:
            self.spindle = spindle.Spindle(wait_service=wait_services)
        if 'arms' not in dontInclude:
            self.arms = arms.Arms(
                self.tf_listener)  #TODO: use self.tf_listener here
        if 'leftArm' not in dontInclude:
            self.leftArm = arms.Arm(arms.Side.LEFT, self.tf_listener)
        if 'rightArm' not in dontInclude:
            self.rightArm = arms.Arm(arms.Side.RIGHT, self.tf_listener)
        if 'ebutton' not in dontInclude:
            self.ebutton = ebutton.EButton()
        self.leftSide = arms.Side.LEFT
        self.rightSide = arms.Side.RIGHT
        self.pub_target = rospy.Publisher("/target_location",
                                          geometry_msgs.msg.Pose2D,
                                          queue_size=10)
Esempio n. 2
0
def load_game():

    # LISTS
    # global GUI_LIST_BACKGROUND
    # global GUI_LIST_FOREGROUND
    # global FOOD_LIST
    # global BODY_PARTS_LIST

    GUI_LIST_FOREGROUND.empty()
    GUI_LIST_BACKGROUND.empty()
    FOOD_LIST.empty()
    BODY_PARTS_LIST.empty()

    gui.init()

    global SCROLL_AVANCEMENT
    SCROLL_AVANCEMENT = 0

    global SCORE
    SCORE[0] = 0

    head = he.Head((488, 320), HEAD_SIZE, HEAD_RADIUS)
    head.add(BODY_PARTS_LIST)
    ha.Hand((SCREEN_WIDTH // 4, 3 * SCREEN_HEIGHT // 4), 0, HAND_SPEED,
            HAND_SIZE, head, True).add(BODY_PARTS_LIST)
    ha.Hand((400, 200), 0, HAND_SPEED, HAND_SIZE, head,
            False).add(BODY_PARTS_LIST)
    pg.time.set_timer(pg.USEREVENT, 1000)
    pg.time.set_timer(MEM_MOUSE_EVENT, MEM_MOUSE_EVENT_TIME)
Esempio n. 3
0
    def __init__(self, robot_name="", wait_services=False):

        self.robot_name = robot_name
        self.tf_listener = tf_server.TFClient()

        # Body parts
        self.parts = dict()
        self.parts['base'] = base.Base(self.robot_name, self.tf_listener)
        self.parts['torso'] = torso.Torso(self.robot_name, self.tf_listener)
        self.parts['leftArm'] = arms.Arm(self.robot_name,
                                         self.tf_listener,
                                         side="left")
        self.parts['rightArm'] = arms.Arm(self.robot_name,
                                          self.tf_listener,
                                          side="right")
        self.parts['head'] = head.Head(self.robot_name, self.tf_listener)

        # Human Robot Interaction
        self.parts['lights'] = lights.Lights(self.robot_name, self.tf_listener)
        self.parts['speech'] = speech.Speech(
            self.robot_name, self.tf_listener,
            lambda: self.lights.set_color(1, 0, 0),
            lambda: self.lights.set_color(0, 0, 1))
        self.parts['hmi'] = api.Api(self.robot_name, self.tf_listener)
        self.parts['ears'] = ears.Ears(self.robot_name,
                                       lambda: self.lights.set_color(0, 1, 0),
                                       lambda: self.lights.set_color(0, 0, 1))

        self.parts['ebutton'] = ebutton.EButton(self.robot_name,
                                                self.tf_listener)

        # Reasoning/world modeling
        self.parts['ed'] = world_model_ed.ED(self.robot_name, self.tf_listener)

        # Miscellaneous
        self.pub_target = rospy.Publisher("/target_location",
                                          geometry_msgs.msg.Pose2D,
                                          queue_size=10)
        self.base_link_frame = "/" + self.robot_name + "/base_link"

        # Grasp offsets
        #TODO: Don't hardcode, load from parameter server to make robot independent.
        self.grasp_offset = geometry_msgs.msg.Point(0.5, 0.2, 0.0)

        # Create attributes from dict
        for k, v in self.parts.iteritems():
            setattr(self, k, v)
        self.spindle = self.torso  # (ToDo: kind of ugly, why do we still have spindle???)
        self.arms = OrderedDict(
            left=self.leftArm, right=self.rightArm
        )  # (ToDo: kind of ugly, why do we need this???)
        self.ears._hmi = self.hmi  # ToDo: when ears is gone, remove this line

        # Wait for connections
        s = rospy.Time.now()
        for k, v in self.parts.iteritems():
            v.wait_for_connections(1.0)
        e = rospy.Time.now()
        rospy.logdebug("Connecting took {} seconds".format((e - s).to_sec()))
Esempio n. 4
0
    def test_train(self):
        options = ranking_head.Options()
        head = ranking_head.Head(loss_fn=_make_loss_fn())
        loss = head.run(mode=ranking_head.ModeKeys.TRAIN,
                        labels=self._default_labels,
                        logits=self._default_logits,
                        features={})

        self.assertAlmostEqual(loss.item(), self._default_loss)
Esempio n. 5
0
 def __init__(self, length=None, **kwargs):
     if length is None:
         length = 100.0
     self.length = length  # length to constrain worm
     self.head_ = head.Head(**kwargs)
     #
     self._bodypoints = [
         self.head_.position.move(self.head_.angle, -self.length)
     ]
Esempio n. 6
0
 def test_eval(self):
     metric_fns = {
         'metric/precision@1':
         metrics_lib.make_ranking_metric_fn(
             metrics_lib.RankingMetricKey.PRECISION, topn=1),
     }
     head = ranking_head.Head(loss_fn=_make_loss_fn(),
                              eval_metric_fns=metric_fns)
     loss, metrics_values = head.run(ranking_head.ModeKeys.EVAL,
                                     self._default_labels,
                                     self._default_logits,
                                     features={})
     self.assertAlmostEqual(loss.item(), self._default_loss, 5)
Esempio n. 7
0
    def __init__(self):

        # Initialise pygame
        os.environ["SDL_VIDEO_CENTERED"] = "1"
        pygame.init()

        s_width = 1600
        s_height = 800
        # Set up the display
        self.screen = pygame.display.set_mode((s_width, s_height))

        self.clock = pygame.time.Clock()

        # define cornerpoints
        wall_thickness = 20.0
        c_depth = 250.0
        p0 = (wall_thickness, wall_thickness)
        p1 = (s_width - wall_thickness, wall_thickness)
        p2 = (s_width - wall_thickness, s_height - wall_thickness)
        p3 = (wall_thickness, s_height - wall_thickness)
        self.walls = [
            RectWall((p0, p1), (0.0, 0.0), (wall_thickness, s_height)),
            RectWall((p1, p2), (s_width - wall_thickness, 0.0),
                     (wall_thickness, s_height)),
            RectWall((p2, p3), (0.0, s_height - wall_thickness),
                     (s_width, wall_thickness)),
            RectWall((p3, p0), (0.0, 0.0), (s_width, wall_thickness)),
            PolyWall(((0, c_depth), (c_depth, 0.0), (0.0, 0.0))),
            PolyWall(((s_width - c_depth, 0.0), (s_width, c_depth), (s_width,
                                                                     0.0))),
            PolyWall(((s_width, s_height - c_depth),
                      (s_width - c_depth, s_height), (s_width, s_height))),
            PolyWall(((0, s_height - c_depth), (c_depth, s_height),
                      (0.0, s_height)))
        ]

        self.orbits = [
            orbit.Orbit((500, 600), 100, [40 for i in range(3)]),
            orbit.Orbit((900, 500), 100, [40 for i in range(3)]),
            orbit.Orbit((200, 400), 100, [40 for i in range(3)])
        ]

        self.orbs = []

        self.players = [
            head.Head(self, (400.0, 400.0), utils.unit_vector((-1, -1.01)))
        ]
        # self.players = [head.Head(self, (random.randrange(200.0, 1400.0), random.randrange(200.0, 600.0)), utils.unit_vector((random.random()*2 - 1, random.random() * 2 - 1))) for i in range(10)]
        # self.players = [orb.Orb(self, (701.0, 200.0), utils.unit_vector((-1, 0))), orb.Orb(self, (100.0, 259.0), utils.unit_vector((1, 0)))]

        self.tail = False
Esempio n. 8
0
    def __init__(self, robot_name="", wait_services=False):

        self.robot_name = robot_name

        self.tf_listener = tf_server.TFClient()

        # Body parts
        self.base = base.Base(self.robot_name,
                              self.tf_listener,
                              wait_service=wait_services)
        self.torso = torso.Torso(self.robot_name, wait_service=wait_services)
        self.spindle = self.torso
        self.leftArm = arms.Arm(self.robot_name, "left", self.tf_listener)
        self.rightArm = arms.Arm(self.robot_name, "right", self.tf_listener)
        self.arms = OrderedDict(left=self.leftArm, right=self.rightArm)

        self.head = head.Head(self.robot_name)

        # Human Robot Interaction
        self.speech = speech.Speech(self.robot_name,
                                    wait_service=wait_services)
        self.ears = ears.Ears(self.robot_name)
        self.ebutton = ebutton.EButton()
        self.lights = lights.Lights()

        # Perception: can we get rid of this???
        self.perception = perception_ed.PerceptionED(
            wait_service=wait_services)

        # Reasoning/world modeling
        self.ed = world_model_ed.ED(wait_service=wait_services)
        self.reasoner = reasoner.Reasoner()

        # Miscellaneous
        self.pub_target = rospy.Publisher("/target_location",
                                          geometry_msgs.msg.Pose2D,
                                          queue_size=10)
        self.base_link_frame = "/" + self.robot_name + "/base_link"

        #Grasp offsets
        #TODO: Don't hardcode, load from parameter server to make robot independent.
        self.grasp_offset = geometry_msgs.msg.Point(0.5, 0.2, 0.0)
Esempio n. 9
0
    def __init__(self, canvas, elements, changes):
        self.canvas = canvas

        self.head_commands = ["<titel>"]

        self.head_tags = [[]]

        # changes = all elements and their attributes seperated
        if not isinstance(changes, str):
            self.changes = changes
            self.chan_ex = True
        else:
            self.changes = 0
            self.chan_ex = False

        #strings of tags+values
        self.elements = elements

        #list of objects
        self.objects = []

        # seperate head tags
        index = 0
        for x in range(len(self.elements) - 1):
            for y in range(len(self.head_commands)):
                if self.elements[x][0] == self.head_commands[y]:
                    self.head_tags.insert(
                        index, [self.head_commands[y], self.elements[x][1]])
                    index += 1

        #number of objects
        self.count = len(self.elements) - 1

        for x in range(self.count):
            if self.elements[x][0] == "<ue1>":
                self.objects.append(
                    H1.H1(self.canvas, self.elements[x][1], 0, 0))
            elif self.elements[x][0] == "<ue2>":
                self.objects.append(
                    H2.H2(self.canvas, self.elements[x][1], 0, 0))
            elif self.elements[x][0] == "<a>":
                self.objects.append(P.P(self.canvas, self.elements[x][1], 0,
                                        0))
            elif self.elements[x][0] == "<ul>":
                self.objects.append(
                    UL.UL(self.canvas, self.elements[x][1], 0, 0))
            elif self.elements[x][0] == "<gl>":
                self.objects.append(
                    OL.OL(self.canvas, self.elements[x][1], 0, 0))

        self.obj_num = len(self.objects)

        #show all the stuff and run ksb
        if self.chan_ex:
            for x in range(len(self.changes)):
                if self.changes[x].element == "koerper":
                    for i in range(len(self.changes[x].functions) -
                                   1):  #go though all attributes
                        if self.changes[x].functions[i][
                                0] == "hintergrundfarbe":
                            self.change_background_body(
                                self.changes[x].functions[i][1])

                # get <a>s done
                if self.changes[x].element == "a":
                    self.scan_element("<a>", x)

                # get ue1 done
                if self.changes[x].element == "ue1":
                    self.scan_element("<ue1>", x)

                # get ue2 done
                if self.changes[x].element == "ue2":
                    self.scan_element("<ue2>", x)

                # get uls done
                if self.changes[x].element == "ul":
                    self.scan_element("<ul>", x)

                # get ols done
                if self.changes[x].element == "gl":
                    self.scan_element("<gl>", x)

        # execute head
        if len(self.head_tags) != 0:
            head.Head(self.canvas, self.head_tags, self.objects)

        overallSize = 0
        for x in range(self.obj_num):
            if x != 0:
                overallSize += self.objects[x - 1].size
                self.objects[x].y += 15 * x + overallSize
            self.objects[x].display()
Esempio n. 10
0
    def __init__(self, robot_name="", wait_services=False):

        self.robot_name = robot_name
        self.tf_listener = tf.TransformListener()

        # Body parts
        self.parts = dict()
        self.parts['base'] = base.Base(self.robot_name, self.tf_listener)
        self.parts['torso'] = torso.Torso(self.robot_name, self.tf_listener)
        self.parts['leftArm'] = arms.Arm(self.robot_name,
                                         self.tf_listener,
                                         side="left")
        self.parts['rightArm'] = arms.Arm(self.robot_name,
                                          self.tf_listener,
                                          side="right")
        self.parts['head'] = head.Head(self.robot_name, self.tf_listener)
        self.parts['perception'] = perception.Perception(
            self.robot_name, self.tf_listener)
        self.parts['ssl'] = ssl.SSL(self.robot_name, self.tf_listener)

        # Human Robot Interaction
        self.parts['lights'] = lights.Lights(self.robot_name, self.tf_listener)
        self.parts['speech'] = speech.Speech(
            self.robot_name, self.tf_listener,
            lambda: self.lights.set_color_colorRGBA(lights.SPEAKING),
            lambda: self.lights.set_color_colorRGBA(lights.RESET))
        self.parts['hmi'] = api.Api(
            self.robot_name, self.tf_listener,
            lambda: self.lights.set_color_colorRGBA(lights.LISTENING),
            lambda: self.lights.set_color_colorRGBA(lights.RESET))
        self.parts['ears'] = ears.Ears(
            self.robot_name, self.tf_listener,
            lambda: self.lights.set_color_colorRGBA(lights.LISTENING),
            lambda: self.lights.set_color_colorRGBA(lights.RESET))

        self.parts['ebutton'] = ebutton.EButton(self.robot_name,
                                                self.tf_listener)

        # Reasoning/world modeling
        self.parts['ed'] = world_model_ed.ED(self.robot_name, self.tf_listener)

        # Miscellaneous
        self.pub_target = rospy.Publisher("/target_location",
                                          geometry_msgs.msg.Pose2D,
                                          queue_size=10)
        self.base_link_frame = "/" + self.robot_name + "/base_link"

        # Check hardware status
        self._hardware_status_sub = rospy.Subscriber(
            "/" + self.robot_name + "/hardware_status", DiagnosticArray,
            self.handle_hardware_status)

        # Grasp offsets
        #TODO: Don't hardcode, load from parameter server to make robot independent.
        self.grasp_offset = geometry_msgs.msg.Point(0.5, 0.2, 0.0)

        # Create attributes from dict
        for partname, bodypart in self.parts.iteritems():
            setattr(self, partname, bodypart)
        self.arms = OrderedDict(
            left=self.leftArm, right=self.rightArm
        )  # (ToDo: kind of ugly, why do we need this???)
        self.ears._hmi = self.hmi  # ToDo: when ears is gone, remove this line

        # Wait for connections
        s = rospy.Time.now()
        for partname, bodypart in self.parts.iteritems():
            bodypart.wait_for_connections(1.0)
        e = rospy.Time.now()
        rospy.logdebug("Connecting took {} seconds".format((e - s).to_sec()))

        if not self.operational:
            not_operational_parts = [
                name for name, part in self.parts.iteritems()
                if not part.operational
            ]
            rospy.logwarn("Not all hardware operational: {parts}".format(
                parts=not_operational_parts))
"""
Author: Jan Bernhard
Last updated: 04/22/18
Purpose: Testing head.py with defined control sequence. 
"""

import head

obj = head.Head()
obj.connect()

print(">>> TEST SEQUENCE STARTED <<<")
print(">>> Testing Turn Table: Standard use")
obj.test_look(turn=-45)
obj.test_look(turn=0)
obj.test_look(turn=45)
obj.test_look(turn=-90)
obj.test_look(turn=0)
obj.test_look(turn=90)
obj.test_look(turn=0)
print(">>> Testing Turn Table: Invalid inputs")
obj.test_look(turn=-100)
obj.test_look(turn=100)
print(">>> Testing Turn Table: Complete")
print(">>> Testing Tilt Axis: Standard use")
obj.test_look(tilt=-15)
obj.test_look(tilt=0)
obj.test_look(tilt=15)
obj.test_look(tilt=-30)
obj.test_look(tilt=0)
obj.test_look(tilt=30)
class avoidanceofObjects:
    BP.set_sensor_type(BP.PORT_4, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
    switcher = 0
    direction = 0
    ScanValues = [0, 0, 0]
    leftScanArray = []
    rightScanArray = []
    headAngle = 0  # 0 is for centre, 1 is for left, and 2 is for right
    i = 0

    closeToObject = False
    positionSet = False
    h = head.Head(120, 107)

    def main(self):
        if (self.switcher == 0):
            self.avoidance()
        elif (self.switcher == 1):
            self.checkObject()
        elif (self.switcher == 2):
            self.aroundObject()
        else:
            print("Error")

    def getAverage(self, someArray):
        print("Old Array:" + str(someArray))
        for x in someArray:
            if (x == 255.0):
                someArray.remove(x)
        print("New Array:" + str(someArray))
        return statistics.median(someArray)

    def getLowest(self, someArray):
        print("Old Array:" + str(someArray))
        return min(someArray)

    def avoidance(self):
        uValue = 70
        uValue = self.getUltrasonic()
        print("Avoidance:" + str(uValue))
        if (uValue <= 10):
            self.closeToObject = True
        if (uValue == 0):
            drive.stop()
        else:
            if (self.closeToObject == True):
                if (self.ScanValues[2] == 0):
                    self.h.returnCentre()
                    drive.stop()
                    time.sleep(1)
                    self.ScanValues[2] = self.getUltrasonic()
                    print("Centre Scan Value:" + str(uValue))
                if (self.i <= 9 and self.ScanValues[1] == 0):
                    if (self.i == 0):
                        scanV = self.h.scan(0)
                    else:
                        scanV = 3
                    if (scanV == 2):
                        self.leftScanArray.append(self.getUltrasonic())
                    elif (scanV == 3):
                        if (self.ScanValues[0] == 0):
                            self.ScanValues[0] = self.getLowest(
                                self.leftScanArray)
                            print("Left Scan Value:" + str(self.ScanValues[0]))

                        self.rightScanArray.insert(self.i,
                                                   self.getUltrasonic())
                        self.i += 1
                        print("i = " + str(self.i))
                        # print("Right Scan Value:" + str(self.getUltrasonic()))
                elif (self.ScanValues[1] == 0):
                    self.ScanValues[1] = self.getLowest(self.rightScanArray)
                    print("Right Scan Value:" + str(self.ScanValues[1]))
                    self.i = 0

                elif (self.positionSet == False):
                    if (self.ScanValues[1] > 60):
                        self.h.turnLeft(0.7)
                        self.direction = 1
                        self.headAngle = 1
                    elif (self.ScanValues[0] > 60):
                        self.h.turnRight(0.7)
                        self.direction = 0
                        self.headAngle = 2
                    else:
                        self.h.returnCentre()
                        self.direction = random.randint(0, 1)
                        self.headAngle = 0
                    self.positionSet = True
                else:
                    if (uValue < 5):
                        if (self.headAngle == 0):
                            if (self.direction == 0):
                                drive.turnLeft90()
                                self.h.turnRight(1)
                            else:
                                drive.turnRight90()
                                self.h.turnLeft(1)
                        elif (self.headAngle == 1):
                            drive.turnRight45()
                            self.h.returnCentre()
                            self.h.turnLeft(1)
                        else:
                            drive.turnLeft45()
                            self.h.returnCentre()
                            self.h.turnRight(1)
                        self.switcher = 1
                    else:
                        drive.moveForward()
            else:
                drive.moveForward()
                self.h.scan(1)

    def checkObject(self):
        self.closeToObject = False
        self.positionSet = False
        self.ScanValues[1] = 0
        self.ScanValues[0] = 0
        self.ScanValues[2] = 0
        self.rightScanArray = []
        self.leftScanArray = []

        uValue = self.getUltrasonic()

        p = -1
        error = (uValue - 15) * p
        if (uValue > 200):
            BP.set_motor_power(BP.PORT_A, -20)
            BP.set_motor_power(BP.PORT_D, -20)
        elif (self.direction == 0):
            BP.set_motor_power(BP.PORT_A, -speed - (error * 0.8))
            BP.set_motor_power(BP.PORT_D, -speed + (error * 0.8))
        else:
            BP.set_motor_power(BP.PORT_A, -speed + (error * 0.8))
            BP.set_motor_power(BP.PORT_D, -speed - (error * 0.8))
        print(uValue)

        if (uValue > 70 and uValue < 200):
            self.switcher = 2
            self.h.returnCentre()
            drive.moveForward()

    def aroundObject(self):
        if (self.direction == 0):
            time.sleep(0.5)
            drive.pivotTurn90(25, 40, -2400)
        else:
            time.sleep(0.5)
            drive.pivotTurn90(40, 25, -2400)

        self.switcher = 0

    def getUltrasonic(self):
        try:
            return BP.get_sensor(BP.PORT_4)
        except brickpi3.SensorError as error:
            print(error)
            return 0
Esempio n. 13
0
class avoidanceofObjects:
    BP.set_sensor_type(BP.PORT_4, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
    switcher = 0
    direction = 0
    h = head.Head(120, 107)

    def main(self):
        if (self.switcher == 0):
            self.avoidance()
        elif (self.switcher == 1):
            self.checkObject()
        elif (self.switcher == 2):
            self.aroundObject()
        else:
            print("Error")

    def avoidance(self):
        uValue = 70
        uValue = self.getUltrasonic()
        if (uValue == 0):
            drive.stop()
        else:
            if (uValue <= 3):
                self.direction = random.randint(0, 1)
                if (self.direction == 0):
                    drive.turnLeft45()
                    drive.pivotTurn45(30, 10)
                    self.h.turnRight()
                    time.sleep(1)
                else:
                    drive.turnRight45()
                    drive.pivotTurn45(10, 30)
                    self.h.turnLeft()
                    time.sleep(1)

                self.switcher = 1
            else:
                drive.moveForward()

    def checkObject(self):

        uValue = self.getUltrasonic()

        p = -1
        error = (uValue - 10) * p
        if (self.direction == 0):
            BP.set_motor_power(BP.PORT_A, -speed - (error * 0.8))
            BP.set_motor_power(BP.PORT_D, -speed + (error * 0.8))
        else:
            BP.set_motor_power(BP.PORT_A, -speed + (error * 0.8))
            BP.set_motor_power(BP.PORT_D, -speed - (error * 0.8))
        print(uValue)

        if (uValue > 40):
            self.switcher = 2
            self.h.returnCenter()
            drive.moveForward()

    def aroundObject(self):
        if (self.direction == 0):
            time.sleep(0.5)
            # drive.pivotTurn90(20,40)
            drive.pivotTurn90(15, 40)
        else:
            time.sleep(0.5)
            drive.pivotTurn90(40, 20)

        self.switcher = 0

    def getUltrasonic(self):
        try:
            return BP.get_sensor(BP.PORT_4)
        except brickpi3.SensorError as error:
            print(error)
            return 0
Esempio n. 14
0
maxY = 400
minY = 50
minX = 0
maxX = 640
minPan = 175
maxPan = 150
minTilt = 100
maxTilt = 150
shutdownTime = 2
switchOlder = False

confidence = float(0.5)
nms_thresh = float(0.3)
#########################

heads = [head.Head() for count in range(4)]
dmx_data = list()
lightSettingsOn = list([255, 0, 0, 0, 200])
lightSettingsOff = list([255, 0, 0, 0, 0])
sender = sacn.sACNsender(bind_port=6553, bind_address="10.8.220.23")


def findList(list, obj):
    counter = 0
    for i in list:
        if i == obj:
            return counter
        counter += 1
    return -1

Esempio n. 15
0
        pygame.mixer.music.play()


#
# INITIALIZATION
#

# Joystick
j = xbox.Joystick()
# Maestro Controllers
servo = maestro.Controller()
# DriveTrain
drivetrain = drive.DriveTrain(servo, CH_RIGHT_MOTOR, CH_LEFT_MOTOR)
speedtoggle = True  # false = slow, true = normal
# Head servo
head = head.Head(servo)
# Emotions
emotion = emotions.Emotions(head)
# Sound
pygame.mixer.init(22050, -16, 1, 1024)
channel = pygame.mixer.Channel(1)
sounds = [
    "Chatter 2.mp3", "Motor whir 3.mp3", "Jitters.mp3", "Motor whir 4.mp3",
    "Ohhhh.mp3", "Oooh.mp3", "Shakey shakey.mp3", "Whir Click.mp3",
    "Whoa 2.mp3"
]

#
# MAIN LOOP
#
print("Robot Online")