class Robot(AlexaGadget):
def __init__(self):
super().__init__()
# initialize all of the motors
print('Initializing devices')
self.leds = Leds()
self.motor_hand = LargeMotor(address='outA')
self.motor_claw = MediumMotor(address='outC')
# called when the EV3 brick connects to an Alexa device
def on_connected(self, device_addr):
self.leds.set_color('LEFT', 'GREEN')
self.leds.set_color('RIGHT', 'GREEN')
print("{} connected to Echo device".format(self.friendly_name))
# called when the EV3 brick disconnects from an Alexa device
def on_disconnected(self, device_addr):
self.leds.set_color('LEFT', 'BLACK')
self.leds.set_color('RIGHT', 'BLACK')
print("{} disconnected from Echo device".format(self.friendly_name))
# the function called to receive gadget control directives from the Alexa Skill through the connected Alexa device
def on_custom_mindstorms_gadget_control(self, directive):
# decode the directive payload into a JSON object
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload))
# determine which command to be executed
control_type = payload['type']
if control_type == 'Go':
# get the source and destination states for this command
src_state = State[payload['state']]
self.motors_claw.on(20)
time.sleep(2)
self.motors_claw.off(brake=True)
elif control_type == 'Back':
# get the source and destination states for this command
src_state = State[payload['state']]
self.motors_claw.on(-20)
time.sleep(2)
self.motors_claw.off(brake=True)
elif control_type == 'Left':
# get the source and destination states for this command
src_state = State[payload['state']]
self.motors_hand.on(20)
time.sleep(2)
self.motors_hand.off(brake=True)
elif control_type == 'Right':
# get the source and destination states for this command
src_state = State[payload['state']]
self.motors_hand.on(-20)
time.sleep(2)
self.motors_hand.off(brake=True)
elif control_type == 'Turn':
# get the source and destination states for this command
src_state = State[payload['state']]
while(danceflag == 1):
self.motors_hand.on(20)
time.sleep(2)
self.motors_hand.on(-20)
self.motors_hand.off(brake=True)
# move the robot straight back at a certain speed for a certain number of rotations
def move_back(self, speed=0.2, distance=1.6):
self.motor_left.on_for_rotations(round(speed * 100),
distance,
block=False)
self.motor_right.on_for_rotations(round(speed * 100), distance)
# turn off all motors and lights
def poweroff(self):
self.motor_hand.off(brake=False)
self.motor_claw.off(brake=False)
#!/usr/bin/env python3
#
# uses ev3dev2-based code
#
import time
from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, SpeedDPS, MoveTank
from ev3dev2.sensor import INPUT_1
print("stopping all motors")
mA = LargeMotor(OUTPUT_A)
mB = LargeMotor(OUTPUT_B)
mC = LargeMotor(OUTPUT_C)
mD = LargeMotor(OUTPUT_D)
time.sleep(0.1)
mA.reset()
mC.reset()
mB.reset()
mD.reset()
mA.off(brake=False)
mC.off(brake=False)
mB.off(brake=False)
mD.off(brake=False)
time.sleep(0.1)
print("all motors should now be stopped, brake = False")
sleep(1) lm.on_for_rotations(50, 3) # 매개 변수 이름을 명시하지 않아도 된다. sleep(1) #endregion #region 2.시간당 지정된 스피드로 rotations 바퀴만큼 회전 lm.on_for_rotations(speed=SpeedDPS(500), rotations=3) # DPS(degrees per second) 초당 500도를 도는 속도로 3바퀴 회전 sleep(1) lm.on_for_rotations(speed=SpeedRPS(1), rotations=3) # RPS(rotations per second) 초당 1 바퀴를 도는 속도로 3바퀴 회전 sleep(1) #endregion #endregion #region break, block # break : True 인 경우 모터가 동작을 완료하면 모터를 고정위치에 붙잡는다. # block : 현재 명령이 완료 될 때까지 프로그램 실행을 일시 중지 #endregion #region 계속 회전하기 => on(speed, brake=True, block=False) lm.on(speed=80, brake=False) # 정격 최고 속도의 45 %의 속도로 계속 회전 sleep(2) lm.off() # 정지 #endregion #region Run a single motor until it stops moving # # 그리퍼의 팔이 바닥까지 내려갈 때 어떤 각도 또는 시간동안 내려가야하는지 정확히 알 수 없다. # # 이 명령은 일반적으로이 on() 명령 다음에 실행된다. # # 움직이는 휠을 잡고 멈 추면 스크립트가 종료된다. lm.on(speed=20) lm.wait_until_not_moving() #endregion
#!/usr/bin/env python3
from time import sleep
from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent
from ev3dev2.sensor.lego import TouchSensor
touch_start_button = TouchSensor()
motor_conveyor = LargeMotor(OUTPUT_A)
motor_fan = LargeMotor(OUTPUT_B)
motor_water = MediumMotor(OUTPUT_C)
while True:
touch_start_button.wait_for_pressed()
motor_conveyor.on(SpeedPercent(8))
sleep(1)
motor_water.on_for_rotations(SpeedPercent(10), 0.5)
sleep(1.5)
motor_water.on_for_rotations(SpeedPercent(10), -0.5)
sleep(1.5)
motor_fan.on(SpeedPercent(100))
motor_conveyor.on(SpeedPercent(3))
sleep(4)
motor_fan.off()
motor_conveyor.off()
else:
i = 0
if i >= 5:
StallDetected = True
if mL.duty_cycle_sp > 0:
mLposU = mL.position
elif mL.duty_cycle_sp < 0:
mLposJ = mL.position
mL.duty_cycle_sp = 0
print("motor stalled; killing power")
print("x, mL.duty_cycle_sp, mL.position, mLlastPos ", x,
mL.duty_cycle_sp, mL.position, mLlastPos)
break
if x == 120: # x key pushed - exit
mL.duty_cycle_sp = 0
mL.off(brake=False)
break
if x == 32: # space key pushed
mL.duty_cycle_sp = 0
if x == 117: # u key pushed
mL.duty_cycle_sp = mLDtyMax
if x == 106: # j key pushed
mL.duty_cycle_sp = -mLDtyMax
if mL.position != mLlastPos:
print("x, mL.duty_cycle_sp, mL.position, mLlastPos ", x,
mL.duty_cycle_sp, mL.position, mLlastPos)
mLlastPos = mL.position
x = 0 # req'd to prevent the equiv of a repeat/stuck keyboad key
time.sleep(.1)
if x == 120: # x key pushed - exit
leds.set_color(led_name, 'GREEN')
RMC.on(rightSpeed)
LMC.on(leftSpeed)
i = 0
while light_intensity > 20 and i < ticks:
i = i + 1
sleep(0.005)
light_intensity = cl.reflected_light_intensity
if light_intensity < 20:
LMC.off()
RMC.off()
leds.all_off()
while True:
if (int(round(time.time() * 1000)) - start_time) > 20000:
LMC.off()
RMC.off()
RMC.on(-50)
LMC.on(-50)
sleep(5)
RMC.on(50)
LMC.on(50)
sleep(5)
start_time = int(round(time.time() * 1000))
light_intensity = cl.reflected_light_intensity
if light_intensity > 20:
# White Part of the Board
LMC.off()
RMC.off()
class R3PTAR(object):
def __init__(self,
drive_motor_port=OUTPUT_B,
strike_motor_port=OUTPUT_D,
steer_motor_port=OUTPUT_A,
drive_speed_pct=60):
self.drive_motor = LargeMotor(drive_motor_port)
self.strike_motor = LargeMotor(strike_motor_port)
self.steer_motor = MediumMotor(steer_motor_port)
self.speaker = Sound()
STEER_SPEED_PCT = 30
self.remote = InfraredSensor()
self.remote.on_channel1_top_left = self.make_move(self.drive_motor, drive_speed_pct)
self.remote.on_channel1_bottom_left = self.make_move(self.drive_motor, drive_speed_pct * -1)
self.remote.on_channel1_top_right = self.make_move(self.steer_motor, STEER_SPEED_PCT)
self.remote.on_channel1_bottom_right = self.make_move(self.steer_motor, STEER_SPEED_PCT * -1)
self.shutdown_event = Event()
self.mrc = MonitorRemoteControl(self)
# Register our signal_term_handler() to be called if the user sends
# a 'kill' to this process or does a Ctrl-C from the command line
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
def make_move(self, motor, speed):
def move(state):
if state:
motor.on(speed)
else:
motor.stop()
return move
def shutdown_robot(self):
if self.shutdown_event.is_set():
return
self.shutdown_event.set()
log.info('shutting down')
self.mrc.shutdown_event.set()
self.remote.on_channel1_top_left = None
self.remote.on_channel1_bottom_left = None
self.remote.on_channel1_top_right = None
self.remote.on_channel1_bottom_right = None
self.drive_motor.off(brake=False)
self.strike_motor.off(brake=False)
self.steer_motor.off(brake=False)
self.mrc.join()
def signal_term_handler(self, signal, frame):
log.info('Caught SIGTERM')
self.shutdown_robot()
def signal_int_handler(self, signal, frame):
log.info('Caught SIGINT')
self.shutdown_robot()
def main(self):
self.mrc.start()
self.shutdown_event.wait()
loopCounter = 0
BlackThereshold = 15
WhiteThereshold = 80
#while (cs4.color != ColorSensor.COLOR_BLACK and cs1.color != ColorSensor.COLOR_BLACK):
while (loopCounter < 48):
print(
"{0:02d} - CS1: {1:10}, CS4: {2:10}, CS1Reflected: {3:03d}, CS4Reflected: {4:03d}"
.format(loopCounter, cs1.color_name, cs4.color_name,
cs1.reflected_light_intensity, cs4.reflected_light_intensity),
file=sys.stderr)
loopCounter += 1
if (cs1.reflected_light_intensity > WhiteThereshold):
sleep(.01)
if (cs1.reflected_light_intensity > WhiteThereshold):
lmB.off()
sleep(.01)
if (cs4.reflected_light_intensity > WhiteThereshold):
if (cs4.reflected_light_intensity > WhiteThereshold):
lmC.off()
break
elif (cs4.reflected_light_intensity > WhiteThereshold):
sleep(.01)
if (cs4.reflected_light_intensity > WhiteThereshold):
lmC.off()
sleep(.01)
if (cs4.reflected_light_intensity > WhiteThereshold):
if (cs4.reflected_light_intensity > WhiteThereshold):
lmB.off()
break
lmB.on(2)
end_pos = 0
start_time = 0.0
end_time = 0.0
m = LargeMotor(OUTPUT_A)
time.sleep(0.1)
m.reset()
time.sleep(0.1)
start_time = time.time()
m.stop_action = 'hold'
for i in range(1, 11):
#print("START --------> m.on_to_position ... or on_for_degrees")
m.on_to_position(SpeedDPS(200), (i * 360), brake=True,
block=True) ### this method FAILS TO BLOCK
# m.on_for_degrees(SpeedDPS(200), (i * 360), brake=False, block=True) ### this method FAILS TO BLOCK
#print("END --------> m.on_to_position ... or on_for_degrees")
print("stop_action is ", m.stop_action)
print("state is ", m.state)
print("START --------> m.wait_while...blah")
m.wait_while('stalled')
#print("END --------> m.wait_while...blah")
print("stop_action is ", m.stop_action)
print("state is ", m.state)
time.sleep(3)
m.off(brake=True)
print("target pos = ", (i * 360), "; actual pos = ", m.position)
m.reset()
m.stop_action = 'hold'
time.sleep(.1)
t1 = time.time()
old_pos = encoder_pos
time.sleep(0.05)
encoder_pos = m.position
dt = time.time() - t1
d_deg = encoder_pos - old_pos
tachospeed = d_deg / dt
d_deg_avg = ((d_deg_avg + d_deg) / (2))
tachospeed_avg = ((tachospeed_avg + tachospeed) / (2))
if tachospeed < tachospeed_min:
tachospeed_min = tachospeed
if tachospeed > tachospeed_max:
tachospeed_max = tachospeed
# if abs(d_deg) > 100:
# print ("d_deg = ", d_deg, "; d_deg_avg", d_deg_avg, "; tachospeed_avg", tachospeed_avg)
m.off()
end_time = time.time()
end_pos = m.position
print("running data collection:")
print("i = ", i, "; j = ", j)
print("d_deg = ", d_deg, "; d_deg_avg", d_deg_avg, "; tachospeed_avg",
tachospeed_avg)
print("tachospeed_min = ", tachospeed_min, "; tachospeed_max = ",
tachospeed_max)
# print ("")
# print ("static data collection:")
# print ("start_time = ", start_time, "; end_time", end_time)
# print ("start_pos = ", start_pos, "; end_pos", end_pos)
# print ("time diff ", end_time - start_time)
# print ("pos diff ", end_pos - start_pos)
print("d_pos / d_t", (end_pos - start_pos) / (end_time - start_time))
The views and conclusions contained in the software and documentation are those of the authors and should not be interpreted as representing official policies, either expressed or implied, of the FLL Robot Framework project. -------------------------------------------------------------------------------- ''' from ev3dev2.sensor.lego import TouchSensor from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_B, OUTPUT_C ts = TouchSensor() largeMotor_Left = LargeMotor(OUTPUT_B) largeMotor_Right = LargeMotor(OUTPUT_C) mediumMotor = MediumMotor() # run these in parallel largeMotor_Left.on_for_rotations(speed=30, rotations=4, brake=True, block=False) largeMotor_Right.on_for_rotations(speed=40, rotations=3, brake=True, block=False) # stop the rotations if the user lifts the robot (simulate by pressing the button) if ts.is_pressed: largeMotor_Left.off() largeMotor_Right.off() largeMotor_Left.wait_until_not_moving() largeMotor_Right.wait_until_not_moving() # run this after the previous have completed mediumMotor.on_for_seconds(speed=10, seconds=6)
# large_motor.on(speed=30)
# large_motor.wait_until_not_moving() ### this does not work with BrickPi3, state never changes as needed
# For BrickPi3 (and other non-EV3 platforms) the following may work better as control method for detecting a stall
mAspeed = 15 # speed setting
duty_cycle_margin = 10 # sensitivity buffer/margin; adjust to suite needs
duty_cycle_avg = 0
duty_cycle_min = 101
duty_cycle_max = -101
valChange = 0
large_motor.on(speed=mAspeed)
while large_motor.duty_cycle <= (mAspeed + duty_cycle_margin):
duty_cycle_avg = (duty_cycle_avg + large_motor.duty_cycle) / 2
duty_cycle_min = min(duty_cycle_min, large_motor.duty_cycle)
duty_cycle_max = max(duty_cycle_max, large_motor.duty_cycle)
if valChange != duty_cycle_min + duty_cycle_max:
print("duty_cycle avg, min, max = ", duty_cycle_avg, duty_cycle_min, duty_cycle_max)
valChange = duty_cycle_min + duty_cycle_max
time.sleep(0.1)
print("duty_cycle avg, min, max = ", duty_cycle_avg, duty_cycle_min, duty_cycle_max)
large_motor.off()
time.sleep(0.1)
print("motor should now be stopped")
class R3ptar:
def __init__(self,
turn_motor_port: str = OUTPUT_A,
move_motor_port: str = OUTPUT_B,
scare_motor_port: str = OUTPUT_D,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.turn_motor = MediumMotor(address=turn_motor_port)
self.move_motor = LargeMotor(address=move_motor_port)
self.scare_motor = LargeMotor(address=scare_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.noise = Sound()
def drive_once_by_ir_beacon(self, speed: float = 100):
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.move_motor.on(speed=-speed, brake=False, block=False)
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=-50, brake=False, block=False)
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=50, brake=False, block=False)
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=-50, brake=False, block=False)
self.move_motor.on(speed=-speed, brake=False, block=False)
elif self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=50, brake=False, block=False)
self.move_motor.on(speed=-speed, brake=False, block=False)
else:
self.turn_motor.off(brake=True)
self.move_motor.off(brake=False)
def bite_by_ir_beacon(self, speed: float = 100):
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.scare_motor.on_for_seconds(speed=speed,
seconds=1,
brake=True,
block=False)
self.noise.play_file(wav_file='/home/robot/sound/Snake hiss.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.scare_motor.on_for_seconds(speed=-speed,
seconds=1,
brake=True,
block=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def run_away_if_chased(self):
if self.color_sensor.reflected_light_intensity > 30:
self.move_motor.on_for_seconds(speed=50,
seconds=4,
brake=True,
block=False)
for i in range(2):
self.turn_motor.on_for_seconds(speed=50,
seconds=1,
brake=False,
block=True)
self.turn_motor.on_for_seconds(speed=-50,
seconds=1,
brake=False,
block=True)
def bite_if_touched(self):
if self.touch_sensor.is_pressed:
self.scare_motor.on_for_seconds(speed=100,
seconds=1,
brake=True,
block=False)
self.noise.play_file(wav_file='/home/robot/sound/Snake hiss.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.scare_motor.on_for_seconds(speed=-10,
seconds=10,
brake=True,
block=True)
def main(self, speed: float = 100):
while True:
self.drive_once_by_ir_beacon(speed=speed)
self.bite_by_ir_beacon(speed=speed)
self.bite_if_touched()
self.run_away_if_chased()
m1 = LargeMotor(OUTPUT_C)
m2 = LargeMotor(OUTPUT_D)
m3 = LargeMotor(OUTPUT_B)
us = UltrasonicSensor()
sound = Sound()
print(us.distance_centimeters, file=sys.stderr)
a_min = m3.position + 10
m3.on(-10, block=False)
while m3.position < a_min:
a_min = m3.position
time.sleep(0.1)
m3.off()
print("a_min", a_min, file=sys.stderr)
a_max = m3.position - 10
m3.on(10, block=False)
while m3.position > a_max:
a_max = m3.position
time.sleep(0.1)
m3.off()
print("a_max", a_max, file=sys.stderr)
def zahvat(a_min, a_max):
a = a_min
m3.on(-10, block=False)
while abs(m3.position - a) > 1:
class AthenaRobot(object):
# constructors for the robot with default parameters of wheel radius and ports
def __init__(self,
wheelRadiusCm=2.75,
leftMotorPort=OUTPUT_C,
rightMotorPort=OUTPUT_B,
leftSensorPort=INPUT_4,
rightSensorPort=INPUT_1):
#self is the current object, everything below for self are member variables
self.wheelRadiusCm = wheelRadiusCm
self.wheelCircumferenceCm = 2 * math.pi * wheelRadiusCm
self.leftMotor = LargeMotor(leftMotorPort)
self.rightMotor = LargeMotor(rightMotorPort)
self.leftSensor = ColorSensor(leftSensorPort)
self.rightSensor = ColorSensor(rightSensorPort)
# run a distance in centimeters at speed of centimeters per second
def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
# Calculate degrees of distances and SpeedDegreePerSecond
degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
print("Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
degreesToRun, speedDegreePerSecond, self.leftMotor.max_speed),
file=sys.stderr)
# run motors based on the calculated results
self.leftMotor.on_for_degrees(SpeedDPS(speedDegreePerSecond),
degreesToRun, brake, False)
self.rightMotor.on_for_degrees(SpeedDPS(speedDegreePerSecond),
degreesToRun, brake, block)
# turn a angle in degrees, positive means turn right and negative means turn left.
def turn(self, degree, brake=True, block=True):
# 1.9 is a scale factor from experiments
degreesToRun = degree * 1.9
# Turn at the speed of 20
self.leftMotor.on_for_degrees(20, degreesToRun, brake, False)
self.rightMotor.on_for_degrees(-20, degreesToRun, brake, block)
# run until find a game line
def onUntilGameLine(self,
consecutiveHit=5,
speed=10,
sleepTime=0.01,
white_threshold=85,
black_threshold=30,
brake=True):
# Start motor at passed speed.
self.leftMotor.on(speed)
self.rightMotor.on(speed)
# flags for whether both left and right wheel are in position
leftLineSquaredWhite = False
rightLineSquaredWhite = False
leftConsecutiveWhite = 0
rightConsecutiveWhite = 0
# first aligned on white
while (not leftLineSquaredWhite or not rightLineSquaredWhite):
left_reflected = self.leftSensor.reflected_light_intensity
right_reflected = self.rightSensor.reflected_light_intensity
# left to detect white
if (left_reflected > white_threshold):
leftConsecutiveWhite += 1
else:
leftConsecutiveWhite = 0
# reset to zero
if (leftConsecutiveWhite >= consecutiveHit):
self.leftMotor.off()
leftLineSquaredWhite = True
# right to detect white
if (right_reflected > white_threshold):
rightConsecutiveWhite += 1
else:
rightConsecutiveWhite = 0
# reset to zero
if (rightConsecutiveWhite >= consecutiveHit):
self.rightMotor.off()
rightLineSquaredWhite = True
print(
"left_reflected: {0:3d}, right_reflected: {1:3d}, leftConsecutiveWhite: {2:3d}, rightConsecutiveWhite: {3:3d}"
.format(left_reflected, right_reflected, leftConsecutiveWhite,
rightConsecutiveWhite),
file=sys.stderr)
sleep(sleepTime)
print("*********** White Line Reached *********", file=sys.stderr)
leftLineSquaredBlack = False
rightLineSquaredBlack = False
leftConsecutiveBlack = 0
rightConsecutiveBlack = 0
# now try black
self.leftMotor.on(speed)
self.rightMotor.on(speed)
while (not leftLineSquaredBlack or not rightLineSquaredBlack):
left_reflected = self.leftSensor.reflected_light_intensity
right_reflected = self.rightSensor.reflected_light_intensity
# left to detect black
if (left_reflected < black_threshold):
leftConsecutiveBlack += 1
else:
leftConsecutiveBlack = 0
# reset to zero
if (leftConsecutiveBlack >= consecutiveHit):
self.leftMotor.off()
leftLineSquaredBlack = True
# right to detect black
if (right_reflected < black_threshold):
rightConsecutiveBlack += 1
else:
rightConsecutiveBlack = 0
# reset to zero
if (rightConsecutiveBlack >= consecutiveHit):
self.rightMotor.off()
rightLineSquaredBlack = True
print(
"left_reflected: {0:3d}, right_reflected: {1:3d}, leftConsecutiveBlack: {2:3d}, rightConsecutiveBlack: {3:3d}"
.format(left_reflected, right_reflected, leftConsecutiveBlack,
rightConsecutiveBlack),
file=sys.stderr)
sleep(sleepTime)
self.leftMotor.off()
self.rightMotor.off()
#Go to the Bridge
def goToBridge(self):
# start from base, run 12.5 cm at 20cm/s
self.run(12.5, 20)
sleep(.2)
# turn right 70 degree
self.turn(70)
sleep(.1)
print("test", file=sys.stderr)
# run 90 cm at speed of 30 cm/s
self.run(90, 30, False)
sleep(.1)
# run until hit game line
self.onUntilGameLine()
sleep(.1)
# move forward 2cm at 15cm/s
self.run(2, 15)
# turn left 90 degree
self.turn(-90)
# move forward 13 cm at 20cm/s
self.run(13, 20)
sleep(.1)
# run until hit game line
self.onUntilGameLine()
class MindCuber(object):
scan_order = [
5, 9, 6, 3, 2, 1, 4, 7, 8, 23, 27, 24, 21, 20, 19, 22, 25, 26, 50, 54,
51, 48, 47, 46, 49, 52, 53, 14, 10, 13, 16, 17, 18, 15, 12, 11, 41, 43,
44, 45, 42, 39, 38, 37, 40, 32, 34, 35, 36, 33, 30, 29, 28, 31
]
hold_cube_pos = 85
rotate_speed = 400
flip_speed = 300
flip_speed_push = 400
def __init__(self):
self.shutdown = False
self.flipper = LargeMotor(address=OUTPUT_A)
self.turntable = LargeMotor(address=OUTPUT_B)
self.colorarm = MediumMotor(address=OUTPUT_C)
self.color_sensor = ColorSensor()
self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
self.infrared_sensor = InfraredSensor()
self.init_motors()
self.state = ['U', 'D', 'F', 'L', 'B', 'R']
self.rgb_solver = None
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
filename_max_rgb = 'max_rgb.txt'
if os.path.exists(filename_max_rgb):
with open(filename_max_rgb, 'r') as fh:
for line in fh:
(color, value) = line.strip().split()
if color == 'red':
self.color_sensor.red_max = int(value)
log.info("red max is %d" % self.color_sensor.red_max)
elif color == 'green':
self.color_sensor.green_max = int(value)
log.info("green max is %d" %
self.color_sensor.green_max)
elif color == 'blue':
self.color_sensor.blue_max = int(value)
log.info("blue max is %d" % self.color_sensor.blue_max)
def init_motors(self):
for x in (self.flipper, self.turntable, self.colorarm):
x.reset()
log.info("Initialize flipper %s" % self.flipper)
self.flipper.on(SpeedDPS(-50), block=True)
self.flipper.off()
self.flipper.reset()
log.info("Initialize colorarm %s" % self.colorarm)
self.colorarm.on(SpeedDPS(500), block=True)
self.colorarm.off()
self.colorarm.reset()
log.info("Initialize turntable %s" % self.turntable)
self.turntable.off()
self.turntable.reset()
def shutdown_robot(self):
log.info('Shutting down')
self.shutdown = True
if self.rgb_solver:
self.rgb_solver.shutdown = True
for x in (self.flipper, self.turntable, self.colorarm):
# We are shutting down so do not 'hold' the motors
x.stop_action = 'brake'
x.off(False)
def signal_term_handler(self, signal, frame):
log.error('Caught SIGTERM')
self.shutdown_robot()
def signal_int_handler(self, signal, frame):
log.error('Caught SIGINT')
self.shutdown_robot()
def apply_transformation(self, transformation):
self.state = [self.state[t] for t in transformation]
def rotate_cube(self, direction, nb):
current_pos = self.turntable.position
final_pos = 135 * round(
(self.turntable.position + (270 * direction * nb)) / 135.0)
log.info(
"rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" %
(direction, nb, current_pos, final_pos))
if self.flipper.position > 35:
self.flipper_away()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
final_pos)
if nb >= 1:
for i in range(nb):
if direction > 0:
transformation = [0, 1, 5, 2, 3, 4]
else:
transformation = [0, 1, 3, 4, 5, 2]
self.apply_transformation(transformation)
def rotate_cube_1(self):
self.rotate_cube(1, 1)
def rotate_cube_2(self):
self.rotate_cube(1, 2)
def rotate_cube_3(self):
self.rotate_cube(-1, 1)
def rotate_cube_blocked(self, direction, nb):
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# OVERROTATE depends on lot on MindCuber.rotate_speed
current_pos = self.turntable.position
OVERROTATE = 18
final_pos = int(135 * round(
(current_pos + (270 * direction * nb)) / 135.0))
temp_pos = int(final_pos + (OVERROTATE * direction))
log.info(
"rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s"
% (direction, nb, current_pos, temp_pos, final_pos))
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
temp_pos)
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4),
final_pos)
def rotate_cube_blocked_1(self):
self.rotate_cube_blocked(1, 1)
def rotate_cube_blocked_2(self):
self.rotate_cube_blocked(1, 2)
def rotate_cube_blocked_3(self):
self.rotate_cube_blocked(-1, 1)
def flipper_hold_cube(self, speed=300):
current_position = self.flipper.position
# Push it forward so the cube is always in the same position
# when we start the flip
if (current_position <= MindCuber.hold_cube_pos - 10
or current_position >= MindCuber.hold_cube_pos + 10):
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed),
MindCuber.hold_cube_pos)
sleep(0.05)
def flipper_away(self, speed=300):
"""
Move the flipper arm out of the way
"""
log.info("flipper_away()")
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed), 0)
def flip(self):
"""
Motors will sometimes stall if you call on_to_position() multiple
times back to back on the same motor. To avoid this we call a 50ms
sleep in flipper_hold_cube() and after each on_to_position() below.
We have to sleep after the 2nd on_to_position() because sometimes
flip() is called back to back.
"""
log.info("flip()")
if self.shutdown:
return
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# Grab the cube and pull back
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 0
self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190)
sleep(0.05)
# At this point the cube is at an angle, push it forward to
# drop it back down in the turntable
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(self.flip_speed_push),
MindCuber.hold_cube_pos)
sleep(0.05)
transformation = [2, 4, 1, 3, 0, 5]
self.apply_transformation(transformation)
def colorarm_middle(self):
log.info("colorarm_middle()")
self.colorarm.on_to_position(SpeedDPS(600), -750)
def colorarm_corner(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_corner(%d)" % square_index)
position_target = -580
if square_index == 1:
position_target -= 10
elif square_index == 3:
position_target -= 30
elif square_index == 5:
position_target -= 20
elif square_index == 7:
pass
else:
raise ScanError(
"colorarm_corner was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_edge(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_edge(%d)" % square_index)
position_target = -640
if square_index == 2:
position_target -= 20
elif square_index == 4:
position_target -= 40
elif square_index == 6:
position_target -= 20
elif square_index == 8:
pass
else:
raise ScanError(
"colorarm_edge was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_remove(self):
log.info("colorarm_remove()")
self.colorarm.on_to_position(SpeedDPS(600), 0)
def colorarm_remove_halfway(self):
log.info("colorarm_remove_halfway()")
self.colorarm.on_to_position(SpeedDPS(600), -400)
def scan_face(self, face_number):
log.info("scan_face() %d/6" % face_number)
if self.shutdown:
return
if self.flipper.position > 35:
self.flipper_away(100)
self.colorarm_middle()
self.colors[int(MindCuber.scan_order[self.k])] = self.color_sensor.rgb
self.k += 1
i = 1
target_pos = 115
self.colorarm_corner(i)
# The gear ratio is 3:1 so 1080 is one full rotation
self.turntable.reset()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
1080,
block=False)
self.turntable.wait_until('running')
while True:
# 135 is 1/8 of full rotation
if self.turntable.position >= target_pos:
current_color = self.color_sensor.rgb
self.colors[int(MindCuber.scan_order[self.k])] = current_color
i += 1
self.k += 1
if i == 9:
# Last face, move the color arm all the way out of the way
if face_number == 6:
self.colorarm_remove()
# Move the color arm far enough away so that the flipper
# arm doesn't hit it
else:
self.colorarm_remove_halfway()
break
elif i % 2:
self.colorarm_corner(i)
if i == 1:
target_pos = 115
elif i == 3:
target_pos = 380
else:
target_pos = i * 135
else:
self.colorarm_edge(i)
if i == 2:
target_pos = 220
elif i == 8:
target_pos = 1060
else:
target_pos = i * 135
if self.shutdown:
return
if i < 9:
raise ScanError('i is %d..should be 9' % i)
self.turntable.wait_until_not_moving()
self.turntable.off()
self.turntable.reset()
log.info("\n")
def scan(self):
log.info("scan()")
self.colors = {}
self.k = 0
self.scan_face(1)
self.flip()
self.scan_face(2)
self.flip()
self.scan_face(3)
self.rotate_cube(-1, 1)
self.flip()
self.scan_face(4)
self.rotate_cube(1, 1)
self.flip()
self.scan_face(5)
self.flip()
self.scan_face(6)
if self.shutdown:
return
log.info("RGB json:\n%s\n" % json.dumps(self.colors))
self.rgb_solver = RubiksColorSolverGeneric(3)
self.rgb_solver.enter_scan_data(self.colors)
self.rgb_solver.crunch_colors()
self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict()
log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba))
# This is only used if you want to rotate the cube so U is on top, F is
# in the front, etc. You would do this if you were troubleshooting color
# detection and you want to pause to compare the color pattern on the
# cube vs. what we think the color pattern is.
'''
log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier")
self.rotate_cube(-1, 1)
self.flip()
self.flipper_away()
self.rotate_cube(1, 1)
input('Paused')
'''
def move(self, face_down):
log.info("move() face_down %s" % face_down)
position = self.state.index(face_down)
actions = {
0: ["flip", "flip"],
1: [],
2: ["rotate_cube_2", "flip"],
3: ["rotate_cube_1", "flip"],
4: ["flip"],
5: ["rotate_cube_3", "flip"]
}.get(position, None)
for a in actions:
if self.shutdown:
break
getattr(self, a)()
def run_kociemba_actions(self, actions):
log.info('Action (kociemba): %s' % ' '.join(actions))
total_actions = len(actions)
for (i, a) in enumerate(actions):
if self.shutdown:
break
if a.endswith("'"):
face_down = list(a)[0]
rotation_dir = 1
elif a.endswith("2"):
face_down = list(a)[0]
rotation_dir = 2
else:
face_down = a
rotation_dir = 3
log.info("Move %d/%d: %s%s (a %s)" %
(i, total_actions, face_down, rotation_dir, pformat(a)))
self.move(face_down)
if rotation_dir == 1:
self.rotate_cube_blocked_1()
elif rotation_dir == 2:
self.rotate_cube_blocked_2()
elif rotation_dir == 3:
self.rotate_cube_blocked_3()
log.info("\n")
def resolve(self):
if self.shutdown:
return
cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))]
output = check_output(cmd).decode('ascii')
if 'ERROR' in output:
msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd),
output)
log.error(msg)
print(msg)
sys.exit(1)
actions = output.strip().split()
self.run_kociemba_actions(actions)
self.cube_done()
def cube_done(self):
self.flipper_away()
def wait_for_cube_insert(self):
rubiks_present = 0
rubiks_present_target = 10
log.info('wait for cube...to be inserted')
while True:
if self.shutdown:
break
dist = self.infrared_sensor.proximity
# It is odd but sometimes when the cube is inserted
# the IR sensor returns a value of 100...most of the
# time it is just a value less than 50
if dist < 50 or dist == 100:
rubiks_present += 1
log.info("wait for cube...distance %d, present for %d/%d" %
(dist, rubiks_present, rubiks_present_target))
else:
if rubiks_present:
log.info('wait for cube...cube removed (%d)' % dist)
rubiks_present = 0
if rubiks_present >= rubiks_present_target:
log.info('wait for cube...cube found and stable')
break
time.sleep(0.1)
class Spik3r:
def __init__(self,
sting_motor_port: str = OUTPUT_D,
go_motor_port: str = OUTPUT_B,
claw_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.sting_motor = LargeMotor(address=sting_motor_port)
self.go_motor = LargeMotor(address=go_motor_port)
self.claw_motor = MediumMotor(address=claw_motor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.dis = Display()
self.speaker = Sound()
def sting_by_ir_beacon(self):
while True:
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.sting_motor.on_for_degrees(speed=-75,
degrees=220,
brake=True,
block=False)
self.speaker.play_file(wav_file='/home/robot/sound/Blip 1.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
self.sting_motor.on_for_seconds(speed=-100,
seconds=1,
brake=True,
block=True)
self.sting_motor.on_for_seconds(speed=100,
seconds=1,
brake=True,
block=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def be_noisy_to_people(self):
while True:
if self.color_sensor.reflected_light_intensity > 30:
for i in range(4):
self.speaker.play_file(
wav_file='/home/robot/sound/Blip 2.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
def pinch_if_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.claw_motor.on_for_seconds(speed=50,
seconds=1,
brake=True,
block=True)
self.claw_motor.on_for_seconds(speed=-50,
seconds=0.3,
brake=True,
block=True)
def keep_driving_by_ir_beacon(self):
while True:
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.go_motor.on(speed=91, block=False, brake=False)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.go_motor.on(speed=-100, brake=False, block=False)
else:
self.go_motor.off(brake=True)
def main(self):
self.dis.image_filename(filename='/home/robot/image/Evil.bmp',
clear_screen=True)
self.dis.update()
# FIXME: ValueError: invalid literal for int() with base 10: ''
# when multiple threads access the same Sensor
Thread(target=self.be_noisy_to_people, daemon=True).start()
Thread(target=self.sting_by_ir_beacon, daemon=True).start()
Thread(target=self.pinch_if_touched, daemon=True).start()
self.keep_driving_by_ir_beacon()
