def __init__(self, left=0, right=1):
self.left = LargeMotor('outA')
self.right = LargeMotor('outB')
self.pos = 0.0
self.speed = 0.0
self.diff = 0
self.target_diff = 0
self.drive = 0
self.steer = 0
self.prev_sum = 0
self.prev_deltas = [0, 0, 0]
def __init__(self, diam, width, r_address=OUTPUT_A, l_address=OUTPUT_B):
super(RobotMotor, self).__init__()
self.diam = diam
self.width = width
self.motors = [
LargeMotor(address) for address in (r_address, l_address)
]
self.reset_position()
def __init__(self, diam, width, r_address=OUTPUT_A, l_address=OUTPUT_D):
super(RobotMotor, self).__init__() # call super class constructor
self.diam = diam # set the diamter wheel
self.width = width # set width of the robot
# collect all robots in addreses and intilize LargeMotor
self.motors = [
LargeMotor(address) for address in (r_address, l_address)
]
self.reset_position() # reset robots psotion
class EV3Motors:
def __init__(self, left=0, right=1):
self.left = LargeMotor('outA')
self.right = LargeMotor('outB')
self.pos = 0.0
self.speed = 0.0
self.diff = 0
self.target_diff = 0
self.drive = 0
self.steer = 0
self.prev_sum = 0
self.prev_deltas = [0, 0, 0]
def get_data(self, interval):
left_pos = self.left.position
right_pos = self.right.position
pos_sum = right_pos + left_pos
self.diff = left_pos - right_pos
delta = pos_sum - self.prev_sum
self.pos += delta
self.speed = (delta + sum(self.prev_deltas)) / (4 * interval)
self.prev_sum = pos_sum
self.prev_deltas = [delta] + self.prev_deltas[0:2]
return self.speed, self.pos
def go(self):
self.left.run_direct()
self.right.run_direct()
def stop(self):
self.left.stop()
self.right.stop()
def set_power(self, power):
self.left.duty_cycle_sp = -power
self.right.duty_cycle_sp = power
buf = array.array('B', [0] * BUF_LEN)
with open('/dev/input/by-path/platform-gpio-keys.0-event', 'r') as fd:
ret = fcntl.ioctl(fd, EVIOCGKEY(len(buf)), buf)
if ret < 0:
print("ioctl error", ret)
sys.exit(1)
#for key in ['UP', 'DOWN', 'LEFT', 'RIGHT', 'ENTER', 'BACKSPACE']:
# key_code = globals()['KEY_' + key]
# key_state = test_bit(key_code, buf) and "pressed" or "released"
# print('%9s : %s' % (key, key_state))
return test_bit(globals()['KEY_ENTER'], buf)
motor_a = LargeMotor('outA')
motor_b = LargeMotor('outB')
motor_c = LargeMotor('outC')
motor_d = LargeMotor('outD')
motor_c.polarity = 'inversed'
motor_d.polarity = 'inversed'
motor_a.stop_action = 'brake'
motor_b.stop_action = 'brake'
motor_c.stop_action = 'brake'
motor_d.stop_action = 'brake'
color_sensor = ColorSensor()
color_sensor.mode = 'COL-REFLECT'
sonar_sensor = UltrasonicSensor()
# import nxt
#
# # brick = nxt.locator.find_one_brick(name = 'Hooper')
# b = nxt.locator.find_one_brick(
# name="NXT", strict=True,
# method=nxt.locator.Method(
# bluetooth=False, fantomusb=True, fantombt=False, usb=False))
from ev3dev.auto import OUTPUT_D, LargeMotor
import time
m = LargeMotor(OUTPUT_D)
print(m)
m.run_forever(speed_sp=360)
time.sleep(1)
m.stop()
print('Hooray')
from ev3dev.auto import OUTPUT_D, LargeMotor import time right = LargeMotor(OUTPUT_D) #move the arm down right.run_timed(speed_sp=360, time_sp=600) time.sleep(1)
buf = array.array('B', [0] * BUF_LEN)
with open('/dev/input/by-path/platform-gpio-keys.0-event', 'r') as fd:
ret = fcntl.ioctl(fd, EVIOCGKEY(len(buf)), buf)
if ret < 0:
print("ioctl error", ret)
sys.exit(1)
#for key in ['UP', 'DOWN', 'LEFT', 'RIGHT', 'ENTER', 'BACKSPACE']:
# key_code = globals()['KEY_' + key]
# key_state = test_bit(key_code, buf) and "pressed" or "released"
# print('%9s : %s' % (key, key_state))
return test_bit(globals()['KEY_ENTER'], buf)
motor_a = LargeMotor('outA')
motor_b = LargeMotor('outB')
motor_c = LargeMotor('outC')
motor_d = LargeMotor('outD')
motor_c.polarity = 'inversed'
motor_d.polarity = 'inversed'
motor_a.stop_action = 'brake'
motor_b.stop_action = 'brake'
motor_c.stop_action = 'brake'
motor_d.stop_action = 'brake'
color_sensor = ColorSensor()
color_sensor.mode = 'COL-REFLECT'
sonar_sensor = UltrasonicSensor()
# import nxt
#
# # brick = nxt.locator.find_one_brick(name = 'Hooper')
# b = nxt.locator.find_one_brick(
# name="NXT", strict=True,
# method=nxt.locator.Method(
# bluetooth=False, fantomusb=True, fantombt=False, usb=False))
from ev3dev.auto import OUTPUT_D, LargeMotor
import time
m = LargeMotor(OUTPUT_D)
print(m)
m.run_forever(speed_sp = 360)
time.sleep(1)
m.stop()
print('Hooray')
from ev3dev.auto import OUTPUT_A, OUTPUT_B, OUTPUT_D, LargeMotor, MediumMotor, InfraredSensor
from ev3dev.auto import INPUT_1, INPUT_2, ColorSensor, UltrasonicSensor
import time
import ev3dev.auto as auto
import ev3dev.ev3 as ev3
uss = UltrasonicSensor(INPUT_1)
colorSensor = ColorSensor(INPUT_2)
clawMotor = MediumMotor(OUTPUT_B)
leftTire = LargeMotor(OUTPUT_A)# and LargeMotor(OUTPUT_D)
rightTire = LargeMotor(OUTPUT_D)
ev3.Sound.speak('Ha Ha Ha this is mine now').wait()
leftTire.run_timed(speed_sp = 900, time_sp = 600)
rightTire.run_timed(speed_sp = 900, time_sp = 600)
#!/usr/bin/python from ev3dev.auto import LargeMotor, OUTPUT_A motor = LargeMotor(OUTPUT_A) motor.duty_cycle_sp = 80 motor.time_sp = 5000 motor.run_timed() #should use speed_sp
#!/usr/bin/python from ev3dev.auto import LargeMotor, Sound, OUTPUT_A from time import sleep, time sound = Sound() motor = LargeMotor(OUTPUT_A) sound.beep() sleep(0.5) motor.run_direct(duty_cycle_sp=50) sleep(1) sound.tone(500, 500) sleep(0.5)
from ev3dev.auto import OUTPUT_A, OUTPUT_B, OUTPUT_D, LargeMotor, MediumMotor
from ev3dev.auto import INPUT_1, INPUT_2, ColorSensor, UltrasonicSensor
import time
import ev3dev.ev3 as ev3
ultrasonicSensor = UltrasonicSensor(INPUT_1)
colorSensor = ColorSensor(INPUT_2)
clawMotor = MediumMotor(OUTPUT_B)
leftTire = LargeMotor(OUTPUT_A)
rightTire = LargeMotor(OUTPUT_D)
def getUltrasonic():
ultrasonicSensor.mode = 'US-DIS-CM'
return ultrasonicSensor.units
def getColor():
colorSensor.mode = 'COL-REFLECT'
return colorSensor.value()
#def findObject():
while getUltrasonic > 5.5:
leftTire.run_timed(power=15, rotations=0.2)
rightTire.run_timed(power=15, rotations=0.2)
time.sleep(1)
clawMotor.run_timed(power=75, rotations=0.8)
time.sleep(1)
#def findTarget():
def runner_stub(sh_mem):
print('Im Runner Stub')
def shutdown_child(signum=None, frame=None):
motor_encoder_left_devfd.close()
motor_encoder_right_devfd.close()
motor_duty_cycle_left_devfd.close()
motor_duty_cycle_right_devfd.close()
tail_motor.stop_action = tail_motor.STOP_ACTION_COAST
tail_motor.stop()
left_motor.stop()
right_motor.stop()
sys.exit()
signal.signal(signal.SIGTERM, shutdown_child)
try:
# Motor setup
left_motor = LargeMotor('outC')
right_motor = LargeMotor('outB')
left_motor.reset()
right_motor.reset()
left_motor.run_direct()
right_motor.run_direct()
#しっぽモーター
tail_motor = Motor('outA')
# しっぽモーターを固定する
tail_motor.stop_action = tail_motor.STOP_ACTION_HOLD
tail_motor.stop()
########################################################################
## Definitions and Initialization variables
########################################################################
# Timing settings for the program
# Time of each loop, measured in miliseconds.
loop_time_millisec = 100
# Time of each loop, measured in seconds.
loop_time_sec = loop_time_millisec / 1000.0
motor_angle_raw_left = 0
motor_angle_raw_right = 0
motor_angular_speed_reference = 0.0
a_r = 0.985 #0.98 # ローパスフィルタ係数(左右車輪の目標平均回転角度用)。左右モーターの目標平均回転角度(rad)の算出時に使用する。小さいほど前進・後退する反応が早くなる。
k_theta_dot = 3.5 # モータ目標回転角速度係数
# filehandles for fast reads/writes
# =================================
# Open motor files for (fast) reading
motor_encoder_left_devfd = open(left_motor._path + "/position", "rb")
motor_encoder_right_devfd = open(right_motor._path + "/position", "rb")
# Open motor files for (fast) writing
motor_duty_cycle_left_devfd = open(left_motor._path + "/duty_cycle_sp",
"w")
motor_duty_cycle_right_devfd = open(
right_motor._path + "/duty_cycle_sp", "w")
speed_reference = sh_mem.read_speed_mem()
steering = sh_mem.read_steering_mem()
########################################################################
## タッチセンサー押し待ち
########################################################################
print('Runner Waiting ...')
while not sh_mem.read_touch_sensor_mem():
sleep(0.025)
print("-----------------------------------")
print("GO!")
print("-----------------------------------")
# 倒立振子スタート時の時間取得
t_balancer_start = clock()
while True:
###############################################################
## Loop info
###############################################################
t_loop_start = clock()
###############################################################
## Reading the Motor Position
###############################################################
motor_angle_raw_left = read_device(motor_encoder_left_devfd)
motor_angle_raw_right = read_device(motor_encoder_right_devfd)
sh_mem.write_motor_encoder_left_mem(motor_angle_raw_left)
sh_mem.write_motor_encoder_right_mem(motor_angle_raw_right)
speed_reference = sh_mem.read_speed_mem()
motor_angular_speed_reference = ((1.0 - a_r) * (
(speed_reference / 100.0) * k_theta_dot)) + (
a_r * motor_angular_speed_reference)
###############################################################
## Computing the motor duty cycle value
###############################################################
motor_duty_cycle = motor_angular_speed_reference
###############################################################
## Apply the signal to the motor, and add steering
###############################################################
steering = sh_mem.read_steering_mem()
set_duty(motor_duty_cycle_right_devfd, speed_reference + steering)
duty = set_duty(motor_duty_cycle_left_devfd,
speed_reference - steering)
###############################################################
## Busy wait for the loop to complete
###############################################################
# clock()の値にはsleep中の経過時間が含まれないので、このwhileの条件文の算出時間をsleep代わりにしている(算出時間はバラバラ…)
sleep(max(loop_time_sec - (clock() - t_loop_start), 0.002))
except Exception as ex:
print("It's a Runner Exception")
g_log.exception(ex)
shutdown_child()
from ev3dev.auto import OUTPUT_D, LargeMotor, OUTPUT_A, MediumMotor, OUTPUT_C import time mup = LargeMotor(OUTPUT_D) mgrab = MediumMotor(OUTPUT_A) mright = LargeMotor(OUTPUT_C) #move the arm down mup.run_timed(speed_sp=360, time_sp=600) #grab the object time.sleep(1) mgrab.run_timed(speed_sp=720, time_sp=500) # move arm back up time.sleep(1) mup.run_timed(speed_sp=-360, time_sp=600) #move arm right time.sleep(1) mright.run_timed(speed_sp=360, time_sp=600) #move arm down at target location time.sleep(1) mup.run_timed(speed_sp=360, time_sp=705) #release the object time.sleep(1) mgrab.run_timed(speed_sp=-360, time_sp=400) #reset position time.sleep(1) mup.run_timed(speed_sp=-360, time_sp=600) time.sleep(1) mright.run_timed(speed_sp=-360, time_sp=400)
from ev3dev.auto import OUTPUT_A, OUTPUT_B, OUTPUT_D, LargeMotor, MediumMotor, InfraredSensor from ev3dev.auto import INPUT_1, INPUT_2, ColorSensor, UltrasonicSensor import time import ev3dev.auto as auto import ev3dev.ev3 as ev3 uss = UltrasonicSensor(INPUT_1) colorSensor = ColorSensor(INPUT_2) clawMotor = MediumMotor(OUTPUT_B) leftTire = LargeMotor(OUTPUT_A) # and LargeMotor(OUTPUT_D) rightTire = LargeMotor(OUTPUT_D) clawMotor.run_timed(speed_sp=720, time_sp=500) time.sleep(1)
from ev3dev.auto import OUTPUT_A, OUTPUT_B, OUTPUT_D, LargeMotor, MediumMotor from ev3dev.auto import INPUT_1, INPUT_2, ColorSensor, UltrasonicSensor import time import ev3dev.ev3 as ev3 #ultrasonicSensor = UltrasonicSensor(INPUT_1) #colorSensor = ColorSensor(INPUT_2) clawMotor = MediumMotor(OUTPUT_B) leftTire = LargeMotor(OUTPUT_A) and LargeMotor(OUTPUT_D) #rightTire = LargeMotor(OUTPUT_D) # def getUltrasonic(): # ultrasonicSensor.mode='US-DIS-CM' # return ultrasonicSensor.units # # def getColor(): # colorSensor.mode='COL-REFLECT' # return colorSensor.value() # # # #def findObject(): # while getUltrasonic > 5.5: leftTire.run_timed(speed_sp=360, time_sp=600) #rightTire.run_timed(speed_sp = 360, time_sp = 600) time.sleep(1) leftTire.run_timed(speed_sp=360, time_sp=600) #rightTire.run_timed(speed_sp = 360, time_sp = 600) time.sleep(1) leftTire.run_timed(speed_sp=360, time_sp=600) #rightTire.run_timed(speed_sp = 360, time_sp = 600)
from ev3dev.auto import OUTPUT_A, OUTPUT_B, OUTPUT_C, LargeMotor, MediumMotor
from ev3dev.auto import INPUT_1, INPUT_2, INPUT_3, TouchSensor, ColorSensor
import time
import ev3dev.ev3 as ev3
baseTouch = TouchSensor(INPUT_1)
armTouch = TouchSensor(INPUT_2)
colorSensor = ColorSensor(INPUT_3)
clawMotor = MediumMotor(OUTPUT_A)
armMotor = LargeMotor(OUTPUT_B)
baseMotor = LargeMotor(OUTPUT_C)
# Claw Opening
ev3.Sound.speak("Now opening claw.").wait()
clawMotor.run_forever(speed_sp=100)
time.sleep(.5)
clawMotor.stop()
# Arm Moving to down position
ev3.Sound.speak("Now moving arm down.").wait()
armMotor.run_forever(speed_sp=120)
time.sleep(2.5)
armMotor.stop()
# Claw closing on object to pick up
ev3.Sound.speak("Now closing claw.").wait()
clawMotor.run_forever(speed_sp=-100)
time.sleep(2)
# Arm moving to the up position
ev3.Sound.speak("Now moving arm up.").wait()