test

ev3dev_remote_control_protype1/.gitignore

@@ -0,0 +1,5 @@
+__pycache__/
+*.pyc
+.coverage
+htmlcov
+.vscode

ev3dev_remote_control_protype1/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2020 Nino Guba
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

ev3dev_remote_control_protype1/README.md

@@ -0,0 +1,3 @@
+Check out my robot arm on Rebrickable:
+
+https://rebrickable.com/mocs/MOC-52151/gubsters/six-axis-6dof-robotic-arm-with-interchangeable-end-effectors/

ev3dev_remote_control_protype1/math_helper.py

@@ -0,0 +1,16 @@
+# Math helpers in separate file for easy unit testing
+def scale(val, src, dst):
+    return (float(val - src[0]) / (src[1] - src[0])) * (dst[1] - dst[0]) + dst[0]
+
+
+def scale_stick(value, deadzone=10, scale_to=80, invert=False):
+    """ scale a range of input to a range of output, optionally applying a deadzone or inverting the result """
+    result = scale(value, (0, 255), (-scale_to, scale_to))
+
+    if deadzone and result < deadzone and result > -deadzone:
+        result = 0
+
+    if invert:
+        result *= -1
+
+    return result

ev3dev_remote_control_protype1/remote_control.py

@@ -0,0 +1,518 @@
+#!/usr/bin/env python3
+# ev3-robot-arm 6dof, originally by Nino Guba.
+# v2 improved by Marno van der Molen;
+# - bugfixes
+# - don't require grabber attachment to run
+# - more debug output for troubleshooting
+# - improved gamepad responsiveness
+# - proportional control for some motors
+# - code cleanup / simplify
+# v2.1 refinements by Marno van der Molen:
+# - simlify code
+# - allow changing speed of movement by holding d-pad up/down
+# - optionally support a color sensor to align waist by pressing d-pad left/right
+# v2.2 minor improvements by Marno van der Molen
+# - maintain grabber grip during spin
+# - increase joystick deadzone a bit to prevent unintended movement while pressing L3/R3
+# - start work on calibration support using touch sensors
+# - prevent calculate_speed() returning values over 100 which causes exceptions
+
+__author__ = 'Nino Guba'
+
+import logging
+import os
+import sys
+import threading
+import time
+
+import evdev
+import rpyc
+from signal import signal, SIGINT
+from ev3dev2 import DeviceNotFound
+from ev3dev2.led import Leds
+from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
+from ev3dev2.sensor.lego import ColorSensor, TouchSensor
+from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, LargeMotor, MoveTank
+from ev3dev2.power import PowerSupply
+
+# from ev3dev2.sound import Sound
+from evdev import InputDevice
+
+from math_helper import scale_stick
+
+
+# Config
+REMOTE_HOST = '10.42.0.3'
+JOYSTICK_DEADZONE = 20
+
+# Define speeds
+FULL_SPEED = 100
+FAST_SPEED = 75
+NORMAL_SPEED = 50
+SLOW_SPEED = 25
+VERY_SLOW_SPEED = 10
+
+# Setup logging
+os.system('setfont Lat7-Terminus12x6')
+logging.basicConfig(level=logging.INFO, stream=sys.stdout,
+                    format='%(message)s')
+logger = logging.getLogger(__name__)
+
+
+def reset_motors():
+    """ reset motor positions to default """
+    logger.info("Resetting motors...")
+    waist_motor.reset()
+    shoulder_motors.reset()
+    elbow_motor.reset()
+    roll_motor.reset()
+    pitch_motor.reset()
+    spin_motor.reset()
+    if grabber_motor:
+        grabber_motor.reset()
+
+
+# Initial setup
+
+# RPyC
+# Setup on slave EV3: https://ev3dev-lang.readthedocs.io/projects/python-ev3dev/en/stable/rpyc.html
+# Create a RPyC connection to the remote ev3dev device.
+# Use the hostname or IP address of the ev3dev device.
+# If this fails, verify your IP connectivty via ``ping X.X.X.X``
+logger.info("Connecting RPyC to {}...".format(REMOTE_HOST))
+# change this IP address for your slave EV3 brick
+conn = rpyc.classic.connect(REMOTE_HOST)
+# remote_ev3 = conn.modules['ev3dev.ev3']
+remote_power_mod = conn.modules['ev3dev2.power']
+remote_motor = conn.modules['ev3dev2.motor']
+remote_led = conn.modules['ev3dev2.led']
+logger.info("RPyC started succesfully")
+
+# Gamepad
+# If bluetooth is not available, check https://github.com/ev3dev/ev3dev/issues/1314
+logger.info("Connecting wireless controller...")
+gamepad = InputDevice(evdev.list_devices()[0])
+if gamepad.name != 'Wireless Controller':
+    logger.error('Failed to connect to wireless controller')
+    sys.exit(1)
+
+# LEDs
+leds = Leds()
+remote_leds = remote_led.Leds()
+
+# Power
+power = PowerSupply(name_pattern='*ev3*')
+remote_power = remote_power_mod.PowerSupply(name_pattern='*ev3*')
+
+# Sound
+# sound = Sound()
+
+# Primary EV3
+# Sensors
+try:
+    color_sensor = ColorSensor(INPUT_1)
+    color_sensor.mode = ColorSensor.MODE_COL_COLOR
+    logger.info("Color sensor detected!")
+except DeviceNotFound:
+    logger.info("Color sensor not detected (primary EV3, input 1) - running without it...")
+    color_sensor = False
+
+try:
+    shoulder_touch = TouchSensor(INPUT_3)
+    logger.info("Shoulder touch sensor detected!")
+except DeviceNotFound:
+    logger.info("Shoulder touch sensor not detected (primary EV3, input 3) - running without it...")
+    shoulder_touch = False
+
+try:
+    elbow_touch = TouchSensor(INPUT_4)
+    logger.info("Elbow touch sensor detected!")
+except DeviceNotFound:
+    logger.info("Elbow touch sensor not detected (primary EV3, input 4) - running without it...")
+    elbow_touch = False
+
+# Motors
+waist_motor = LargeMotor(OUTPUT_A)
+shoulder_motors = MoveTank(OUTPUT_B, OUTPUT_C)
+elbow_motor = LargeMotor(OUTPUT_D)
+
+# Secondary EV3
+# Motors
+roll_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_A)
+pitch_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_B)
+pitch_motor.stop_action = remote_motor.MediumMotor.STOP_ACTION_COAST
+spin_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_C)
+
+try:
+    grabber_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_D)
+    grabber_motor.stop_action = remote_motor.MediumMotor.STOP_ACTION_COAST
+    logger.info("Grabber motor detected!")
+except DeviceNotFound:
+    logger.info("Grabber motor not detected (secondary EV3, port D) - running without it...")
+    grabber_motor = False
+
+
+# Not sure why but resetting all motors before doing anything else seems to improve reliability
+reset_motors()
+
+
+# Variables for stick input
+shoulder_speed = 0
+elbow_speed = 0
+
+# Variables for button input
+waist_left = False
+waist_right = False
+roll_left = False
+roll_right = False
+pitch_up = False
+pitch_down = False
+spin_left = False
+spin_right = False
+grabber_open = False
+grabber_close = False
+
+# We are running!
+running = True
+
+def log_power_info():
+    logger.info('Local battery power: {}V / {}A'.format(round(power.measured_volts,2 ), round(power.measured_amps, 2)))
+    logger.info('Remote battery power: {}V / {}A'.format(round(remote_power.measured_volts, 2), round(remote_power.measured_amps, 2)))
+
+
+speed_modifier = 0
+def calculate_speed(speed, max=100):
+    if speed_modifier == 0:
+        return min(speed, max)
+    elif speed_modifier == -1:  # dpad up
+        return min(speed * 1.5, max)
+    elif speed_modifier == 1:  # dpad down
+        return min(speed / 1.5, max)
+    
+
+waist_target_color = 0
+aligning_waist = False
+def align_waist_to_color(waist_target_color):
+    if waist_target_color == -1:
+        target_color = ColorSensor.COLOR_RED
+    elif waist_target_color == 1:
+        target_color = ColorSensor.COLOR_BLUE
+    else:
+        # if someone asks us to move to an unknown/unmapped
+        # color, just make this a noop.
+        return
+    
+    # Set a flag for the MotorThread to prevent stopping the waist motor while
+    # we're trying to align it
+    global aligning_waist
+    aligning_waist = True
+
+    # If we're not on the correct color, start moving but make sure there's a 
+    # timeout to prevent trying forever.
+    if color_sensor.color != target_color:
+        logger.info('Moving to color {}...'.format(target_color))
+        waist_motor.on(NORMAL_SPEED)
+
+        max_iterations = 100
+        iterations = 0
+        while color_sensor.color != target_color:
+            # wait a bit between checks. Ideally there would be a wait_for_color() 
+            # method or something, but as far as I know that's not possible with the 
+            # current libraries, so we do it like this.
+            time.sleep(0.1)
+            
+            # prevent running forver
+            iterations += 1
+            if iterations >= max_iterations:
+                logger.info('Failed to align waist to requested color {}'.format(target_color))
+                break
+        
+        # we're either aligned or reached a timeout. Stop moving.
+        waist_motor.stop()
+
+    # update flag for MotorThead so waist control works again.
+    aligning_waist = False
+
+
+def clean_shutdown(signal_received=None, frame=None):
+    """ make sure all motors are stopped when stopping this script """
+    logger.info('Shutting down...')
+
+    global running
+    running = False
+    
+    logger.info('waist..')
+    waist_motor.stop()
+    logger.info('shoulder..')
+    shoulder_motors.stop()
+    logger.info('elbow..')
+    elbow_motor.stop()
+    logger.info('pitch..')
+    # For some reason the pitch motor sometimes gets stuck here, and a reset helps?
+    # pitch_motor.reset()
+    pitch_motor.stop()
+    logger.info('roll..')
+    roll_motor.stop()
+    logger.info('spin..')
+    spin_motor.stop()
+
+    if grabber_motor:
+        logger.info('grabber..')
+        grabber_motor.stop()
+
+    # See https://github.com/gvalkov/python-evdev/issues/19 if this raises exceptions, but it seems 
+    # stable now.
+    gamepad.close()
+
+    logger.info('Shutdown completed.')
+    sys.exit(0)
+
+
+class WaistAlignThread(threading.Thread):
+    def __init__(self):
+        threading.Thread.__init__(self)
+
+    def run(self):
+        logger.info("WaistAlignThread running!")
+        while running:
+            if waist_target_color != 0 and not aligning_waist:
+                align_waist_to_color(waist_target_color)
+            time.sleep(2)  # prevent performance impact, drawback is you need to hold the button for a bit before it registers
+        logger.info("WaistAlignThread stopping!")
+
+
+class MotorThread(threading.Thread):
+    def __init__(self):
+        threading.Thread.__init__(self)
+
+    def run(self):
+        logger.info("MotorThread running!")
+        # os.system('setfont Lat7-Terminus12x6')
+        leds.set_color("LEFT", "BLACK")
+        leds.set_color("RIGHT", "BLACK")
+        remote_leds.set_color("LEFT", "BLACK")
+        remote_leds.set_color("RIGHT", "BLACK")
+        # sound.play_song((('C4', 'e'), ('D4', 'e'), ('E5', 'q')))
+        leds.set_color("LEFT", "GREEN")
+        leds.set_color("RIGHT", "GREEN")
+        remote_leds.set_color("LEFT", "GREEN")
+        remote_leds.set_color("RIGHT", "GREEN")
+
+        logger.info("Starting main loop...")
+        while running:
+            # Proportional control
+            if shoulder_speed != 0:
+                shoulder_motors.on(shoulder_speed, shoulder_speed)
+            elif shoulder_motors.is_running:
+                shoulder_motors.stop()
+            
+            # Proportional control
+            if elbow_speed != 0:
+                elbow_motor.on(elbow_speed)
+            elif elbow_motor.is_running:
+                elbow_motor.stop()
+
+            # on/off control
+            if not aligning_waist:
+                if waist_left:
+                    waist_motor.on(calculate_speed(-SLOW_SPEED))
+                elif waist_right:
+                    waist_motor.on(calculate_speed(SLOW_SPEED))
+                elif waist_motor.is_running:
+                    waist_motor.stop()
+
+            # on/off control
+            if roll_left:
+                roll_motor.on(calculate_speed(-SLOW_SPEED))
+            elif roll_right:
+                roll_motor.on(calculate_speed(SLOW_SPEED))
+            elif roll_motor.is_running:
+                roll_motor.stop()
+
+            # on/off control
+            #
+            # Pitch affects grabber as well, but to a lesser degree. We could improve this 
+            # in the future to adjust grabber based on pitch movement as well.
+            if pitch_up:
+                pitch_motor.on(calculate_speed(VERY_SLOW_SPEED))
+            elif pitch_down:
+                pitch_motor.on(calculate_speed(-VERY_SLOW_SPEED))
+            elif pitch_motor.is_running:
+                pitch_motor.stop()
+
+            # on/off control
+            #
+            # If we keep spinning, the grabber motor can get stuck because it remains stationary
+            # but is forced to move around the worm gear. We need to adjust it while spinning.
+            # 
+            # spin motor: 7:1 (=23.6RPM) 
+            # grabber motor: 1:1 (=165RPM) untill the worm gear which we need to keep steady
+            # 
+            # So, I think the grabber_motor needs to move 7 times slower than the spin_motor 
+            # to maintain it's position.
+            # 
+            # NOTE: I'm using knob wheels to control the grabber, which is not smoothly rotating 
+            # at these low speeds. Therefor the grabber has to move a bit quicker for me, but I 
+            # think when using regular gears the 7 ratio should be sufficient.
+            # NOTE: Yes, with regular gears the calculated ratio is correct!
+            GRABBER_SPIN_RATIO = 7
+            if spin_left:
+                spin_motor_speed = calculate_speed(-SLOW_SPEED)
+                spin_motor.on(spin_motor_speed)
+                if grabber_motor:
+                    # determine grabber_motor speed based on spin_motor speed & invert
+                    grabber_spin_sync_speed = (spin_motor_speed / GRABBER_SPIN_RATIO) * -1
+                    grabber_motor.on(grabber_spin_sync_speed, False)
+                    # logger.info('Spin motor {}, grabber {}'.format(spin_motor_speed, grabber_spin_sync_speed))
+            elif spin_right:
+                spin_motor_speed = calculate_speed(SLOW_SPEED)
+                spin_motor.on(spin_motor_speed)
+                if grabber_motor:
+                    # determine grabber_motor speed based on spin_motor speed & invert
+                    grabber_spin_sync_speed = (spin_motor_speed / GRABBER_SPIN_RATIO) * -1
+                    grabber_motor.on(grabber_spin_sync_speed, False)
+                    # logger.info('Spin motor {}, grabber {}'.format(spin_motor_speed, grabber_spin_sync_speed))
+            elif spin_motor.is_running:
+                spin_motor.stop()
+                if grabber_motor:
+                    grabber_motor.stop()
+
+            # on/off control - can only control this directly if we're not currently spinning
+            elif grabber_motor:
+                if grabber_open:
+                    grabber_motor.on(calculate_speed(NORMAL_SPEED), False)
+                elif grabber_close:
+                    grabber_motor.on(calculate_speed(-NORMAL_SPEED), False)
+                elif grabber_motor.is_running:
+                    grabber_motor.stop()
+        
+        logger.info("MotorThread stopping!")
+
+
+# Ensure clean shutdown on CTRL+C
+signal(SIGINT, clean_shutdown)
+
+log_power_info()
+
+# Main motor control thread
+motor_thread = MotorThread()
+motor_thread.setDaemon(True)
+motor_thread.start()
+
+# We only need the WaistAlignThread if we detected a color sensor
+if color_sensor:
+    waist_align_thread = WaistAlignThread()
+    waist_align_thread.setDaemon(True)
+    waist_align_thread.start()
+
+# Handle gamepad input
+for event in gamepad.read_loop():  # this loops infinitely
+    if event.type == 3:  # stick input
+        if event.code == 0:  # Left stick X-axis
+            shoulder_speed = scale_stick(event.value, deadzone=JOYSTICK_DEADZONE, invert=True)
+        elif event.code == 3:  # Right stick X-axis
+            elbow_speed = scale_stick(event.value, deadzone=JOYSTICK_DEADZONE)
+        elif event.code == 17:  # dpad up/down
+            speed_modifier = event.value
+        elif event.code == 16:  # dpad left/right
+            waist_target_color = event.value
+
+    elif event.type == 1:  # button input
+
+        if event.code == 310:  # L1
+            if event.value == 1:
+                waist_right = False
+                waist_left = True
+            elif event.value == 0:
+                waist_left = False
+
+        elif event.code == 311:  # R1
+            if event.value == 1:
+                waist_left = False
+                waist_right = True
+            elif event.value == 0:
+                waist_right = False
+
+        elif event.code == 308:  # Square
+            if event.value == 1:
+                roll_right = False
+                roll_left = True
+            elif event.value == 0:
+                roll_left = False
+
+        elif event.code == 305:  # Circle
+            if event.value == 1:
+                roll_left = False
+                roll_right = True
+            elif event.value == 0:
+                roll_right = False
+
+        elif event.code == 307:  # Triangle
+            if event.value == 1:
+                pitch_down = False
+                pitch_up = True
+            elif event.value == 0:
+                pitch_up = False
+
+        elif event.code == 304:  # X
+            if event.value == 1:
+                pitch_up = False
+                pitch_down = True
+            elif event.value == 0:
+                pitch_down = False
+
+        elif event.code == 312:  # L2
+            if event.value == 1:
+                spin_right = False
+                spin_left = True
+            elif event.value == 0:
+                spin_left = False
+
+        elif event.code == 313:  # R2
+            if event.value == 1:
+                spin_left = False
+                spin_right = True
+            elif event.value == 0:
+                spin_right = False
+
+        elif event.code == 317:  # L3
+            if event.value == 1:
+                grabber_close = False
+                grabber_open = True
+            elif event.value == 0:
+                grabber_open = False
+
+        elif event.code == 318:  # R3
+            if event.value == 1:
+                grabber_open = False
+                grabber_close = True
+            elif event.value == 0:
+                grabber_close = False
+
+        elif event.code == 314 and event.value == 1:  # Share
+            # debug info
+            log_power_info()
+         
+        elif event.code == 315 and event.value == 1:  # Options
+            # debug info
+            logger.info('Elbow motor state: {}'.format(elbow_motor.state))
+            logger.info('Elbow motor duty cycle: {}'.format(elbow_motor.duty_cycle))
+            logger.info('Elbow motor speed: {}'.format(elbow_motor.speed))
+
+        elif event.code == 316 and event.value == 1:  # PS
+            # stop control loop
+            running = False
+
+            # Move motors to default position
+            # motors_to_center()
+
+            # sound.play_song((('E5', 'e'), ('C4', 'e')))
+            leds.set_color("LEFT", "BLACK")
+            leds.set_color("RIGHT", "BLACK")
+            remote_leds.set_color("LEFT", "BLACK")
+            remote_leds.set_color("RIGHT", "BLACK")
+
+            time.sleep(1)  # Wait for the motor thread to finish
+            break
+
+clean_shutdown()

