LEGO EV3 Python - establishing 2 bricks bluetooth communication
When I saw Akiyuki's robotic arm pure arm, GBC and new type of arm, I really wanted to build it myself. As I possess 2 LEGO sets Mindstorms-EV3 31313, I thought I had everything I need. The process of setting up one Python piece of code, which would control all 6 motors (3x medium + 3x large) on both bricks took much longer than I thought. So I decided to write this tutorial in order to help others.
You will need:
- 2 sets of LEGO Mindstorms-EV3 31313
- 2 micro SDHC cards - size between 4 GB and 32 GB
- 12 AA baterries or any other power source to power both your EV3 bricks
- 1 extra LEGO EV3 medium motor in case you want to build Akiyuki's arm
- extra set of EV3 long cabels in case you want to build Akiyuki's arm
The long cabels can be home made by using the following links:
I will recap the following ev3dev.org guidelines:
- Download the latest ev3dev image file ev3dev-stretch-ev3-generic.zip.
- Flash both SD cards:
Download and install Etcher from their website.
Use it to flash both cards (one after another).
Follow www.ev3dev.org/docs/getting-started further: Insert the cards in your EV3 bricks and boot them. After a while brick will flash with a green light.
Follow connecting-to-the-internet-via-usb to get your EV3 bricks connected to the Internet.
Special trick comes in step 9 of the tutorial:In the window that opens, select the Sharing tab (1) and check the box that says Allow other network users to connect through this computer's Internet connection (2). screenshot
You will find out, that by switching betweem the 2 EV3 bricks, you will not be able to do this for the second EV3. You FIRST have to undo sharing on the first brick and then you will be able to do it on the second brick.
Connect to the EV3 brick via Putty program - follow those instructions.
Use username "robot" and password "maker".
Execute sudo /usr/sbin/ev3dev-config to change your hostname.
I used ev3dev01 and ev3dev02.
Very precious tutorial here!
You must be connected to the Internet first (see step 5 above) and connected to your brick (step 6).
Installed rfkill
============================================
sudo apt-get update;sudo apt-get upgrade
============================================
sudo apt-get install rfkill
============================================
/usr/bin/hciattach -t 30 /dev/ttyS2 any 2000000 flow nosleep
============================================
sudo rfkill list all
============================================
0: phy0: Wireless LAN
Soft blocked: no
Hard blocked: no
1: hci0: Bluetooth
Soft blocked: yes
Hard blocked: no
Unblocked the hci0 device:
============================================
sudo rfkill unblock 1
sudo rfkill list all
============================================
0: phy0: Wireless LAN
Soft blocked: no
Hard blocked: no
1: hci0: Bluetooth
Soft blocked: no
Hard blocked: no
Rebooted the EV3
After the reboot I checked "systemctl status ev3-bluetooth" and it came up with:
You can use BlueTooth to connect your bricks, but I finally settleted for USB and "daisy-chaining":
Use the same cabel provided by LEGO 13131 and connect the 2 EV3 bricks in the onlyway it is possible.
Mind you: you can (and will have to) connect the second cabel to connect the other EV3 brick to your computer.
On one brick ("first"), set "tethering" via the USB cabel (WIRED); follow using-usb-tethering tutorial.
Then connect the second to the first.
You may run into problems and won't be able to connect bricks to each other. In those cases:
disconnect cabel from your computer. On both EV3 bricks set "connect automatically". Reboot both bricks (while being "daisy chained" connected).
After a while, the connection will be established. THEN connect your computer to one of the bricks (the one with free port for it).
Install VISUAL STUDIO CODE https://code.visualstudio.com/. You will use it to type and publish your code to the EV3 brick.
Type your code in Visual Studio Code. Once ready for test, execute F5 (Start Debugging in menu). It will automatically offer you your detected device at the bottom right corner. Click "Connect Now" and your code will get executed.
Please, don't judge :)
#!/usr/bin/env python3
import math
from time import sleep
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, \
MoveTank, MediumMotor, SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM
from ev3dev2.display import Display
from ev3dev2.button import Button
from ev3dev2.sound import Sound
import ev3dev2.fonts as fonts
import rpyc
import socket
import logging
from sys import stderr, stdout, exit
from ev3dev2.console import Console
logging.basicConfig(level=logging.INFO, stream=stdout,
format='%(message)s')
logging.getLogger().addHandler(logging.StreamHandler(stderr))
logger = logging.getLogger(__name__)
logger.info("Starting...")
def frd(degrees):
return degrees * 7
def wbd(degrees):
return degrees * 10
def wrd(degrees):
return degrees * 5
def ebd(degrees):
return degrees * 12.5
def abd(degrees):
return degrees * 15
def ard(degrees):
return degrees * 10
def arcw(clockwise): # as seen from TOP
return -clockwise
def abcw(clockwise): # as seen from RIGHT
return clockwise
def ebcw(clockwise): # as seen from RIGHT
return clockwise
def wrcw(clockwise): # as seen from FRONT
return -clockwise
def wbcw(clockwise): # as seen from RIGHT
return clockwise
def frcw(clockwise): # as seen from FRONT
return -clockwise
def wb_comp_wr_d(degrees):
return 2 * degrees # 2 rotations to compensate wrist rotation by 360 deg
def wb_comp_wr_cw(clockwise):
return -clockwise
def fr_comp_wb_d(degrees):
return degrees # 1 rotation to compensate wrist bend by 360 deg
def fr_undo_wb_d(degrees):
return 8 * degrees # 1 rotation + extra 7 rotations (due to gear ration 1:7)
def fr_comp_wb_cw(clockwise):
return clockwise
def get_speed(all_degrees, degrees_index, max_speed):
max_degrees = max([abs(x) for x in all_degrees])
if len(all_degrees) == 1: # or max_degrees == min_degrees:
return max_speed
speed = (max_speed * abs(all_degrees[degrees_index])) / max_degrees
return speed # * mult
def rotate(motor, clockwise, all_degrees, degrees_index = 0, max_speed = 100, block = True):
motor.on_for_degrees(
speed = get_speed(all_degrees, degrees_index, max_speed),
degrees = clockwise * all_degrees[degrees_index],
brake = True,
block = block)
def rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, max_speed = 100, fingers_fixed = True, direction = 1, degrees = 90, fingers_direction = 1):
all_degrees = [
wrd(degrees),
wb_comp_wr_d(degrees),
fr_undo_wb_d(degrees) if fingers_fixed else fr_comp_wb_d(degrees)]
rotate(motor = wrist_rotor, clockwise = wrcw(direction), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed, block = False)
rotate(motor = wrist_bend, clockwise = wb_comp_wr_cw(direction), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = fingers_direction * fr_comp_wb_cw(direction), all_degrees = all_degrees, degrees_index = 2, max_speed = max_speed, block = False)
wrist_rotor.wait_while('running')
wrist_bend.wait_while('running')
fingers_rotor.wait_while('running')
def bend_wrist(wrist_bend, fingers_rotor, degrees = 90, direction = 1):
all_degrees = [wbd(degrees), fr_comp_wb_d(degrees)]
max_speed = 50
rotate(motor = wrist_bend, clockwise = wbcw(direction), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = -fr_comp_wb_cw(direction), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed, block = False)
wrist_bend.wait_while('running')
fingers_rotor.wait_while('running')
def bend_wrist_and_rotate(wrist_rotor, wrist_bend, fingers_rotor, direction = 1):
bend_wrist(wrist_bend, fingers_rotor, 90, direction)
max_speed = 50
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, max_speed, False, direction, 360)
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, max_speed, False, -direction, 360)
bend_wrist(wrist_bend, fingers_rotor, 90, -direction)
def rotate_and_move_arm(arm_rotor, arm_bend, elbow_bend, direction = 1, degrees = 45):
all_degrees = [ard(degrees), abd(degrees), ebd(degrees)]
max_speed = 50
rotate(motor = arm_rotor, clockwise = arcw(direction), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed, block = False)
rotate(motor = arm_bend, clockwise = -abcw(direction), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed, block = False)
rotate(motor = elbow_bend, clockwise = ebcw(direction), all_degrees = all_degrees, degrees_index = 2, max_speed = max_speed, block = False)
arm_rotor.wait_while('running')
arm_bend.wait_while('running')
elbow_bend.wait_while('running')
def extend_arm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1):
all_degrees = [abd(60), ebd(30), wbd(90), fr_comp_wb_d(90)]
max_speed = 40
rotate(motor = arm_bend, clockwise = abcw(direction), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed * 1.4, block = False)
rotate(motor = elbow_bend, clockwise = ebcw(direction), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed * 1.4, block = False)
rotate(motor = wrist_bend, clockwise = -wbcw(direction), all_degrees = all_degrees, degrees_index = 2, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = fr_comp_wb_cw(direction), all_degrees = all_degrees, degrees_index = 3, max_speed = max_speed, block = False)
arm_bend.wait_while('running')
elbow_bend.wait_while('running')
wrist_bend.wait_while('running')
fingers_rotor.wait_while('running')
def move_forearm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1, degrees = 45):
all_degrees = [ebd(degrees), wbd(degrees), fr_comp_wb_d(degrees)]
max_speed = 50
rotate(motor = elbow_bend, clockwise = ebcw(direction), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed * 1.4, block = False)
rotate(motor = wrist_bend, clockwise = -wbcw(direction), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = fr_comp_wb_cw(direction), all_degrees = all_degrees, degrees_index = 2, max_speed = max_speed, block = False)
wrist_bend.wait_while('running')
elbow_bend.wait_while('running')
fingers_rotor.wait_while('running')
def rotate_bent_arm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1, degrees = 45):
all_degrees = [ard(degrees), wrd(degrees), wb_comp_wr_d(degrees), fr_comp_wb_d(degrees)]
max_speed = 40
rotate(motor = arm_rotor, clockwise = arcw(direction), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed * 1.4, block = False)
rotate(motor = wrist_rotor, clockwise = wrcw(direction), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed, block = False)
rotate(motor = wrist_bend, clockwise = wb_comp_wr_cw(direction), all_degrees = all_degrees, degrees_index = 2, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = fr_comp_wb_cw(direction), all_degrees = all_degrees, degrees_index = 3, max_speed = max_speed, block = False)
fingers_rotor.wait_while('running')
arm_rotor.wait_while('running')
wrist_rotor.wait_while('running')
wrist_bend.wait_while('running')
UNIT = 7.97
ARM_LENGTH = 19 * UNIT
FOREARM_LENGTH = 19.925 * UNIT
def get_forearm_angle_real(arm_angle, adjust = 0):
return math.degrees(math.acos((math.sin(math.radians(arm_angle)) * ARM_LENGTH + adjust) / FOREARM_LENGTH))
def get_forearm_angle(arm_angle, adjust = 0):
return arm_angle - get_forearm_angle_real(arm_angle, adjust)
def get_height(alpha, real_beta):
return math.cos(math.radians(alpha)) * ARM_LENGTH + math.sin(math.radians(real_beta)) * FOREARM_LENGTH
def move_arm_diff_degrees(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, directions, degrees, max_speed):
if len(degrees) != 3:
logger.error("ERROR function move_arm_diff_degrees called with wrong list of degrees: 3 expected!")
return
if len(directions) != 3:
logger.error("ERROR function move_arm_diff_degrees called with wrong list of directions: 3 expected!")
return
all_degrees = [abd(degrees[0]), ebd(degrees[1]), wbd(degrees[2]), fr_comp_wb_d(degrees[2])]
rotate(motor = arm_bend, clockwise = abcw(directions[0]), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed * 1.4, block = False)
rotate(motor = elbow_bend, clockwise = ebcw(directions[1]), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed * 1.4, block = False)
rotate(motor = wrist_bend, clockwise = wbcw(directions[2]), all_degrees = all_degrees, degrees_index = 2, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = frcw(directions[2]), all_degrees = all_degrees, degrees_index = 3, max_speed = max_speed, block = False)
arm_bend.wait_while('running')
elbow_bend.wait_while('running')
wrist_bend.wait_while('running')
fingers_rotor.wait_while('running')
def straight_line(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor):
alpha_from = 60
alpha_to = 20
arm_bend.reset()
arm_bend.position = 0
elbow_bend.reset()
elbow_bend.position = 0
adjust = 0
beta_from = get_forearm_angle(alpha_from, adjust)
beta_from_real = get_forearm_angle_real(alpha_from, adjust)
max_speed = 50
degrees_from = [alpha_from, beta_from, beta_from_real]
logger.info("alpha-from: {0}, beta-from: {1}, real-beta-from: {2}".format(alpha_from, beta_from, beta_from_real))
move_arm_diff_degrees(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, directions = [-1,1,1], degrees = degrees_from, max_speed = max_speed)
sleep(3)
beta_to = get_forearm_angle(alpha_to, adjust)
beta_to_real = get_forearm_angle_real(alpha_to, adjust)
logger.info("alpha-to: {0}, beta-to: {1}, real-beta-to: {2}, computed: {3}".format(alpha_to, beta_to, beta_to_real, beta_to_real - beta_from_real))
logger.info("alpha-to-delta: {0}, beta-to-delta: {1}".format(alpha_from - alpha_to, beta_to - beta_from))
degrees_to = [alpha_from - alpha_to, beta_from - beta_to, beta_to_real - beta_from_real]
move_arm_diff_degrees(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, directions = [1, -1, 1], degrees = degrees_to, max_speed = max_speed)
sleep(3)
move_arm_diff_degrees(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, directions = [-1, 1, -1], degrees = degrees_to, max_speed = max_speed)
sleep(3)
move_arm_diff_degrees(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, directions = [1,-1,-1], degrees = degrees_from, max_speed = max_speed)
sleep(3)
def move_all(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction, all_degrees, max_speed):
rotate(motor = arm_rotor, clockwise = arcw(direction), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed * 1.4, block = False)
rotate(motor = arm_bend, clockwise = abcw(direction), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed * 1.4, block = False)
rotate(motor = elbow_bend, clockwise = ebcw(direction), all_degrees = all_degrees, degrees_index = 2, max_speed = max_speed * 1.4, block = False)
rotate(motor = wrist_rotor, clockwise = wrcw(direction), all_degrees = all_degrees, degrees_index = 3, max_speed = max_speed, block = False)
rotate(motor = wrist_bend, clockwise = -wbcw(direction), all_degrees = all_degrees, degrees_index = 4, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = -frcw(direction), all_degrees = all_degrees, degrees_index = 5, max_speed = max_speed, block = False)
arm_rotor.wait_while('running')
arm_bend.wait_while('running')
elbow_bend.wait_while('running')
wrist_rotor.wait_while('running')
wrist_bend.wait_while('running')
fingers_rotor.wait_while('running')
def draw_square(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor):
for motor in [arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor]:
motor.reset()
motor.position = 0
max_speed = 25
wr_ang = 47
wb_and = 61
all_degrees = [ard(45), abd(22.5), ebd(22.5), wrd(wr_ang), wbd(wb_and) + wb_comp_wr_d(wr_ang), frd(wb_and) - fr_comp_wb_d(wb_and)]
logger.info("all-degrees = {0}".format(all_degrees))
move_all(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1, all_degrees = all_degrees, max_speed = max_speed)
degrees = [-ard(45), -wrd(wr_ang), wbd(wb_and - 45) + wb_comp_wr_d(wr_ang), frd(45) - fr_comp_wb_d(wb_and - 45)]
logger.info("degrees = {0}".format(degrees))
logger.info("sum-deg: {0}, {1}, {2}".format(
all_degrees[3] + degrees[1],
all_degrees[4] + degrees[2],
all_degrees[5] + degrees[3]))
rotate(motor = arm_rotor, clockwise = arcw(1), all_degrees = degrees, degrees_index = 0, max_speed = max_speed * 1.4, block = False)
rotate(motor = wrist_rotor, clockwise = wrcw(1), all_degrees = degrees, degrees_index = 1, max_speed = max_speed, block = False)
rotate(motor = wrist_bend, clockwise = wbcw(1), all_degrees = degrees, degrees_index = 2, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = frcw(1), all_degrees = degrees, degrees_index = 3, max_speed = max_speed, block = False)
arm_rotor.wait_while('running')
wrist_rotor.wait_while('running')
wrist_bend.wait_while('running')
fingers_rotor.wait_while('running')
degrees2 = [-ard(45), -wrd(wr_ang), -wbd(wb_and - 45) + wb_comp_wr_d(wr_ang), fr_comp_wb_d(45) + frd(wb_and) - fr_comp_wb_d(wb_and)]
logger.info("degrees2 = {0}".format(degrees2))
logger.info("sum-deg: {0}, {1}, {2}".format(
all_degrees[3] + degrees[1] + degrees2[1],
all_degrees[4] + degrees[2] + degrees2[2],
all_degrees[5] + degrees[3] + degrees2[3]))
rotate(motor = arm_rotor, clockwise = arcw(1), all_degrees = degrees2, degrees_index = 0, max_speed = max_speed * 1.4, block = False)
rotate(motor = wrist_rotor, clockwise = wrcw(1), all_degrees = degrees2, degrees_index = 1, max_speed = max_speed, block = False)
rotate(motor = wrist_bend, clockwise = wbcw(1), all_degrees = degrees2, degrees_index = 2, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = frcw(1), all_degrees = degrees2, degrees_index = 3, max_speed = max_speed, block = False)
arm_rotor.wait_while('running')
wrist_rotor.wait_while('running')
wrist_bend.wait_while('running')
fingers_rotor.wait_while('running')
all_degrees2 = [ard(45), -abd(22.5), -ebd(22.5), wrd(wr_ang), -wbd(wb_and) + wb_comp_wr_d(wr_ang), +frd(wb_and) + fr_comp_wb_d(wb_and)]
logger.info("all_degrees2 = {0}".format(all_degrees2))
logger.info("sum-deg: {0}, {1}, {2}".format(
all_degrees[3] + all_degrees2[3] + degrees[1] + degrees2[1],
all_degrees[4] + all_degrees2[4] + degrees[2] + degrees2[2],
all_degrees[5] + all_degrees2[5] + degrees[3] + degrees2[3]))
move_all(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1, all_degrees = all_degrees2, max_speed = max_speed)
for motor in [arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor]:
motor.on_to_position(speed=50, position=0, block=True)
motor.stop()
def rotate_arm_hold_wrist(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1):
all_degrees = [ard(90), wbd(90), fr_comp_wb_d(90)]
max_speed = 50
rotate(motor = arm_rotor, clockwise = arcw(direction), all_degrees = all_degrees, degrees_index = 0, max_speed = max_speed * 1.4, block = False)
rotate(motor = wrist_bend, clockwise = -wbcw(direction), all_degrees = all_degrees, degrees_index = 1, max_speed = max_speed, block = False)
rotate(motor = fingers_rotor, clockwise = fr_comp_wb_cw(direction), all_degrees = all_degrees, degrees_index = 2, max_speed = max_speed, block = False)
arm_rotor.wait_while('running')
wrist_bend.wait_while('running')
fingers_rotor.wait_while('running')
def test(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1):
for x in [a for a in range(1,10)] + [b for b in range(10,0,-1)]:
logger.info("Speed: {0}".format(direction * x * 10))
fingers_rotor.on(speed = direction * x * 10)
sleep(0.2)
hostname = socket.gethostname()
logger.info("I am {0}".format(hostname))
wrist_motors_engaged = False
arm_motors_engaged = False
arm_rotor = None
arm_bend = None
elbow_bend = None
wrist_rotor = None
wrist_bend = None
fingers_rotor = None
if hostname == "ev3dev01":
wrist_rotor = MediumMotor(OUTPUT_A)
wrist_bend = MediumMotor(OUTPUT_B)
fingers_rotor = MediumMotor(OUTPUT_C)
wrist_motors_engaged = True
try:
conn = rpyc.classic.connect('ev3dev02') # I am ev3dev01
ev3dev2_motor = conn.modules['ev3dev2.motor']
arm_rotor = ev3dev2_motor.LargeMotor(OUTPUT_A)
arm_bend = ev3dev2_motor.LargeMotor(OUTPUT_B)
elbow_bend = ev3dev2_motor.LargeMotor(OUTPUT_C)
arm_motors_engaged = True
except Exception as exc:
logger.info("ERROR connecting to ev3dev02: {0}".format(exc))
else:
arm_rotor = LargeMotor(OUTPUT_A)
arm_bend = LargeMotor(OUTPUT_B)
elbow_bend = LargeMotor(OUTPUT_C)
arm_motors_engaged = True
try:
conn = rpyc.classic.connect('ev3dev01') # I am ev3dev02
ev3dev2_motor = conn.modules['ev3dev2.motor']
wrist_rotor = ev3dev2_motor.MediumMotor(OUTPUT_A)
wrist_bend = ev3dev2_motor.MediumMotor(OUTPUT_B)
fingers_rotor = ev3dev2_motor.MediumMotor(OUTPUT_C)
wrist_motors_engaged = True
except Exception as exc:
logger.info("ERROR connecting to ev3dev01: {0}".format(exc))
ENGINE = 0
ENGINES = [arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor]
ANGLE_STEP = 10
POS_ANGLE = []
POS_ROTATION = []
for eng in ENGINES:
POS_ANGLE.append(0)
POS_ROTATION.append(0)
ENG_CNT = len(ENGINES)
def rotate_enginge(engine, dir, angle, max_speed):
if engine == 0:
rotate(motor = arm_rotor, clockwise = arcw(dir), all_degrees = [ard(angle)], degrees_index = 0, max_speed = max_speed, block = True)
elif engine == 1:
rotate(motor = arm_bend, clockwise = abcw(dir), all_degrees = [abd(angle)], degrees_index = 0, max_speed = max_speed, block = True)
elif engine == 2:
rotate(motor = elbow_bend, clockwise = ebcw(dir), all_degrees = [ebd(angle)], degrees_index = 0, max_speed = max_speed, block = True)
elif engine == 3:
rotate(motor = wrist_rotor, clockwise = wrcw(dir), all_degrees = [wrd(angle)], degrees_index = 0, max_speed = max_speed, block = True)
elif engine == 4:
rotate(motor = wrist_bend, clockwise = wbcw(dir), all_degrees = [wbd(angle)], degrees_index = 0, max_speed = max_speed, block = True)
elif engine == 5:
rotate(motor = fingers_rotor, clockwise = frcw(dir), all_degrees = [frd(angle)], degrees_index = 0, max_speed = max_speed, block = True)
def change_engine(changed_buttons, button, dir):
global ENGINE, ENGINES, ENG_CNT
if (button, True) in changed_buttons:
ENGINE = (ENGINE + ENG_CNT + dir) % ENG_CNT
logger.info("Current engine: {0}".format(ENGINE))
sound = Sound()
sound.speak("Engine {0}".format(ENGINE))
def change_angle(changed_buttons, button, dir):
global ENGINE, ENGINES, ANGLE_STEP, POS_ANGLE, POS_ROTATION
if (button, True) in changed_buttons:
ENGINES[ENGINE].on(speed = dir * 10)
elif (button, False) in changed_buttons:
ENGINES[ENGINE].stop()
def buttons_changed(changed_buttons):
logger.info("These buttons changed state: {0}".format(changed_buttons))
change_engine(changed_buttons, 'left', -1)
change_engine(changed_buttons, 'right', 1)
change_angle(changed_buttons, 'up', 1)
change_angle(changed_buttons, 'down', -1)
def fix_arm_init_position():
global ENGINES
for motor in ENGINES:
motor.reset()
motor.position = 0
btn = Button()
btn.on_change = buttons_changed
while not btn.enter:
btn.process()
sleep(0.01)
fix_arm_init_position()
if wrist_motors_engaged:
degrees = 360
max_speed = 50
rotate(motor = fingers_rotor, clockwise = frcw(1), all_degrees = [frd(360)], degrees_index = 0, max_speed = 100, block = True)
rotate(motor = fingers_rotor, clockwise = -frcw(1), all_degrees = [frd(360)], degrees_index = 0, max_speed = 100, block = True)
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, max_speed, False, 1, degrees)
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, max_speed, True, -1, degrees)
bend_wrist_and_rotate(wrist_rotor, wrist_bend, fingers_rotor, 1)
if arm_motors_engaged and wrist_motors_engaged:
degrees = 45
rotate_and_move_arm(arm_rotor, arm_bend, elbow_bend, 1, degrees)
sleep(3)
move_forearm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, -1, 90)
sleep(3)
rotate(motor = fingers_rotor, clockwise = frcw(1), all_degrees = [frd(360)], degrees_index = 0, max_speed = 100, block = True)
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, 100, False, 1, 360)
move_forearm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, 1, 90)
sleep(3)
rotate_and_move_arm(arm_rotor, arm_bend, elbow_bend, -2, degrees)
move_forearm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, 1, 90)
sleep(3)
rotate(motor = fingers_rotor, clockwise = -frcw(1), all_degrees = [frd(360)], degrees_index = 0, max_speed = 100, block = True)
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, 100, False, -1, 360)
move_forearm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, -1, 90)
sleep(3)
rotate_and_move_arm(arm_rotor, arm_bend, elbow_bend, 1, degrees)
sleep(3)
if arm_motors_engaged and wrist_motors_engaged:
extend_arm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, 1)
sleep(3)
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, 100, False, 1, 360)
sleep(3)
rotate_bent_arm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, 1, 90)
sleep(3)
rotate_bent_arm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, -1, 180)
sleep(3)
rotate_bent_arm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, 1, 90)
sleep(3)
extend_arm(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, -1)
sleep(3)
if arm_motors_engaged and wrist_motors_engaged:
straight_line(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor)
if arm_motors_engaged and wrist_motors_engaged:
max_speed = 100
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, max_speed = max_speed, fingers_fixed = True, direction = 1, degrees = 90, fingers_direction = 1)
sleep(3)
rotate_arm_hold_wrist(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1)
sleep(3)
rotate_arm_hold_wrist(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = -2)
sleep(3)
rotate_arm_hold_wrist(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, direction = 1)
sleep(3)
rotate_wrist(wrist_rotor, wrist_bend, fingers_rotor, max_speed = max_speed, fingers_fixed = True, direction = -1, degrees = 90, fingers_direction = 1)
if arm_motors_engaged and wrist_motors_engaged:
fingers_rotor.reset()
fingers_rotor.position = 0
test(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, 1)
test(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor, -1)
fingers_rotor.on_to_position(speed=80, position=0, block=True)
fingers_rotor.stop()
if arm_motors_engaged and wrist_motors_engaged:
draw_square(arm_rotor, arm_bend, elbow_bend, wrist_rotor, wrist_bend, fingers_rotor)
stránky vyrobil Tomáš Ullrich - žonglér a programátor - listopad 2008