From 6d3196acceedacbed94cb369594c1a30c45a28f6 Mon Sep 17 00:00:00 2001 From: stachir Date: Wed, 10 Jun 2026 10:45:16 +0200 Subject: [PATCH] test --- ev3dev_remote_control_protype1/.gitignore | 5 + ev3dev_remote_control_protype1/LICENSE | 21 + ev3dev_remote_control_protype1/README.md | 3 + ev3dev_remote_control_protype1/math_helper.py | 16 + .../remote_control.py | 518 ++++++++++++++++++ ev3dev_remote_control_protype1/robot-arm.py | 350 ++++++++++++ 6 files changed, 913 insertions(+) create mode 100644 ev3dev_remote_control_protype1/.gitignore create mode 100644 ev3dev_remote_control_protype1/LICENSE create mode 100644 ev3dev_remote_control_protype1/README.md create mode 100644 ev3dev_remote_control_protype1/math_helper.py create mode 100644 ev3dev_remote_control_protype1/remote_control.py create mode 100644 ev3dev_remote_control_protype1/robot-arm.py diff --git a/ev3dev_remote_control_protype1/.gitignore b/ev3dev_remote_control_protype1/.gitignore new file mode 100644 index 0000000..eba9a18 --- /dev/null +++ b/ev3dev_remote_control_protype1/.gitignore @@ -0,0 +1,5 @@ +__pycache__/ +*.pyc +.coverage +htmlcov +.vscode \ No newline at end of file diff --git a/ev3dev_remote_control_protype1/LICENSE b/ev3dev_remote_control_protype1/LICENSE new file mode 100644 index 0000000..769d906 --- /dev/null +++ b/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. diff --git a/ev3dev_remote_control_protype1/README.md b/ev3dev_remote_control_protype1/README.md new file mode 100644 index 0000000..3ab695d --- /dev/null +++ b/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/ diff --git a/ev3dev_remote_control_protype1/math_helper.py b/ev3dev_remote_control_protype1/math_helper.py new file mode 100644 index 0000000..0be9381 --- /dev/null +++ b/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 diff --git a/ev3dev_remote_control_protype1/remote_control.py b/ev3dev_remote_control_protype1/remote_control.py new file mode 100644 index 0000000..54dde59 --- /dev/null +++ b/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() diff --git a/ev3dev_remote_control_protype1/robot-arm.py b/ev3dev_remote_control_protype1/robot-arm.py new file mode 100644 index 0000000..c43e875 --- /dev/null +++ b/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