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 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 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 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)
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.sensor.lego import ColorSensor, UltrasonicSensor, GyroSensor from ev3dev2.led import Leds from ev3dev2.sound import Sound from time import sleep from threading import Thread import socket print("Fertig") sound = Sound() driver = MoveTank(OUTPUT_A, OUTPUT_B) us = UltrasonicSensor(INPUT_4) gs = GyroSensor(INPUT_2) gs.mode = "GYRO-ANG" us.mode = "US-DIST-CM" def server_program(): host = socket.gethostname() port = 5000 # initiate port no above 1024 server_socket = socket.socket() server_socket.bind((host, port)) server_socket.listen(2) conn, address = server_socket.accept() print("Connection from: " + str(address)) while True: data = conn.recv(1024).decode()
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)
import math 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) def avanzar(): motor_pair.on_for_seconds(steering=0, speed=50, seconds=1) angle = gy.value() print(str(angle))
RMC.on(-10) LMC.on(10) while(abs(start - gy.value()) <= 90): sleep(0.001) RMC.off() LMC.off() # Initialization of Sensors & Sensor Vars RMC.off() LMC.off() leds.all_off() cl.mode='COL-COLOR' gy.mode='GYRO-ANG' while cl.value() != HOME: nagHumans() sleep(0.25) # **************************************** MAIN ****************************** foundGoal = False right = 1 start = int(round(time.time() * 1000)) while not foundGoal: RMC.on(right) LMC.on(100) if int(round(time.time() * 1000)) - start >= 250: right = right + 1
#!/usr/bin/env python3 import os import sys import time # TODO: implement fast read/write from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_D from ev3dev2.sensor.lego import GyroSensor, TouchSensor touch = TouchSensor() gyro = GyroSensor() gyro.mode = gyro.MODE_GYRO_G_A offset = 0 left = LargeMotor(OUTPUT_D) left.duty_cycle_sp = 0 left.command = left.COMMAND_RUN_DIRECT right = LargeMotor(OUTPUT_A) right.duty_cycle_sp = 0 right.command = right.COMMAND_RUN_DIRECT alpha = 0.8 dc_gain = 1.0 p_gain = 1 i_gain = 0.029 d_gain = 0.049 #dt = 30 / 1000 dt = 250 / 1000
#!/usr/bin/python3 from multiprocessing import Process, Value from time import sleep from ev3dev2.motor import OUTPUT_A, OUTPUT_C, SpeedPercent, MoveDifferential from ev3dev2.sensor import INPUT_4 from ev3dev2.sensor.lego import GyroSensor from ev3dev2.wheel import EV3EducationSetTire from ev3_toys.logger import get_logger # done = False gs = GyroSensor(INPUT_4) # reset the angle and rate values for the Gyro by changing the mode gs.mode = GyroSensor.MODE_GYRO_CAL sleep(1) gs.mode = GyroSensor.MODE_GYRO_RATE gs.mode = GyroSensor.MODE_GYRO_ANG gs.mode = GyroSensor.MODE_GYRO_G_A sleep(1) motors = MoveDifferential(OUTPUT_C, OUTPUT_A, EV3EducationSetTire, 103) def check_gyro(val: Value, logger, gyro: GyroSensor): # write to a file for testing while val.value == 0: logger.info('(Angle, Rate) = {}\n'.format(gyro.angle_and_rate)) sleep(0.5)
#!/usr/bin/env python3 from ev3dev2.motor import (OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, MediumMotor, MoveSteering, SpeedPercent) from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.sensor.lego import ColorSensor, GyroSensor, UltrasonicSensor medMotor = MediumMotor(OUTPUT_C) ultrasonic_sensor_front = UltrasonicSensor(INPUT_4) ultrasonic_sensor_side = UltrasonicSensor(INPUT_2) ultrasonic_sensor_front.mode = 'US-DIST-CM' ultrasonic_sensor_side.mode = 'US-DIST-CM' gyro = GyroSensor(INPUT_1) gyro.mode = GyroSensor.MODE_GYRO_ANG drivetrain = MoveSteering(OUTPUT_B, OUTPUT_D) front_dist = ultrasonic_sensor_front.distance_centimeters side_dist = ultrasonic_sensor_side.distance_centimeters while True: if ultrasonic_sensor_front.distance_centimeters <= 10: if ultrasonic_sensor_side.distance_centimeters <= 15: drivetrain.on(steering=100, speed=20) gyro.wait_until_angle_changed_by(90) drivetrain.on(steering=0, speed=0) else: drivetrain.on(steering=-100, speed=20) gyro.wait_until_angle_changed_by(90) drivetrain.on(steering=0, speed=0)
from ev3dev2.sensor.lego import GyroSensor gy = GyroSensor() gy.mode = GyroSensor.MODE_TILT_ANG gy.reset() def is_in_angle(): gy.mode = GyroSensor.MODE_TILT_ANG return abs(gy.tilt_angle) > 2
os.system('setfont Lat15-TerminusBold14') # Large font # os.system('setfont Lat15-TerminusBold32x16') # Very large font # ======================================================================= # Hardware Definitions # ======================================================================= # Access the two motors leftMotor = LargeMotor(OUTPUT_B) rightMotor = LargeMotor(OUTPUT_D) print('Motors: Ready') # The gyro is calibrated as soon as it has power # So if the robot is not perfectly still there will be drift # Calibrate the gyro and set it up for measuring angles gyro = GyroSensor(INPUT_1) gyro.mode = 'GYRO-CAL' # Calibrate - Robot must be perfectly still gyro.mode = 'GYRO-ANG' # Angle mode print('Gyro Sensor: Calibrated') # ======================================================================= # Main Procedure # ======================================================================= # Initialise the target motion # Move forward while turning 20deg clockwise, then straight forwardSpeed = 30 targetAngle = gyro.angle + 20 # Main program loop # Pressing the Back button will halt the program halt = False while not halt:
import sys from time import sleep import ev3dev.ev3 as ev3 from ev3dev2.sensor.lego import GyroSensor import config from util import util FWDLEFT = ev3.LargeMotor("outA") # outA means the motor connect to EV3 port A. FWDRIGHT = ev3.LargeMotor("outD") BWDLEFT = ev3.LargeMotor("outB") BWDRIGHT = ev3.LargeMotor("outC") GYRO = GyroSensor() GYRO.mode = "GYRO-ANG" def stop_when_rotation_complete(angle): c_angle = GYRO.angle iteration = 0 last_angle = 0 angle = int(angle) while abs(GYRO.angle - c_angle) < angle: if last_angle == GYRO.angle: iteration += 1 else: iteration = 0 last_angle = GYRO.angle
MOVE_TANK = MoveTank(OUT_LEFT, OUT_RIGHT) MOVE_STEERING = MoveSteering(OUT_LEFT, OUT_RIGHT) DEGREE_SUCCESS_RATE = 0.92 DEGREE_SUCCESS_RATE_NEG = 0.88 METER_IN_MS = 8200 BRAKE_TYPES_LIST = [ ("coast", "lets the motor continue to turn until stopped by friction"), ("hold", "an active, forceful effort to stop the motor turning"), ("brake", "passively and less forcefully tries to stop the motor turning"), ] BRAKE_TYPE = "brake" GYRO = GyroSensor(INPUT_2) GYRO.mode = 'GYRO-ANG' SPEED = 25 # in percent CUR_DEG = 0 # Call the function to turn the robot with a angle that is pre-defined def fix_rotation(): turn(-CUR_DEG) # Call the function to move forward def forward(t): print("start") wait_time = t / 1000 # in seconds
# Connect sensor to sensor port 3 gyro = GyroSensor(INPUT_2) # allow for some time to load the new drivers time.sleep(0.5) print( "#######################################################################") print( "############# starting in gyro angle mode #########################") print( "#######################################################################") #gyro.mode = 'GYRO-ANG' gyro.mode = 'GYRO-FAS' gyroVal = 0 prev_gyroVal = -1 time.sleep(2) # give some time to stabilize gyro before reading values startTime = time.time() badValCount = 0 while (1): gyroVal = gyro.angle # work-around for bogus intermittent readings of '0' (possibly a f/w bug in BrickPi3) if gyroVal == 0 and abs(gyroVal - prev_gyroVal) > 1: badValCount += 1
#!/usr/bin/env python3 from ev3dev.ev3 import * from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, MediumMotor, MoveSteering, SpeedPercent from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.sensor.lego import ColorSensor, GyroSensor, UltrasonicSensor medMotor = MediumMotor(OUTPUT_C) ultrasonic_sensor_front = UltrasonicSensor(INPUT_4) ultrasonic_sensor_side = UltrasonicSensor(INPUT_2) ultrasonic_sensor_front.mode = 'US-DIST-CM' ultrasonic_sensor_side.mode = 'US-DIST-CM' gyro = GyroSensor(INPUT_1) gyro.mode = 'MODE_GYRO_ANG' drivetrain = MoveSteering(OUTPUT_B, OUTPUT_D) drivetrain.on(steering=0, speed=20) if ultrasonic_sensor_front.distance_centimeters <= 2.0: drivetrain.on(steering=0, speed=0) #stop robot if something is at front. if ultrasonic_sensor_side.distance_centimeters <= 5.0: drivetrain.on(steering=-100, speed=20) gyro.wait_until_angle_changed_by(90) drivetrain.on(steering=0, speed=0) elif ultrasonic_sensor_side.distance_centimeters > 5.0: drivetrain.on(steering=100, speed=10) gyro.wait_until_angle_changed_by(90)
format='%(asctime)s %(levelname)5s: %(message)s') log = logging.getLogger(__name__) log.info("Starting Gyro Boy") # Initialize running = True state = "reset" timer = StopWatch() timer.start() # Brick variables sound = Sound() leds = Leds() console = Console() gyro_sensor = GyroSensor(INPUT_2) gyro_sensor.mode = GyroSensor.MODE_GYRO_RATE arms_motor = MediumMotor(OUTPUT_C) left_motor = LargeMotor(OUTPUT_D) right_motor = LargeMotor(OUTPUT_A) color_sensor = ColorSensor(INPUT_1) ultra_sonic_sensor = UltrasonicSensor(INPUT_4) class SensorThread(Thread): """Sensor thread, based on Color and Ultrasonic sensors.""" def run(self): global state global drive_speed global steering log.debug("Sensor Thread running!")
import bluetooth import threading from time import sleep from ev3dev2.sound import Sound from ev3dev2.sensor.lego import GyroSensor, TouchSensor, UltrasonicSensor from ev3dev2._platform.ev3 import INPUT_1, INPUT_2, INPUT_3, INPUT_4 # SERVER_MAC = 'CC:78:AB:50:B2:46' SERVER_MAC = '00:17:E9:B2:1E:41' mission_ongoing = True s = Sound() gs = GyroSensor(INPUT_3) gs.mode = 'GYRO-ANG' ts_left = TouchSensor(INPUT_1) ts_right = TouchSensor(INPUT_4) us_front = UltrasonicSensor(INPUT_2) us_front.mode = 'US-DIST-CM' def connect(): port = 3 sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM) print('Connecting...') sock.connect((SERVER_MAC, port)) print('Connected to ', SERVER_MAC) return sock, sock.makefile('r'), sock.makefile('w')
lml.reset() lmr.reset() sleep(0.1) mt.on(8.1, 8) while True: if lml.position > 299 or lmr.position > 299: mt.off() break sleep(0.1) mt.on(0, trs) gs.mode = 'GYRO-RATE' gs.mode = 'GYRO-ANG' #紅綠燈前左彎 while True: lml.off() if gs.angle < -40 : mt.off() break sleep(0.1) lmr.reset() lml.reset() mt.on(10.6, 10.5)
definir a melhor velocidade para a garra abrir e fechar definir quantas rotações (ou segundos) são necessarias para andar 1 casa definir os angulos de rotação definir multiprocessing para andar e fazer a leitura ao mesmo tempo ''' ########################################################################################## #motores steer_pair = MoveSteering(OUTPUT_A, OUTPUT_D) motor_garra = MediumMotor(OUTPUT_B) #sensores gyro = GyroSensor() us = UltrasonicSensor() # Put the gyro sensor into ANGLE mode. gyro.mode = 'GYRO-ANG' us.mode = 'US-DIST-CM' #constantes ROTACOES_CASA = 2.2 #cada casa é +/- 2.2 rotações DISTANCIA_PROCURA = 40 #distância maxima a que o objeto tem de estar DISTANCIA_MIN = 10 #distância final do robot ao objeto VELOCIDADE_PADRAO = 30 VELOCIDADE_PROCURA = 20 #velocidade durante a procura, mais lenta para maior precisão VELOCIDADE_AJUSTE = 10 #velocidade para ajustar a posição do robot, super lento para não andar em excesso aos zig zags #variaveis globais #angulos por padrão ANGULO_AJUSTAMENTO = 2 #angulo para ajustar a direção do robot (andar o mais reto possivel) -- talvez mudar para 0 --