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