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 __init__(self, sensorList=[]): try: self.tank = MoveTank(OUTPUT_B, OUTPUT_C) self.outputList = [OUTPUT_B, OUTPUT_C] except: self.tank = None try: self.cs = ColorSensor() except: self.cs = None try: self.gyro = GyroSensor() except: self.gyro = None try: self.ultrasonicSensor = UltrasonicSensor() except: self.ultrasonicSensor = None try: self.large = LargeMotor(OUTPUT_D) self.outputList.append(OUTPUT_D) except: self.large = None try: self.medium = MediumMotor(OUTPUT_D) self.outputList.append(OUTPUT_D) except: self.medium = None
def __init__(self, output_a, output_d): self.right_motor = LargeMotor(output_a) self.left_motor = LargeMotor(output_d) self.btn = Button() self.cl = ColorSensor() self.us = UltrasonicSensor() self.us.mode='US-DIST-CM' self.gy = GyroSensor() self.gy.mode='GYRO-ANG' self.defaultSpeed = 450 self.right_sp = 450 self.left_sp = 450 self.lastColor1 = None self.lastColor2 = None self.lastColor3 = None self.node = [(0,0)] # self.direction = None self.directionStr = ['top', 'right', 'bottom', 'left']
def gyroRotateAbsoluteAngle(): """Test code for rotating using the gyror""" tank_drive = MoveTank(OUTPUT_A, OUTPUT_D) gyro = GyroSensor() logger = logToDisplay() for sleepTime in [2, 0.5]: gyro.mode = 'GYRO-RATE' gyro.mode = 'GYRO-ANG' sleep(sleepTime) startingAngle = gyro.angle endingAngle = 90 while True: currentAngle = gyro.angle logger.log("Current angle {}".format(currentAngle)) if (currentAngle >= endingAngle - 2 and currentAngle <= endingAngle + 2): tank_drive.stop() break elif (currentAngle > endingAngle): leftSpeed = SpeedPercent(-5) rightSpeed = SpeedPercent(5) else: leftSpeed = SpeedPercent(5) rightSpeed = SpeedPercent(-5) tank_drive.on(leftSpeed, rightSpeed) sleep(0.1)
def __init__(self): """Constructor for the internal state of the robot, e.g. in which direction he is currently looking""" self.direction = Directions.up self.gy = GyroSensor() self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_D, motor_class= LargeMotor) self.zero_point = ColorSensor().reflected_light_intensity self.s = Sensors()
def __init__(self, *args, **kwargs): self.exposed_tank_left = MoveTank(LargeMotor(OUTPUT_A), LargeMotor(OUTPUT_B)) self.exposed_tank_right = MoveTank(LargeMotor(OUTPUT_C), LargeMotor(OUTPUT_D)) self.exposed_gyro_up = GyroSensor(INPUT_1) self.exposed_gyro_side = GyroSensor(INPUT_2) self.exposed_stop = TouchSensor(INPUT_3) super().__init__(*args, **kwargs)
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 Calibrate(): gs = GyroSensor(INPUT_2) debug_print('GS Calibration begin') for x in range(2): gs.mode = 'GYRO-RATE' gs.mode = 'GYRO-ANG' time.sleep(.5) gsnow = gs.angle debug_print('GS Calibration finish ' + str(gsnow))
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 __init__(self, wheel1 = OUTPUT_B, wheel2 = OUTPUT_C): self.steer_pair = MoveSteering(wheel1, wheel2) self.gyro = GyroSensor() self.tank_pair = MoveTank(wheel1, wheel2) self.motor1 = LargeMotor(wheel1) self.motor2 = LargeMotor(wheel2) self.color1 = ColorSensor(INPUT_1) self.color4 = ColorSensor(INPUT_4) self._black1 = 0 self._black4 = 0 self._white1 = 100 self._white4 = 100
def __init__(self, motor1Port, motor2Port, gyroPort=None, wheelDiameter=None): #init if GyroSensor != None: self.GS = GyroSensor(gyroPort) else: self.GS = GyroSensor() self.M1 = LargeMotor(motor1Port) self.M2 = LargeMotor(motor2Port) self.motor_stop = True self.wheelDiameter = wheelDiameter self.time = 0 self.MDistanceRunning = True self.distance = 0 self.pauseDistance = [] self.GS.mode = 'GYRO-ANG'
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
def gyroMove(): """Test code for gyro PID drive""" gyro_drive = MoveTankWithGyro(OUTPUT_A, OUTPUT_D) gyro_drive.gyro = GyroSensor() sleep(2) gyro_drive.calibrate_gyro() target_angle = 90 # Pivot 90 degrees gyro_drive.pivot_gyro( speed=SpeedPercent(5), target_angle=target_angle ) # Drive straight at the angle gyro_drive.follow_gyro( kp=11.3, ki=0.05, kd=3.2, speed=SpeedPercent(30), target_angle=target_angle, follow_for=follow_for_ms, ms=5000 )
def initmotors(): lw = Motor(OUTPUT_C) rw = Motor(OUTPUT_B) # for x in (lw,rw): # x.polarity = 'inversed' # x.ramp_up_sp = 2000 # x.ramp_down_sp = 2000 # lw.polarity = 'inversed' # rw.polarity = 'inversed' lw.ramp_up_sp = 2000 rw.ramp_up_sp = 2000 lw.ramp_down_sp = 1000 rw.ramp_down_sp = 1000 global mdiff global mtank mdiff = MoveDifferential(OUTPUT_C, OUTPUT_B, MCTire, 129.3) mtank = MoveTank(OUTPUT_C, OUTPUT_B) mtank.gyro = GyroSensor() mdiff.set_polarity('inversed')
def __init__(self): self.tankDrive = MoveTank(OUTPUT_A, OUTPUT_D) self.motorA = LargeMotor(OUTPUT_A) self.motorD = LargeMotor(OUTPUT_D) self.myGyroSensor = GyroSensor() self.myGripper = RobotGripper() self.myUltrasonicSensor = UltrasonicSensor() self.myColorSensor = ColorSensor() self.Turning = False self.Moving = False self.myAngle = 0 self.positionX = 0 self.positionY = 0 self.myDefaultSpeed = 50 self.desiredBarCode = "BWWB" self.myGPS = IGPS()
def main(): '''The main function of our program''' # set the console just how we want it reset_console() set_cursor(OFF) set_font('Lat15-Terminus24x12') # print something to the output panel in VS Code debug_print('Hello VS Code!') Xob = 8 Yob = 8 v = 1 x = 0 y = 0 tetha = 0 l = 4 xc = [x] yc = [y] motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C) gy = GyroSensor() gy.mode = 'GYRO-RATE' gy.mode = 'GYRO-ANG' units = gy.units angle = gy.value() print(str(angle)) sleep(1) while True: x = -v * math.sin(tetha) + x y = v * math.cos(tetha) + y deltaX = (Xob - x) * math.cos(tetha) + (Yob - y) * math.sin(tetha) curvatura = -(2 * deltaX)/(l**2) tetha += v * curvatura xc.append(x) yc.append(y) girar(tetha, gy, ang, motor_pair) avanzar(motor_pair, gy) sleep(1) print(str(x) + " - " + str(y)) if math.fabs(x) >= Xob or math.fabs(y) >= Yob: break
def GyroDrift(gyro=GyroSensor(INPUT_2)): sound = Sound() gyro.mode = 'GYRO-RATE' while math.fabs(gyro.rate) > 0: show_text("Gyro drift rate: " + str(gyro.rate)) sound.beep() sleep(0.5) gyro.mode='GYRO-ANG'
def setup(): global tank_drive, colorSensorLeft, colorSensorRight, ultra, sound, touchSensor, gyroSensor tank_drive = MoveTank(OUTPUT_A, OUTPUT_B) tank_drive.gyro = GyroSensor('in4') colorSensorLeft = ColorSensor('in1') colorSensorRight = ColorSensor('in2') colorSensorLeft.mode = 'COL-COLOR' colorSensorRight.mode = 'COL-COLOR' #ultra = UltrasonicSensor('in3') touchSensor = TouchSensor('in3') gyroSensor = GyroSensor('in4') gyroSensor.calibrate() sound = Sound()
def povorot(rotation, m1, m2): gyr = GyroSensor() k = gyr.angle + rotation print("povorot") print(gyr.angle, file=sys.stderr) print(k, file=sys.stderr) while abs(gyr.angle-k) > 10: print(gyr.angle, file = sys.stderr) m1.on_for_seconds(35,0.2, block=False) m2.on_for_seconds(-35,0.2, block=False) time.sleep(0.2)
def __init__(self): self.gyro = GyroSensor() self.move = movement.Movement(self.gyro) self.LEFT = self.move.left_turn self.RIGHT = self.move.right_turn self.sensor = ColorSensor() while True: self.follow_line(ColorSensor.COLOR_WHITE, line_color2=ColorSensor.COLOR_YELLOW)
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 test_gyro(): gs = GyroSensor(INPUT_2) # for mode in gs.modes: # gs.mode = mode # print('The current gyro mode is: {}'.format(gs.mode)) # print('The angle is at {} degrees'.format(gs.angle)) # print('The rate of rotation is {} degrees/second'.format(gs.rate)) # # print('Here\'s both as a tuple: {}'.format(gs.angle_and_rate)) # # print('Tilt angle: {} degrees?'.format(gs.tilt_angle)) # # print('Tilt rate: {} degrees/second?'.format(gs.tilt_rate)) # # if gs.mode in (GyroSensor.MODE_GYRO_ANG, GyroSensor.MODE_GYRO_G_A, GyroSensor.MODE_TILT_ANG): # # print('Waiting for angle to change by 90 degrees clockwise: {}'.format(gs.wait_until_angle_changed_by(90, True))) # # print('Waiting for angle to change by 90 degrees any direction: {}'.format( # # gs.wait_until_angle_changed_by(90, False)) # # ) gs.mode = GyroSensor.MODE_GYRO_G_A while not button.any(): print('Angle: {}'.format(gs.angle_and_rate)) sleep(0.3)
def GyroTurn(steering, angle, gyro = GyroSensor(INPUT_2), steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B)): """Function to do precise turns using gyro sensor Input steering and angle to turn. Angle must be a +ve value gyro: gyro sensor if different than default steer_pair: MoveSteering if different than default """ if True == Constants.STOP: return gyro.mode='GYRO-ANG' steer_pair.on(steering = steering, speed = 15) gyro.wait_until_angle_changed_by(abs(angle)) steer_pair.off()
def set_up_sensors_motors(self): """ Creates all sensors and motors or returns None or the error """ try: self.leds = Leds() self.left_color_sensor = ColorSensor(LEFT_COLOR_SENSOR_INPUT) self.cs = self.left_color_sensor self.left_gyro_sensor = GyroSensor(LEFT_GYRO_SENSOR_INPUT) self.right_gyro_sensor = GyroSensor(RIGHT_GYRO_SENSOR_INPUT) self.gyro = self.right_gyro_sensor self.right_color_sensor = ColorSensor(RIGHT_COLOR_SENSOR_INPUT) self.left_medium_motor = MediumMotor(LEFT_MEDIUM_MOTOR_PORT) self.right_medium_motor = MediumMotor(RIGHT_MEDIUM_MOTOR_PORT) self.left_large_motor = LargeMotor(LEFT_LARGE_MOTOR_PORT) self.right_large_motor = LargeMotor(RIGHT_LARGE_MOTOR_PORT) except Exception as e: self.write_to_console(str(e), column=1, row=3, reset_console=True, inverse=True, alignment='L', font_size='M') return e else: return None
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()
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()
def __init__(self, motorL=None, motorR=None, motorM=None, gyroPort=None, colorL=None, colorR=None, colorM=None): if motorL: self.motorL = LargeMotor(motorL) if motorR: self.motorR = LargeMotor(motorR) if motorM: self.motorM = Motor(motorM) if gyroPort: self.gyro = GyroSensor(gyroPort) self.gyro.mode = 'GYRO-CAL' time.sleep(0.2) self.gyro.mode = 'GYRO-ANG' if colorL: self.colorL = ColorSensor(colorL) if colorR: self.colorR = ColorSensor(colorR) if colorM: self.colorM = ColorSensor(colorM) if motorL and motorR: self.drive = MoveSteering(motorL, motorR) self.move = MoveTank(motorL, motorR) print("Successful initialization!")
def GyroDrift(gyro=GyroSensor(INPUT_2)): ''' Checking if the Gyro sensor's output is drifing and moving when the actual sensor is not. we need to do this becuase if the sensor is drifting then our GyroTurn's will not work correctly ''' sound = Sound() #Creating sound Shortcut gyro.mode = 'GYRO-RATE' #setting gyro mode while math.fabs( gyro.rate ) > 0: #while gyro is drifting loop, if it is not drifting then the loop will not take place show_text( "Gyro drift rate: " + str(gyro.rate)) #Showing the rate of how much gyro is drifting sound.beep() # Beep to indicate that it is displaying current value sleep(0.5) #waiting for value to change gyro.mode = 'GYRO-ANG' #reseting gyro mode for our use in other functions
def GyroTurn(steering, angle, gyro=GyroSensor(INPUT_2), steer_pair=MoveSteering(OUTPUT_A, OUTPUT_B)): """Function to do precise turns using gyro sensor Input steering and angle to turn. Angle must be a +ve value gyro: gyro sensor if different than default steer_pair: MoveSteering if different than default """ if True == Constants.STOP: return # gyro.mode = 'GYRO-ANG' #setting gyro value mode steer_pair.on( steering=steering, speed=15 ) #starting to turn using MoveSteering. If steering equals 50 then it will do a pivot turn, if it is 100 then it will do a spin turn gyro.wait_until_angle_changed_by( abs(angle)) #keeps turning until the correct angle is reached steer_pair.off() #stopping the turning after target angle is reached