class Ev3Robot: def __init__(self, wheel1=OUTPUT_B, wheel2=OUTPUT_C): self.steer_pair = MoveSteering(wheel1, wheel2) self.gyro = GyroSensor() self.tank_pair = MoveTank(wheel1, wheel2) self.motorB = LargeMotor(OUTPUT_B) self.motorC = LargeMotor(OUTPUT_C) def pivot_right(self, degrees, speed=20): self.tank_pair.on(left_speed=0, right_speed=speed) self.gyro.wait_until_angle_changed_by(degrees - 10) self.tank_pair.off() def pivot_left(self, degrees, speed=20): self.tank_pair.on(left_speed=speed, right_speed=0) self.gyro.wait_until_angle_changed_by(degrees - 10) self.tank_pair.off() def spin_right(self, degrees, speed=20): self.gyro.mode = 'GYRO-ANG' value1 = self.gyro.angle self.tank_pair.on(left_speed=speed, right_speed=speed * -1) self.gyro.wait_until_angle_changed_by(degrees) self.tank_pair.off() value2 = self.gyro.angle self.tank_pair.on(left_speed=-20, right_speed=20) self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees) def spin_left(self, degrees, speed=20): self.gyro.mode = 'GYRO-ANG' value1 = self.gyro.angles self.gyro.reset() self.tank_pair.on(left_speed=speed * -1, right_speed=speed) self.gyro.wait_until_angle_changed_by(degrees) self.tank_pair.off() value2 = self.gyro.angle self.tank_pair.on(left_speed=20, right_speed=-20) self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees) def go_straight_forward(self, cm, speed=30): value1 = self.motorB.position angle0 = self.gyro.angle rotations = cm / 19.05 while (self.motorB.position - value1) / 360 < rotations: self.gyro.mode = 'GYRO-ANG' angle = self.gyro.angle - angle0 print(angle, file=stderr) self.steer_pair.on(steering=angle * -1, speed=speed)
class EV: def __init__(self): self.button = Button() self.tank = MoveSteering(OUTPUT_C, OUTPUT_B) self.cs = ColorSensor() self.cs.mode = ColorSensor.MODE_COL_REFLECT self.gs = GyroSensor() self.gs.reset() self.before_direction = self.gyro() def steer(self, steer, speed=SPEED, interval=INTERVAL): """ steer the motor by given params for time intarval [ms] """ if self.is_white(): # clockwise self.tank.on_for_seconds(steer, speed, interval / 1000) else: self.tank.on_for_seconds(-steer, speed, interval / 1000) data = (self._update_direction(), not self.is_white()) sleep(interval / 1000) return data def turn_degrees(self, radian): self.tank.turn_degrees(SPEED, math.degrees(radian)) def on_for_millis(self, millis): self.tank.on_for_seconds(0, SPEED, millis / 1000) def gyro(self): return self.gs.value() def is_white(self): return self.cs.value() > 30 # def _send(self, data): # data = str(data).encode() # self.socket.send(data) # print(data) def _update_direction(self): current_direction = self.gyro() m_direction = (current_direction + self.before_direction) / 2 self.before_direction = current_direction return m_direction
class GyroCar: def __init__(self, left_motor_port, right_motor_port, gyro_mode): self.__moveSteering = MoveSteering(left_motor_port, right_motor_port) self.__gyro = GyroSensor() self.__gyro.mode = gyro_mode def __run_forward(self): self.__moveSteering.on(0, 20) def run_rotations(self, rotations): self.__moveSteering.on_for_rotations(0, 20, rotations) def stop(self): self.__moveSteering.off() def __back_and_turn(self, steering): self.__moveSteering.on_for_rotations(-steering, 20, -1) def reset_angle(self): self.__gyro.reset() def is_on_flat(self): print("is_on_flat: %d" % self.__gyro.angle) time.sleep(1) return (math.fabs(self.__gyro.angle) < 10) def is_on_slope(self): print("is_on_slope: %d" % self.__gyro.angle) time.sleep(1) return (math.fabs(self.__gyro.angle) >= 10) def run(self): self.__run_forward() @property def angle(self): return self.__gyro.angle
class Gyro(SimplePeriodicWorkerThread): def __init__(self, logger=None): self._logger = logger or logging.getLogger(__name__) super().__init__(thread_name='EV3Gyro') self._gyro = GyroSensor(address='in2') self._gyro.mode = GyroSensor.MODE_GYRO_ANG self._gyro.reset() self._angle = 0 def perform_cycle(self): # Occasionally, the EV3 gyro sensor gives some exeptions. Usually it happens # for a few seconds after it is started. Maybe its initialization is not yet # complete. According to experiments, we can ignore this. try: self._angle = self._gyro.angle except: self._logger.warning('Unable to get angle from EV3 gyro sensor') def get_orientation(self): # It is ok to read a little outdated data return self._angle def reset(self): self._gyro.reset()
class Ev3Robot: def __init__(self, wheel1=OUTPUT_B, wheel2=OUTPUT_C, wheel3=OUTPUT_A, wheel4=OUTPUT_D): self.steer_pair = MoveSteering(wheel1, wheel2) self.gyro = GyroSensor() self.tank_pair = MoveTank(wheel1, wheel2) self.motor1 = LargeMotor(wheel1) self.motor2 = LargeMotor(wheel2) self.motor3 = MediumMotor(wheel3) self.motor4 = MediumMotor(wheel4) self.color1 = ColorSensor(INPUT_1) self.color4 = ColorSensor(INPUT_4) self._black1 = 0 self._black4 = 0 self._white1 = 100 self._white4 = 100 self.gyro.mode = 'GYRO-ANG' self.led = Leds() self.btn = Button() self.btn._state = set() def write_color(self, file, value): # opens a file f = open(file, "w") # writes a value to the file f.write(str(value)) f.close() def read_color(self, file): # opens a file f = open(file, "r") # reads the value color = int(f.readline().strip()) f.close() return color def pivot_right(self, degrees, speed=20): # makes the robot pivot to the right until the gyro sensor # senses that it has turned the required number of degrees self.tank_pair.on(left_speed=speed, right_speed=0) self.gyro.wait_until_angle_changed_by(degrees - 10) self.tank_pair.off() def pivot_left(self, degrees, speed=20): # makes the robot pivot to the left until the gyro sensor # senses that it has turned the required number of degrees self.tank_pair.on(left_speed=0, right_speed=speed) self.gyro.wait_until_angle_changed_by(degrees - 10) self.tank_pair.off() def old_spin_right(self, degrees, speed=20): #old program; don't use self.gyro.reset() value1 = self.gyro.angle self.tank_pair.on(left_speed=speed, right_speed=speed * -1) self.gyro.wait_until_angle_changed_by(degrees) value2 = self.gyro.angle self.tank_pair.on(left_speed=-30, right_speed=30) self.gyro.wait_until_angle_changed_by(value1 - value2 - degrees) self.stop() def old_spin_left(self, degrees, speed=20): #old program; don't use value1 = self.gyro.angle self.tank_pair.on(left_speed=speed * -1, right_speed=speed) self.gyro.wait_until_angle_changed_by(degrees) value2 = self.gyro.angle self.tank_pair.on(left_speed=8, right_speed=-8) self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees + 5) self.tank_pair.off() def dog_gear(self, degrees, motor, speed=30): if motor == 3: self.motor3.on_for_degrees(degrees=degrees, speed=speed) self.motor3.off() else: self.motor4.on_for_degrees(degrees=degrees, speed=speed) def go_straight_forward(self, cm, speed=20, wiggle_factor=1): value1 = self.motor1.position angle0 = self.gyro.angle rotations = cm / 19.05 #divides by circumference of the wheel # calculates the amount of degrees the robot has turned, then turns the # opposite direction and repeats until the robot has gone the required # number of centimeters while abs(self.motor1.position - value1) / 360 < rotations: angle = self.gyro.angle - angle0 self.steer_pair.on(steering=angle * wiggle_factor * -1, speed=speed * -1) self.steer_pair.off() def go_straight_backward(self, cm, speed=20, wiggle_factor=1): # see go_straight_forward value1 = self.motor1.position angle0 = self.gyro.angle rotations = cm / 19.05 while abs(self.motor1.position - value1) / 360 < rotations: angle = self.gyro.angle - angle0 self.steer_pair.on(steering=angle * wiggle_factor, speed=speed) self.steer_pair.off() def calibrate(self): # turns the led lights orange, and waits for a button to be pressed # (signal that the robot is on black), then calculates the reflected # light intensity of the black line, then does the same on the white line self.led.set_color('LEFT', 'ORANGE') self.led.set_color('RIGHT', 'ORANGE') while not self.btn.any(): sleep(0.01) self.led.set_color('LEFT', 'GREEN') self.led.set_color('RIGHT', 'GREEN') self._black1 = self.color1.reflected_light_intensity self._black4 = self.color4.reflected_light_intensity sleep(2) self.led.set_color('LEFT', 'ORANGE') self.led.set_color('RIGHT', 'ORANGE') while not self.btn.any(): sleep(0.01) self.led.set_color('LEFT', 'GREEN') self.led.set_color('RIGHT', 'GREEN') self._white1 = self.color1.reflected_light_intensity self._white4 = self.color4.reflected_light_intensity sleep(3) self.write_color("/tmp/black1", self._black1) self.write_color("/tmp/black4", self._black4) self.write_color("/tmp/white1", self._white1) self.write_color("/tmp/white4", self._white4) def read_calibration(self): # reads the color files self._black1 = self.read_color("/tmp/black1") self._black4 = self.read_color("/tmp/black4") self._white1 = self.read_color("/tmp/white1") self._white4 = self.read_color("/tmp/white4") def align_white(self, speed=20, t=96.8, direction=1): # goes forward until one of the color sensors sees the white line. while self.calibrate_RLI(self.color1) < t and self.calibrate_RLI( self.color4) < t: self.steer_pair.on(steering=0, speed=speed * direction) self.steer_pair.off() # determines which sensor sensed the white line, then moves the opposite # motor until both sensors have sensed the white line if self.calibrate_RLI(self.color4) > t: while self.calibrate_RLI(self.color1) < t: self.motor1.on(speed=speed * direction) self.motor1.off() else: while self.calibrate_RLI(self.color4) < t: self.motor2.on(speed=speed * direction) self.motor2.off() def align_black(self, speed=20, t=4.7, direction=1): # see align_white while self.calibrate_RLI(self.color1) > t and self.calibrate_RLI( self.color4) > t: self.steer_pair.on(steering=0, speed=speed * direction) self.steer_pair.off() if self.calibrate_RLI(self.color4) < t: while self.calibrate_RLI(self.color1) > t: self.motor1.on(speed=speed * direction) self.motor1.off() else: while self.calibrate_RLI(self.color4) > t: self.motor2.on(speed=speed * direction) self.motor2.off() def align(self, t, speed=-20, direction=1): # aligns three times for extra accuracy self.align_white(speed=speed, t=100 - t, direction=direction) self.align_black(speed=-5, t=t, direction=direction) self.align_white(speed=-5, t=100 - t, direction=direction) def calibrate_RLI(self, color_sensor): # returns a scaled value based on what black and white are if (color_sensor.address == INPUT_1): black = self._black1 white = self._white1 else: black = self._black4 white = self._white4 return (color_sensor.reflected_light_intensity - black) / (white - black) * 100 def stop(self): self.tank_pair.off() def spin_right(self, degrees, speed=30): self.turn(degrees, 100, speed) def spin_left(self, degrees, speed=30): self.turn(degrees, -100, speed) def turn(self, degrees, steering, speed=30): # turns at a decreasing speed until it turns the required amount of degrees value1 = self.gyro.angle s1 = speed d1 = 0 while d1 < degrees - 2: value2 = self.gyro.angle d1 = abs(value1 - value2) slope = speed / degrees s1 = (speed - slope * d1) * (degrees / 90) + 3 self.steer_pair.on(steering=steering, speed=s1) self.steer_pair.off()
def Robotrun4(): robot = MoveSteering(OUTPUT_A, OUTPUT_B) tank = MoveTank(OUTPUT_A, OUTPUT_B) colorLeft = ColorSensor(INPUT_1) colorRight = ColorSensor(INPUT_3) gyro = GyroSensor(INPUT_2) motorC = LargeMotor(OUTPUT_C) motorD = LargeMotor(OUTPUT_D) motorB = LargeMotor(OUTPUT_B) motorA = LargeMotor(OUTPUT_A) Constants.STOP = False gyro.reset() GyroDrift() gyro.reset() show_text("Robot Run 2") motorC.off(brake=True) #GyroTurn(steering=-50, angle=5) acceleration(degrees=DistanceToDegree(30), finalSpeed=30) lineFollowPID(degrees=DistanceToDegree(30), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3)) acceleration(degrees=DistanceToDegree(5), finalSpeed=30) accelerationMoveBackward(degrees = DistanceToDegree(7), finalSpeed=50, steering=0) motorC.on_for_seconds(speed=15, seconds=1, brake=False) GyroTurn(steering=50, angle=20) acceleration(degrees=DistanceToDegree(20), finalSpeed=30) GyroTurn(steering=-55, angle=22) acceleration(degrees=DistanceToDegree(17), finalSpeed=30) gyro.mode = "GYRO-ANG" while gyro.value() < -10 and False == Constants.STOP: motorA.on(speed = 20) lineFollowPID(degrees=DistanceToDegree(15), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3)) lineFollowTillIntersectionPID(kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3), color2=ColorSensor(INPUT_1)) lineFollowPID(degrees=DistanceToDegree(10), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3)) lineFollowTillIntersectionPID(kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3), color2=ColorSensor(INPUT_1)) if gyro.angle > 2 and False == Constants.STOP: GyroTurn(steering=-50, angle=gyro.angle) elif gyro.angle < -2 and False == Constants.STOP: ang = -1 * gyro.angle GyroTurn(steering=50, angle=ang) accelerationMoveBackward(degrees = DistanceToDegree(5), finalSpeed=50, steering=0) acceleration(degrees=DistanceToDegree(12), finalSpeed=50, steering=2.5) motorC.on_for_degrees(speed=-25, degrees=150, brake=True) motorD.on_for_degrees(speed=-25, degrees=150, brake=True) acceleration(degrees=DistanceToDegree(12), finalSpeed=45, steering=4) #acceleration(degrees=DistanceToDegree(12), finalSpeed=45, steering=5) #Moving treadmill if False == Constants.STOP: tank.on_for_seconds(left_speed=1, right_speed=20, seconds=5.5) #motorB.on_for_seconds(speed=15, seconds=10) motorC.on_for_seconds(speed=25, seconds=2, brake=False) motorD.on_for_seconds(speed=25, seconds=2, brake=False) accelerationMoveBackward(degrees = DistanceToDegree(5), finalSpeed=20, steering=0) while colorLeft.reflected_light_intensity < Constants.WHITE: robot.on(steering=0, speed=-20) accelerationMoveBackward(degrees = DistanceToDegree(2), finalSpeed=10, steering=0) GyroTurn(steering=-50, angle=gyro.angle) MoveBackwardBlack(10) ang = -1 * (88 + gyro.angle) GyroTurn(steering=-100, angle=ang) # wall square if False == Constants.STOP: robot.on_for_seconds(steering=5, speed=-10, seconds=2.7, brake=False) # moving towards row machine acceleration(degrees=DistanceToDegree(3), finalSpeed=50, steering=0) ang = math.fabs(89 + gyro.angle) show_text(str(ang)) if ang > 2 and False == Constants.STOP: GyroTurn(steering=-100, angle=ang) acceleration(degrees=DistanceToDegree(26), finalSpeed=50, steering=0) GyroTurn(steering=100, angle=68) #acceleration(degrees=DistanceToDegree(1), finalSpeed=20, steering=0) if False == Constants.STOP: motorC.on_for_seconds(speed=-10, seconds=1.5, brake=False) GyroTurn(steering=100, angle=2) sleep(0.2) GyroTurn(steering=-100, angle=2) motorC.on_for_seconds(speed=-10, seconds=0.2, brake=True) motorC.off(brake=True) acceleration(degrees=DistanceToDegree(1), finalSpeed=20, steering=0) accelerationMoveBackward(degrees = DistanceToDegree(10), finalSpeed=20, steering=0) GyroTurn(steering=-100, angle=10) #DOING Row Machine if False == Constants.STOP: motorC.on_for_seconds(speed=20, seconds=2) ang = 90 + gyro.angle GyroTurn(steering=-100, angle=ang) acceleration(degrees=DistanceToDegree(28), finalSpeed=50, steering=0) lineSquare() #Moving towards weight machine show_text(str(gyro.angle)) ang = math.fabs(87 + gyro.angle) show_text(str(ang)) GyroTurn(steering=100, angle=ang) acceleration(degrees=DistanceToDegree(22), finalSpeed=30, steering=0) if False == Constants.STOP: motorD.on_for_degrees(speed=-20, degrees=160) GyroTurn(steering=-100, angle=ang) accelerationMoveBackward(degrees = DistanceToDegree(20), finalSpeed=20, steering=0) if False == Constants.STOP: motorD.on_for_seconds(speed=20, seconds=2, brake=True) lineSquare() if False == Constants.STOP: GyroTurn(steering=-40, angle=85) lineFollowRightPID(degrees=DistanceToDegree(30), kp=1.3, ki=0.01, kd=15, color=colorLeft) lineFollowTillIntersectionRightPID(kp=1.3, ki=0.01, kd=15, color=colorLeft, color2=colorRight) lineFollowRightPID(degrees=DistanceToDegree(34), kp=1.3, ki=0.01, kd=15, color=colorLeft) if False == Constants.STOP: GyroTurn(steering=50, angle=20) acceleration(degrees=DistanceToDegree(12), finalSpeed=30, steering=2) lineSquare() if False == Constants.STOP: GyroTurn(steering=100, angle=75) motorC.on_for_seconds(speed=-10, seconds=1, brake=False) acceleration(degrees=DistanceToDegree(6.5), finalSpeed=30, steering=0) motorC.on_for_seconds(speed=10, seconds=2, brake=True) if False == Constants.STOP: GyroTurn(steering=50, angle=75) acceleration(degrees=DistanceToDegree(5), finalSpeed=30, steering=0) while Constants.STOP == False: acceleration(degrees=DistanceToDegree(3), finalSpeed=31, steering=0) accelerationMoveBackward(degrees = DistanceToDegree(3), finalSpeed=30, steering=0) motorC.off(brake=False) motorD.off(brake=False)
m.on_for_rotations(SpeedPercent(75), 5) #hoi bois!!!! def gyro_test(): m1 = LargeMotor(OUTPUT_A) m2 = LargeMotor(OUTPUT_D) m1.on(20) m2.on(-20) gyro.wait_until_angle_changed_by(90) m1.off() m2.off() # Resets to 0, does not fix drift gyro.reset() def long_gyro_test(): s.beep() show("push a button") #b.wait_for_pressed([Button.enter, Button.right]) #b.wait_for_bump([Button.enter, Button.right]) b.wait_for_pressed(["enter"]) s.beep() gyro_test() #motor_test() #for i in range(50): # Loop until button pressed, displaying gyro angle while not b.any(): ang = gyro.angle
import sys import threading from time import sleep #netstat -nap | grep 5000 running program check # fuser -k -n tcp 5000 port kill sio = socketio.AsyncServer(async_mode='aiohttp', ping_timeout=1500000, ping_interval=600000) app = web.Application() sio.attach(app) gy = GyroSensor(INPUT_2) gy._ensure_mode(gy.MODE_GYRO_CAL) gy._direct = gy.set_attr_raw(gy._direct, 'direct', bytes(17, )) gy._ensure_mode(gy.MODE_GYRO_G_A) gy.reset() motors = MoveTank(OUTPUT_A, OUTPUT_D) #motors.on(50,50) sleep(0.2) motors.off(brake=False) @sio.event async def connect(sid, environ): #raise ConnectionRefusedError('authentication failed') print('connected with Client', sid) steps = 0
class MindstormsGadget(AlexaGadget): """ A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill. """ def __init__(self): """ Performs Alexa Gadget initialization routines and ev3dev resource allocation. """ super().__init__() # Connect two large motors on output ports B and C self.drive = MoveTank(OUTPUT_B, OUTPUT_C) self.grip = MediumMotor(OUTPUT_A) self.sound = Sound() self.leds = Leds() self.ir = InfraredSensor() self.touch = TouchSensor() self.gyro = GyroSensor() self.isComing = False self.isTaking = False self.isBringing = False self.isTurning = False # Start threads threading.Thread(target=self._proximity_thread, daemon=True).start() def on_connected(self, device_addr): """ Gadget connected to the paired Echo device. :param device_addr: the address of the device we connected to """ self.leds.set_color("LEFT", "GREEN") self.leds.set_color("RIGHT", "GREEN") logger.info("{} connected to Echo device".format(self.friendly_name)) def on_disconnected(self, device_addr): """ Gadget disconnected from the paired Echo device. :param device_addr: the address of the device we disconnected from """ self.leds.set_color("LEFT", "BLACK") self.leds.set_color("RIGHT", "BLACK") logger.info("{} disconnected from Echo device".format( self.friendly_name)) def on_custom_mindstorms_gadget_control(self, directive): """ Handles the Custom.Mindstorms.Gadget control directive. :param directive: the custom directive with the matching namespace and name """ try: payload = json.loads(directive.payload.decode("utf-8")) print("Control payload: {}".format(payload), file=sys.stderr) control_type = payload["type"] if control_type == "move": # Expected params: [direction, duration, speed] self._move(payload["direction"], int(payload["duration"]), int(payload["speed"])) elif control_type == "come": self._come() elif control_type == "take": self._take() elif control_type == "bring": self._bring() except KeyError: print("Missing expected parameters: {}".format(directive), file=sys.stderr) def _come(self, duration=10, speed=50): self.leds.set_color("LEFT", "GREEN", 1) self.leds.set_color("RIGHT", "GREEN", 1) self.isComing = True self._move(Direction.FORWARD.value[0], duration, speed) def _take(self, duration=20, speed=50): self.grip.on_for_rotations(SpeedPercent(100), 1) self.leds.set_color("LEFT", "GREEN", 1) self.leds.set_color("RIGHT", "GREEN", 1) self.gyro.reset() self.gyro.mode = 'GYRO-RATE' self.gyro.mode = 'GYRO-ANG' self.isTurning = True self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3) self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40) self.isTaking = True self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), duration) def _bring(self, duration=20): self.isTurning = True self.gyro.mode = 'GYRO-RATE' self.gyro.mode = 'GYRO-ANG' self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3) self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40) self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), 1.5) self.grip.on_for_rotations(SpeedPercent(100), 1) self.isTurning = True self.gyro.mode = 'GYRO-RATE' self.gyro.mode = 'GYRO-ANG' self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3) self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40) self.isBringing = True self.now = time.time() self.drive.on_for_seconds(SpeedPercent(50), SpeedPercent(50), duration) self.leds.set_color("LEFT", "GREEN", 1) self.leds.set_color("RIGHT", "GREEN", 1) def _move(self, direction, duration: int, speed: int, is_blocking=False): """ Handles move commands from the directive. Right and left movement can under or over turn depending on the surface type. :param direction: the move direction :param duration: the duration in seconds :param speed: the speed percentage as an integer :param is_blocking: if set, motor run until duration expired before accepting another command """ print("Move command: ({}, {}, {}, {})".format(direction, speed, duration, is_blocking), file=sys.stderr) if direction in Direction.FORWARD.value: self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking) if direction in Direction.BACKWARD.value: self.drive.on_for_seconds(SpeedPercent(-speed), SpeedPercent(-speed), duration, block=is_blocking) if direction in (Direction.RIGHT.value + Direction.LEFT.value): self._turn(direction, speed) self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking) if direction in Direction.STOP.value: self.drive.off() self.patrol_mode = False def _turn(self, direction, speed): """ Turns based on the specified direction and speed. Calibrated for hard smooth surface. :param direction: the turn direction :param speed: the turn speed """ if direction in Direction.LEFT.value: self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2) if direction in Direction.RIGHT.value: self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2) def _send_event(self, name: EventName, payload): """ Sends a custom event to trigger a sentry action. :param name: the name of the custom event :param payload: the sentry JSON payload """ self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload) def _proximity_thread(self): """ Monitors the distance between the robot and an obstacle when sentry mode is activated. If the minimum distance is breached, send a custom event to trigger action on the Alexa skill. """ while True: distance = self.ir.proximity angle = self.gyro.angle #print("Proximity: {}".format(distance), file=sys.stderr) if self.isTurning == True: print("angle: {}".format(angle), file=sys.stderr) self.leds.set_color("LEFT", "YELLOW", 1) self.leds.set_color("RIGHT", "YELLOW", 1) if abs(angle) >= 179 or abs(angle) <= -181: self.isTurning = False self._move( Direction.STOP.value[0], 0, 0, ) self.gyro.reset() self.gyro.mode = 'GYRO-RATE' self.gyro.mode = 'GYRO-ANG' self.leds.set_color("LEFT", "GREEN", 1) self.leds.set_color("RIGHT", "GREEN", 1) if distance <= 55: """ When the bot is coming, it will stop and open up it's arm """ if self.isComing == True: self.isComing = False print("Proximity breached, stopping") self._move( Direction.STOP.value[0], 0, 0, ) self.leds.set_color("LEFT", "RED", 1) self.leds.set_color("RIGHT", "RED", 1) while not self.touch.is_pressed: self.grip.on_for_degrees(SpeedPercent(10), -90) print("Lowered the grip") elif self.isTaking == True: self.isTaking = False print("Proximity breached, stopping") self._move( Direction.STOP.value[0], 0, 0, ) self.leds.set_color("LEFT", "RED", 1) self.leds.set_color("RIGHT", "RED", 1) while not self.touch.is_pressed: self.grip.on_for_degrees(SpeedPercent(10), -90) print("Dropping Item") self.drive.on_for_seconds(SpeedPercent(-50), SpeedPercent(-50), 1) self.gyro.reset() self.isTurning = True self.gyro.mode = 'GYRO-RATE' self.gyro.mode = 'GYRO-ANG' self.drive.on_for_seconds(SpeedPercent(100), SpeedPercent(-100), 1.3) self.drive.on_for_seconds(SpeedPercent(4), SpeedPercent(-4), 40, block=False) self.leds.set_color("LEFT", "GREEN", 1) self.leds.set_color("RIGHT", "GREEN", 1) elif self.isBringing == True: self.isBringing = False print("Proximity breached, stopping") self._move(Direction.STOP.value[0], 0, 0) self.later = time.time() self.leds.set_color("LEFT", "RED", 1) self.leds.set_color("RIGHT", "RED", 1) while not self.touch.is_pressed: self.grip.on_for_degrees(SpeedPercent(10), -90) print("Dropping Item") difference = int(self.later - self.now) self.drive.on_for_seconds(SpeedPercent(-50), SpeedPercent(-50), difference - 0.5) time.sleep(0.2)
class CarControl: def __init__(self): self.tank_pair = MoveTank(OUTPUT_A, OUTPUT_B) self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B) self.line_sensor = ColorSensor(INPUT_2) self.line_counter = ColorSensor(INPUT_3) self.sound = Sound() self.gyro = GyroSensor(INPUT_1) self.gyro.reset() self.dir_state = "S" def drive(self, l_speed, r_speed): self.tank_pair.on(left_speed=l_speed, right_speed=r_speed) def reverse(self): self.tank_pair.on_for_degrees(left_speed=-90, right_speed=-90, degrees=360, brake=True) def brake(self): self.tank_pair.off(brake=True) def turn_left(self): self.steer_pair.on_for_degrees(steering=-100, speed=90, degrees=180, brake=True) def turn_right(self): self.steer_pair.on_for_degrees(steering=100, speed=90, degrees=170, brake=True) def turn_arround(self): self.steer_pair.on_for_degrees(steering=-100, speed=90, degrees=370, brake=True) def read_line_sensor(self): return (self.line_sensor.reflected_light_intensity) def read_line_counter(self): return (self.line_counter.reflected_light_intensity) def play_sound(self): self.sound.beep() def read_gyro(self): return self.gyro.angle def turn_left_new(self): self.tank_pair.on(left_speed=-90, right_speed=90) if self.dir_state == "S": while True: if self.read_gyro( ) <= -90: #Ikke sikker på at den altid kan læse ved præcist -90 self.tank_pair.off(brake=True) self.dir_state = "L" return 0 elif self.dir_state == "L": while True: if self.read_gyro() == 180 or self.read_gyro() == -180: self.steer_pair.stop() self.dir_state = "B" return 0 elif self.dir_state == "B": while True: if self.read_gyro() == 90 or self.read_gyro() == -270: self.steer_pair.stop() self.dir_state = "R" return 0 elif self.dir_state == "R": while True: if self.read_gyro( ) == 0 or self.read_gyro == -360 or self.read_gyro == 360: self.gyro.reset() self.steer_pair.stop() self.dir_state = "S" return 0 def turn_right_new(self): self.steer_pair.on(90, 90) print("JEG ER HER") if self.dir_state == "S": while True: if self.read_gyro == 90: self.steer_pair.stop() self.dir_state = "R" return 0 elif self.dir_state == "L": while True: if self.read_gyro() == 360 or self.read_gyro() == 0: self.gyro.reset() self.steer_pair.stop() self.dir_state = "S" return 0 elif self.dir_state == "B": while True: if self.read_gyro() == 270 or self.read_gyro == -90: self.steer_pair.stop() self.dir_state == "R" return 0 elif self.dir_state == "R": while True: if self.read_gyro() == 180 or self.read_gyro() == -180: self.steer_pair.stop() self.dir_state == "B" return 0
class SimpleRobotDriveBase(MoveDifferential): ''' A simplified drive base class. Args: leftDriveMotor: motor port for left side drive motor rightDriveMotor: motor port for right side drive motor tire: tire to use axle_track: distance between centerline of drive wheels (mm) leftMotor: port for left side attachment motor rightMotor: port for right side attachment motor leftColor: sensor port for left side color sensor rightColor: sensor port for right side color sensor centerColor: sensor port for center color sensor useGyro (bool): enable gyro functions ''' def __init__(self, leftDriveMotor, rightDriveMotor, tire, axle_track, leftMotor, rightMotor, leftColor, rightColor, centerColor, useGyro=False): ''' Initialize the SimpleRobotDriveBase. ''' self.defaultSpeed = 200 # mm/s self.defaultTurnTime = 2 # time to complete turn in place (s) self.blackThreshold = 10 # values less than this are black self.whiteThreshold = 55 # values more than this are white self.defaultTimeout = 15000 # Set up two drive motors self.leftDriveMotor = leftDriveMotor self.rightDriveMotor = rightDriveMotor # set up output shafts self.leftMotor = leftMotor self.rightMotor = rightMotor # aliases for output shafts when using XY table self.xMotor = self.leftMotor self.yMotor = self.rightMotor # set up color sensors self.leftColor = leftColor self.rightColor = rightColor self.centerColor = centerColor # set up gryo sensor if useGyro: self.gyro = GyroSensor() self.gyro.reset() # store wheel diameter self.wheel_diameter = tire().diameter_mm # initialize the parent class: MoveDifferential super().__init__(leftDriveMotor.address, rightDriveMotor.address, tire, axle_track) # # Invert motor polarities: positive drives CCW # rightDriveMotor.polarity = "inversed" # leftDriveMotor.polarity = "inversed" # set the ratio of motor angle to linear motion (deg/mm) self.xRatio = 2392 / 144 # deg/mm self.yRatio = 1695 / 80 # deg/mm # set time to accelerate or deccelerate to 100% speed (ms) self.ramp_time = 1000 self.leftDriveMotor.ramp_up_sp = self.ramp_time self.leftDriveMotor.ramp_down_sp = self.ramp_time self.rightDriveMotor.ramp_up_sp = self.ramp_time self.rightDriveMotor.ramp_down_sp = self.ramp_time self.leftMotor.ramp_up_sp = self.ramp_time self.leftMotor.ramp_down_sp = self.ramp_time self.rightMotor.ramp_up_sp = self.ramp_time self.rightMotor.ramp_down_sp = self.ramp_time def speed_mm_s(self, speed): ''' Convert speed in mm/s into percent of max speed. Args: speed: speed in mm/s. Returns: speed in SpeedRPS form. ''' rps = speed / (pi * self.wheel_diameter) speed = SpeedRPS(rps) return speed def driveArc(self, distance, radius, speed=200, direction=Direction.CW, brake=True, block=True): ''' Drive for given distance along arc of specified radius. Args: distance: distance to drive in mm radius: radius of arc in mm speed: speed in mm/s direction: direction to travel along arc -- Direction.CW or Direction.CCW brake (bool): brake at end of move block (bool): block until move is complete ''' speed = self.speed_mm_s(speed) self.enableRampDown(brake) if direction == Direction.CW: self.on_arc_right(speed, radius, distance, brake, block) if direction == Direction.CCW: self.on_arc_left(speed, radius, distance, brake, block) self.enableRampDown(True) def pointTurn(self, degrees_to_turn, speed=8, brake=True, block=True): ''' Turn in place. Args: degrees_to_turn: degrees to turn (positive to right) speed: percentage of maximum speed brake (bool): brake at end of move block (bool): block until move is complete ''' self.enableRampDown(brake) if degrees_to_turn >= 0: self.turn_right(speed, degrees_to_turn, brake, block) else: self.turn_left(speed, degrees_to_turn, brake, block) self.enableRampDown(True) def pointTurnGyro(self, degrees_to_turn, speed=10, brake=True, block=True): ''' Turn in place using gyro sensor. Args: degrees_to_turn: degrees to turn (positive to right) speed: percentage of maximum speed brake (bool): brake at end of move block (bool): block until move is complete (unused for now) ''' startAngle = self.gyro.angle # start rotation if degrees_to_turn >= 0: self.turn_right(speed, 5000, brake, False) else: self.turn_left(speed, 5000, brake, False) # loop until we hit target angle while self.gyro.angle - startAngle < degrees_to_turn: sleep(.01) # stop both motors self.stop(brake=brake) def enableRampDown(self, enable): ''' Enable or disable the motor decceleration for drive motors. Args: enable (bool): Enable deccleration if True ''' if enable: self.leftDriveMotor.ramp_down_sp = self.ramp_time self.rightDriveMotor.ramp_down_sp = self.ramp_time else: self.leftDriveMotor.ramp_down_sp = 0 self.rightDriveMotor.ramp_down_sp = 0 def driveStraight(self, distance, speed=None, brake=True, block=True): ''' Drive for given distance. Args: distance: distance to drive in mm speed: speed in mm/s. Uses default speed if not specified. brake (bool): brake at end of movement block (bool): block until move is complete ''' if speed == None or speed == 0: speed = self.defaultSpeed speed = self.speed_mm_s(speed) self.enableRampDown(brake) self.on_for_distance(speed, distance, brake, block) self.enableRampDown(True) def driveStraightForTime(self, time, speedLeft=None, speedRight=None, brake=True, block=True): ''' Drive for given time. Args: time: time for move in s speedLeft: speed in mm/s (left motor) speedRight: speed in mm/s (right motor) brake (bool): brake at end of movement block (bool): block until move is complete ''' if speedLeft == None or speedLeft == 0: speedLeft = self.defaultSpeed speedLeft = self.speed_mm_s(speedLeft) if speedRight == None or speedRight == 0: speedRight = self.defaultSpeed speedRight = self.speed_mm_s(speedRight) self.enableRampDown(brake) self.on_for_seconds(speedLeft, speedRight, time, brake, block) self.enableRampDown(True) def driveStraightForDistance(self, distance, speedLeft=None, speedRight=None, stopAtEnd=True): ''' Drive for given distance. Args: distance: distance to drive in mm speedLeft: speed in mm/s (left motor) speedRight: speed in mm/s (right motor) stopAtEnd (bool): brake at end of movement ''' if speedLeft == None or speedLeft == 0: speedLeft = self.defaultSpeed if speedRight == None or speedRight == 0: speedRight = self.defaultSpeed degPerSecLeft = 360 * speedLeft / (pi * self.wheel_diameter) degPerSecRight = 360 * speedRight / (pi * self.wheel_diameter) driveTime = StopWatch() driveTime.start() self.leftDriveMotor.on(SpeedNativeUnits(degPerSecLeft), block=False) self.rightDriveMotor.on(SpeedNativeUnits(degPerSecRight), block=False) while driveTime.value_ms <= abs(distance / speedLeft) * 1000: sleep(0.02) if stopAtEnd: self.leftDriveMotor.stop() self.rightDriveMotor.stop() sleep(0.01) def driveStraightGyro(self, distance, speed=None, brake=False): ''' Drive for given distance using gyro sensor. Robot goes in straight line in the direction it was facing when this function is called. Uses proportional control. Args: distance: distance to drive in mm speed: speed in mm/s brake (bool): brake at end of movement ''' # starting angle for this move startAngle = self.gyro.angle if speed == None or speed == 0: speed = self.defaultSpeed if distance < 0: distance = abs(distance) speed = -speed assert ( distance > 0 or speed > 0 ), "At least one of distance or speed must be posiitve in driveStraightGyro." # calculate motor speeds in percentage units rps = speed / (pi * self.wheel_diameter) left_speed = rps right_speed = rps # set encoder counts to zero self.rightDriveMotor.position = 0 # proportional gain (rps/deg) kp = 0.01 # number of wheel rotations in this move targetRotations = distance / (pi * self.wheel_diameter) while abs(self.rightDriveMotor.rotations) < targetRotations: error = self.gyro.angle - startAngle left_speed = SpeedRPS(rps - kp * error) right_speed = SpeedRPS(rps + kp * error) self.on(left_speed, right_speed) sleep(0.01) # stop both motors self.stop(brake=brake) def followLineDistNative(self, distance, sensor, sideToFollow, stopAtEnd, speed, gain_mult=1, brake=False): ''' Drive forward following line. Args: distance: distance to follow line in mm sensor: colorsensor to use for line following sideToFollow: which side of line to follow: LineEdge.LEFT or LineEdge.RIGHT stopAtEnd (bool): stop at end of movement speed: speed in mm/s gain_mult: multiplier for P and D gains brake (bool): brake at end of move ''' # calculate the target reflectance, halfway between black ad white target = (self.blackThreshold + self.whiteThreshold) / 2 # set parameters for which side to follow if sideToFollow == LineEdge.LEFT: followLeft = True else: followLeft = False # calculate the required motor rotation rate in RPS rotationRate = speed / (pi * self.wheel_diameter) # time to follow line timeToFollow_ms = abs(distance / speed * 1000) # set which color sensor to use self.cs = sensor # call line follower built-in function self.follow_line(kp=5 * gain_mult, ki=0 * gain_mult, kd=1 * gain_mult, speed=SpeedRPS(rotationRate), target_light_intensity=target, follow_left_edge=followLeft, white=self.whiteThreshold, off_line_count_max=100, sleep_time=0.01, follow_for=follow_for_ms, ms=timeToFollow_ms) self.stop(brake=brake) def followLineDist(self, distance, sensor, sideToFollow, stopAtEnd, speed, gain_multiplier=1): ''' Drive forward following line. Uses direct control of drive motors and PD control. Args: distance: distance to follow line in mm sensor: colorsensor to use for line following sideToFollow: which side of line to follow: LineEdge.LEFT or LineEdge.RIGHT stopAtEnd (bool): Stop motors at end of line following speed: speed in mm/s gain_multiplier: multiplier for P and D gains ''' degPerSec = 360 * speed / (pi * self.wheel_diameter) propGain = 6.0 * gain_multiplier derivGain = 6 * gain_multiplier target = (self.blackThreshold + self.whiteThreshold) / 2 print("******* starting line following ********") # print("error,Pterm,Dterm") # initialize term for derivative calc previousError = target - avgReflection(sensor, 2) i = 0 driveTime = StopWatch() driveTime.start() while driveTime.value_ms <= abs(distance / speed) * 1000: # calc error and proportional term error = target - avgReflection(sensor, 2) Pterm = propGain * error # calc d(error)/d(t) and derivative term d_error_dt = error - previousError Dterm = derivGain * d_error_dt previousError = error if sideToFollow == LineEdge.RIGHT: self.leftDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm + Dterm), block=False) self.rightDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm - Dterm), block=False) # eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging if sideToFollow == LineEdge.LEFT: self.leftDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm - Dterm), block=False) self.rightDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm + Dterm), block=False) # eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging # eprint("line following complete") if stopAtEnd: self.leftDriveMotor.stop() self.rightDriveMotor.stop() sleep(0.01) # Play sound when we stop line following sound.beep() def homeXY(self, speed=60): ''' Move xy table to home position. Args: speed: linear speed of the axes (mm/s) ''' # self.xMotor.set_dc_settings(80, 0) self.xMotor.position = 0 self.yMotor.position = 0 self.gotoXY(-200, -200, speed) self.xMotor.position = 0 self.yMotor.position = 0 # self.xMotor.set_dc_settings(100, 0) def homeXY2(self, speed=60): ''' Move xy table to home position. Args: speed: linear speed of the axes (mm/s) ''' eprint("starting homeXY") # run motors at fixed duty cycle self.yMotor.duty_cycle_sp = -90 self.xMotor.run_direct(duty_cycle_sp=-50) self.yMotor.run_direct(duty_cycle_sp=-80) # sleep until motors stop (or timeout is reached) xExitType = self.xMotor.wait_until_not_moving(self.defaultTimeout) yExitType = self.yMotor.wait_until_not_moving(self.defaultTimeout) # set xMotor and yMotor encoders to zero self.xMotor.position = 0 self.yMotor.position = 0 eprint("HomeXY complete.", xExitType, yExitType) def gotoXY(self, x, y, speed=30, block=True): ''' Move to absolute position (x,y). moves axes simultaneously. homeXY must be called first. Args: x: position of the x-axis (mm) y: position of the y-axis (mm) speed: the speed of the axes (mm/s) block (bool): block until move is complete ''' # start move to x position self.xMotor.on_to_position(SpeedNativeUnits(speed * self.xRatio), x * self.xRatio, brake=False, block=False) # start move to y position self.yMotor.on_to_position(SpeedNativeUnits(speed * self.yRatio), y * self.yRatio, brake=False, block=False) if block: # sleep until either move is complete or motors stall self.xMotor.wait_until_not_moving(self.defaultTimeout) self.yMotor.wait_until_not_moving(self.defaultTimeout) def incrementXY(self, dx, dy, speed=30): ''' Move xy table by a given amount. Moves axes simultaneously. Args: dx: amount to move x-axis (mm) dy: amount to move y-axis (mm) speed: the speed of the axes (mm/s) ''' # get current table positions in mm from home position x = self.xMotor.position / self.xRatio y = self.yMotor.position / self.yRatio eprint("XY table position", x, y) self.gotoXY(x + dx, y + dy, speed) def squareToLine(self): ''' Position robot perpendicular to a line. ''' # drive forward until one sensor is on edge of line. firstSensor = self.stopOnLine() sound.beep() target = (self.blackThreshold + self.whiteThreshold) / 2 # target = self.blackThreshold targetTolerance = 2 # how close we need to be to target reflectance leds.set_color('LEFT', 'RED') # drive opposite wheel forward to square robot on line if firstSensor == self.leftColor: # left sensor on line eprint("found left sensor first") # start right wheel rotating self.rightDriveMotor.speed_sp = 30 self.rightDriveMotor.run_forever() self.leftDriveMotor.speed_sp = -30 self.leftDriveMotor.run_forever() # drive slowly forward until sensor hits white while True: if avgReflection(self.rightColor) >= self.whiteThreshold: # found a white line eprint("found white") break sleep(0.010) while True: if abs(avgReflection(self.rightColor) - target) < targetTolerance: # we are centered over edge of line self.rightDriveMotor.stop() self.leftDriveMotor.stop() eprint("found line edge") break sleep(0.010) else: # right sensor is on line # start left wheel rotating eprint("found right sensor first") self.leftDriveMotor.on(30) while True: if avgReflection(self.leftColor) >= self.whiteThreshold: # found a white line break while True: if abs(avgReflection(self.leftColor) - target) < targetTolerance: # we are centered over edge of line self.leftDriveMotor.stop() break sleep(0.010) # set brick light when we are done leds.set_color('LEFT', 'GREEN') # play long beep sound.beep() eprint(avgReflection(self.rightColor), avgReflection(self.leftColor, 5)) def stopOnLine(self, driveForward=True, brake=False): ''' Stop when one sensor is centered over edge of a line. Drives slowly to line. Args: driveForward (bool): drive in forward direction brake (bool): brake at end of move Returns: firstSensor: the sensor that hit line first ''' # calculate the target sensor reading: halfway between black and white target = (self.blackThreshold + self.whiteThreshold) / 2 # set brick light while we are seeking white leds.set_color('LEFT', 'RED') if driveForward: # drive slowly until at least one sensor hits white self.on(left_speed=5, right_speed=5) # speeds in percent else: # drive slowly until at least one sensor hits white self.on(left_speed=-5, right_speed=-5) # speeds in percent while True: if avgReflection(self.leftColor) >= self.whiteThreshold: # found a white line break if avgReflection(self.rightColor) >= self.whiteThreshold: # found a white line break sleep(0.010) # set brick light while we are seeking the line leds.set_color('LEFT', 'ORANGE') targetTolerance = 5 # how close we need to be to target reflectance # continue driving until we see the target reflectance while True: if abs(avgReflection(self.leftColor) - target) < targetTolerance: # we are centered over edge of line firstSensor = self.leftColor # left sensor hit first eprint("left sensor hit first in stop on line") break if abs(avgReflection(self.rightColor) - target) < targetTolerance: # we are centered over edge of line firstSensor = self.rightColor # right sensor hit first eprint("right sensor hit first in stop on line") break sleep(0.010) # stop moving self.stop(brake=brake) return (firstSensor) def stopOnLineOneSensor(self, sensor, driveForward=False, brake=False): ''' Stop when given sensor is centered over edge of a line. Drives slowly to line. Args: sensor: Color sensor to use Drive forward (bool): weather to drive forward brake (bool): brake at end of move Returns: distanceTravelled: distance travelled in mm ''' # calculate the target sensor reading: halfway between black and white target = (self.blackThreshold + self.whiteThreshold) / 2 # set brick light while we are seeking white leds.set_color('LEFT', 'RED') # set encoder counts to zero self.rightDriveMotor.position = 0 if driveForward: # drive slowly until at least one sensor hits white self.on(left_speed=15, right_speed=15) # speeds in percent else: # drive slowly until at least one sensor hits white self.on(left_speed=-15, right_speed=-15) # speeds in percent while avgReflection(sensor) < self.whiteThreshold: sleep(0.010) targetTolerance = 5 # how close we need to be to target reflectance leds.set_color('LEFT', 'ORANGE') # continue driving until we see the target reflectance while abs(avgReflection(sensor) - target) > targetTolerance: sleep(0.010) # stop moving self.stop(brake=brake) leds.set_color('LEFT', 'GREEN') distanceTravelled = self.rightDriveMotor.rotations * self.wheel_diameter * pi eprint("Distance travelled:", distanceTravelled) return distanceTravelled