StanislawSkulimowski/EV3_robot_arm_2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

test

Browse Source
This commit is contained in:
stachir
2026-06-10 10:45:16 +02:00
parent a317c4c69d
commit 6d3196acce
6 changed files with 913 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
ev3dev_remote_control_protype1/.gitignore vendored Normal file
View File

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

21
ev3dev_remote_control_protype1/LICENSE Normal file
View File

@@ -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.

3
ev3dev_remote_control_protype1/README.md Normal file
View File

@@ -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/

16
ev3dev_remote_control_protype1/math_helper.py Normal file
View File

@@ -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

518
ev3dev_remote_control_protype1/remote_control.py Normal file
View File

@@ -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()

350
ev3dev_remote_control_protype1/robot-arm.py Normal file
View File

@@ -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