ev3dev_remote_control_protype1/robot-arm.py

@@ -0,0 +1,350 @@
+#!/usr/bin/env python3
+__author__ = 'Nino Guba'
+
+import os
+import sys
+import logging
+
+import evdev
+import threading
+import time
+
+from evdev import InputDevice, categorize, ecodes
+from ev3dev2.led import Leds
+from ev3dev2.sound import Sound
+from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, LargeMotor, MediumMotor, MoveTank
+
+import rpyc
+
+# Create a RPyC connection to the remote ev3dev device.
+# Use the hostname or IP address of the ev3dev device.
+# If this fails, verify your IP connectivty via ``ping X.X.X.X``
+conn = rpyc.classic.connect('1.1.1.1') #change this IP address for your slave EV3 brick
+#remote_ev3 = conn.modules['ev3dev.ev3']
+remote_motor = conn.modules['ev3dev2.motor']
+remote_led = conn.modules['ev3dev2.led']
+
+logging.basicConfig(level=logging.INFO, stream=sys.stdout, format='%(message)s')
+logging.getLogger().addHandler(logging.StreamHandler(sys.stderr))
+logger = logging.getLogger(__name__)
+
+## Some helpers ##
+def scale(val, src, dst):
+    return (float(val - src[0]) / (src[1] - src[0])) * (dst[1] - dst[0]) + dst[0]
+
+def scale_stick(value):
+    return scale(value,(0,255),(-1000,1000))
+
+## Initializing ##
+logger.info("Finding wireless controller...")
+devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()]
+#for device in devices:
+#    logger.info("{}".format(device.name))
+
+ps4gamepad = devices[0].fn # PS4 gamepad
+#ps4motion = devices[1].fn # PS4 accelerometer
+#ps4touchpad = devices[2].fn # PS4 touchpad
+
+gamepad = evdev.InputDevice(ps4gamepad)
+
+leds = Leds()
+remote_leds = remote_led.Leds()
+sound = Sound()
+
+waist_motor = LargeMotor(OUTPUT_A)
+shoulder_control1 = LargeMotor(OUTPUT_B)
+shoulder_control2 = LargeMotor(OUTPUT_C)
+shoulder_motor = MoveTank(OUTPUT_B,OUTPUT_C)
+elbow_motor = LargeMotor(OUTPUT_D)
+roll_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_A)
+pitch_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_B)
+spin_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_C)
+grabber_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_D)
+
+waist_motor.position = 0
+shoulder_control1.position = 0
+shoulder_control2.position = 0
+shoulder_motor.position = 0
+elbow_motor.position = 0
+roll_motor.position = 0
+pitch_motor.position = 0
+spin_motor.position = 0
+grabber_motor.position = 0
+
+waist_ratio = 7.5
+shoulder_ratio = 7.5
+elbow_ratio = 5
+roll_ratio = 7
+pitch_ratio = 5
+spin_ratio = 7
+grabber_ratio = 24
+
+waist_max = 360
+waist_min = -360
+shoulder_max = -60 #-75 max without grabber
+shoulder_min = 50 #65 min without grabber
+elbow_max = -175
+elbow_min = 0
+roll_max = 180
+roll_min = -180
+pitch_max = 80
+pitch_min = -90
+spin_max = -360
+spin_min = 360
+grabber_max = -68
+grabber_min = 0
+
+full_speed = 100
+fast_speed = 75
+normal_speed = 50
+slow_speed = 25
+
+forward_speed = 0
+forward_side_speed = 0
+
+upward_speed = 0
+upward_side_speed = 0
+
+turning_left = False
+turning_right = False
+
+roll_left = False
+roll_right = False
+
+pitch_up = False
+pitch_down = False
+
+spin_left = False
+spin_right = False
+
+grabber_open = False
+grabber_close = False
+
+running = True
+
+class MotorThread(threading.Thread):
+    def __init__(self):
+        threading.Thread.__init__(self)
+
+    def run(self):
+        logger.info("Engine running!")
+        os.system('setfont Lat7-Terminus12x6')
+        leds.set_color("LEFT", "BLACK")
+        leds.set_color("RIGHT", "BLACK")
+        remote_leds.set_color("LEFT", "BLACK")
+        remote_leds.set_color("RIGHT", "BLACK")
+        sound.play_song((('C4', 'e'), ('D4', 'e'), ('E5', 'q')))
+        leds.set_color("LEFT", "GREEN")
+        leds.set_color("RIGHT", "GREEN")
+        remote_leds.set_color("LEFT", "GREEN")
+        remote_leds.set_color("RIGHT", "GREEN")
+
+        """
+        #Calibrations
+        waist_motor.on_to_position(fast_speed,(waist_max/2)*waist_ratio,True,True) #Right
+        waist_motor.on_to_position(fast_speed,(waist_min/2)*waist_ratio,True,True) #Left
+        waist_motor.on_to_position(fast_speed,0,True,True)
+
+        shoulder_motor.on_for_degrees(normal_speed,normal_speed,shoulder_max*shoulder_ratio,True,True) #Forward
+        shoulder_motor.on_for_degrees(normal_speed,normal_speed,-shoulder_max*shoulder_ratio,True,True) 
+        shoulder_motor.on_for_degrees(normal_speed,normal_speed,shoulder_min*shoulder_ratio,True,True) #Backward
+        shoulder_motor.on_for_degrees(normal_speed,normal_speed,-shoulder_min*shoulder_ratio,True,True)
+
+        elbow_motor.on_to_position(normal_speed,elbow_max*elbow_ratio,True,True) #Up
+        elbow_motor.on_to_position(normal_speed,elbow_min*elbow_ratio,True,True) #Down
+
+        roll_motor.on_to_position(normal_speed,(roll_max/2)*roll_ratio,True,True) #Right
+        roll_motor.on_to_position(normal_speed,(roll_min/2)*roll_ratio,True,True) #Left
+        roll_motor.on_to_position(normal_speed,0,True,True)
+
+        pitch_motor.on_to_position(normal_speed,pitch_max*pitch_ratio,True,True) #Up
+        pitch_motor.on_to_position(normal_speed,pitch_min*pitch_ratio,True,True) #Down
+        pitch_motor.on_to_position(normal_speed,0,True,True)
+
+        spin_motor.on_to_position(normal_speed,(spin_max/2)*spin_ratio,True,True) #Right
+        spin_motor.on_to_position(normal_speed,(spin_min/2)*spin_ratio,True,True) #Left
+        spin_motor.on_to_position(normal_speed,0,True,True)
+
+        grabber_motor.on_to_position(normal_speed,grabber_max*grabber_ratio,True,True) #Close
+        grabber_motor.on_to_position(normal_speed,grabber_min*grabber_ratio,True,True) #Open
+
+        #Reach
+        elbow_motor.on_to_position(normal_speed,-90*elbow_ratio,True,False) #Up
+        shoulder_motor.on_for_degrees(normal_speed,normal_speed,shoulder_max*shoulder_ratio,True,True) #Forward
+        waist_motor.on_to_position(fast_speed,waist_max*waist_ratio,True,True) #Right
+        elbow_motor.on_to_position(normal_speed,elbow_max*elbow_ratio,True,False) #Up
+        shoulder_motor.on_for_degrees(normal_speed,normal_speed,75*shoulder_ratio,True,True) #Reset
+        elbow_motor.on_to_position(normal_speed,-90*elbow_ratio,True,False) #Down
+        shoulder_motor.on_for_degrees(normal_speed,normal_speed,65*shoulder_ratio,True,True) #Backward
+        waist_motor.on_to_position(fast_speed,0,True,True) #Center
+        elbow_motor.on_to_position(slow_speed,elbow_min*elbow_ratio,True,True) #Reset
+        shoulder_motor.on_for_degrees(slow_speed,slow_speed,-65*shoulder_ratio,True,True) #Reset
+        """
+
+        while running:
+            if forward_speed > 0 and shoulder_control1.position > ((shoulder_max*shoulder_ratio)+100):
+                shoulder_motor.on( -normal_speed,-normal_speed )
+            elif forward_speed < 0 and shoulder_control1.position < ((shoulder_min*shoulder_ratio)-100):
+                shoulder_motor.on( normal_speed,normal_speed )
+            else:
+                shoulder_motor.stop()
+
+            if upward_speed > 0:
+                elbow_motor.on_to_position(normal_speed,elbow_max*elbow_ratio,True,False) #Up
+            elif upward_speed < 0:
+                elbow_motor.on_to_position(normal_speed,elbow_min*elbow_ratio,True,False) #Down
+            else:
+                elbow_motor.stop()
+
+            if turning_left:
+                waist_motor.on_to_position(fast_speed,waist_min*waist_ratio,True,False) #Left
+            elif turning_right:
+                waist_motor.on_to_position(fast_speed,waist_max*waist_ratio,True,False) #Right
+            else:
+                waist_motor.stop()
+
+            if roll_left:
+                roll_motor.on_to_position(normal_speed,roll_min*roll_ratio,True,False) #Left
+            elif roll_right:
+                roll_motor.on_to_position(normal_speed,roll_max*roll_ratio,True,False) #Right
+            else:
+                roll_motor.stop()
+
+            if pitch_up:
+                pitch_motor.on_to_position(normal_speed,pitch_max*pitch_ratio,True,False) #Up
+            elif pitch_down:
+                pitch_motor.on_to_position(normal_speed,pitch_min*pitch_ratio,True,False) #Down
+            else:
+                pitch_motor.stop()
+
+            if spin_left:
+                spin_motor.on_to_position(normal_speed,spin_min*spin_ratio,True,False) #Left
+            elif spin_right:
+                spin_motor.on_to_position(normal_speed,spin_max*spin_ratio,True,False) #Right
+            else:
+                spin_motor.stop()
+
+            if grabber_open:
+                grabber_motor.on_to_position(normal_speed,grabber_max*grabber_ratio,True,True) #Close
+                grabber_motor.stop()
+            elif grabber_close:
+                grabber_motor.on_to_position(normal_speed,grabber_min*grabber_ratio,True,True) #Open
+                grabber_motor.stop()
+            else:
+                grabber_motor.stop()
+
+        waist_motor.stop()
+        shoulder_motor.stop()
+        elbow_motor.stop()
+        roll_motor.stop()
+        pitch_motor.stop()
+        spin_motor.stop()
+        grabber_motor.stop()
+
+motor_thread = MotorThread()
+motor_thread.setDaemon(True)
+motor_thread.start()
+
+for event in gamepad.read_loop():   #this loops infinitely
+    if event.type == 3:
+        if event.code == 0: #Left stick X-axis
+            forward_speed = scale_stick(event.value)
+        #if event.code == 1: #Left stick Y-axis
+        #    forward_side_speed = scale_stick(event.value)
+        if event.code == 3: #Right stick X-axis
+            upward_speed = -scale_stick(event.value)
+        #if event.code == 4: #Right stick Y-axis
+        #    upward_side_speed = scale_stick(event.value)
+        if forward_speed < 100 and forward_speed > -100:
+            forward_speed = 0
+        if upward_speed < 100 and upward_speed > -100:
+            upward_speed = 0
+        if forward_side_speed < 100 and forward_side_speed > -100:
+            forward_side_speed = 0
+        if upward_side_speed < 100 and upward_side_speed > -100:
+            upward_side_speed = 0
+
+    if event.type == 1 and event.code == 310 and event.value == 1:  #L1
+        turning_left = True
+    elif event.type == 1 and event.code == 310 and event.value == 0:
+        turning_left = False
+
+    if event.type == 1 and event.code == 311 and event.value == 1:  #R1
+        turning_right = True
+    elif event.type == 1 and event.code == 311 and event.value == 0:
+        turning_right = False
+
+    if event.type == 1 and event.code == 308 and event.value == 1:  #Square
+        roll_left = True
+    elif event.type == 1 and event.code == 308 and event.value == 0:
+        roll_left = False
+
+    if event.type == 1 and event.code == 305 and event.value == 1:  #Circle
+        roll_right = True
+    elif event.type == 1 and event.code == 305 and event.value == 0:
+        roll_right = False
+
+    if event.type == 1 and event.code == 307 and event.value == 1:  #Triangle
+        pitch_up = True
+    elif event.type == 1 and event.code == 307 and event.value == 0:
+        pitch_up = False
+
+    if event.type == 1 and event.code == 304 and event.value == 1:  #X
+        pitch_down = True
+    elif event.type == 1 and event.code == 304 and event.value == 0:
+        pitch_down = False
+
+    if event.type == 1 and event.code == 312 and event.value == 1:  #L2
+        spin_left = True
+    elif event.type == 1 and event.code == 312 and event.value == 0:
+        spin_left = False
+
+    if event.type == 1 and event.code == 313 and event.value == 1:  #R2
+        spin_right = True
+    elif event.type == 1 and event.code == 313 and event.value == 0:
+        spin_right = False
+
+    if event.type == 1 and event.code == 318 and event.value == 1:  #R3
+        if grabber_open:
+            grabber_open = False
+            grabber_close = True
+        else:
+            grabber_open = True
+            grabber_close = False
+
+    # if event.type == 1 and event.code == 314 and event.value == 1:  #Share
+    #     #Demo
+
+    if event.type == 1 and event.code == 315 and event.value == 1:  #Options
+        #Reset
+        roll_motor.on_to_position(normal_speed,0,True,False)
+        pitch_motor.on_to_position(normal_speed,0,True,False)
+        spin_motor.on_to_position(normal_speed,0,True,False)
+        grabber_motor.on_to_position(normal_speed,0,True,True)
+        elbow_motor.on_to_position(slow_speed,0,True,False)
+        shoulder_control1.on_to_position(slow_speed,0,True,False)
+        shoulder_control2.on_to_position(slow_speed,0,True,False)
+        waist_motor.on_to_position(fast_speed,0,True,True)
+
+    if event.type == 1 and event.code == 316 and event.value == 1:  #PS
+        logger.info("Engine stopping!")
+        running = False
+
+        #Reset
+        roll_motor.on_to_position(normal_speed,0,True,False)
+        pitch_motor.on_to_position(normal_speed,0,True,False)
+        spin_motor.on_to_position(normal_speed,0,True,False)
+        grabber_motor.on_to_position(normal_speed,0,True,True)
+        elbow_motor.on_to_position(slow_speed,0,True,False)
+        shoulder_control1.on_to_position(slow_speed,0,True,False)
+        shoulder_control2.on_to_position(slow_speed,0,True,False)
+        waist_motor.on_to_position(fast_speed,0,True,True)
+
+        sound.play_song((('E5', 'e'), ('C4', 'e')))
+        leds.set_color("LEFT", "BLACK")
+        leds.set_color("RIGHT", "BLACK")
+        remote_leds.set_color("LEFT", "BLACK")
+        remote_leds.set_color("RIGHT", "BLACK")
+
+        time.sleep(1) # Wait for the motor thread to finish
+        break