watch = StopWatch() amplitude = 90 # ================================= mB = Motor(Port.B) # initialize two large motors mC = Motor(Port.C) robot = DriveBase(mB, mC, 94, 115) # wheel OD=94mm, wheelbase=115mm mB.set_run_settings(200, 250) # max speed, max accel mC.set_run_settings(200, 250) # max speed, max accel gy = GyroSensor(Port.S3) gy2 = GyroSensor(Port.S2) gy.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle) gy2.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle) sleep(0.5) gy.mode='GYRO-ANG' # changing modes causes recalibration gy2.mode='GYRO-ANG' # changing modes causes recalibration sleep(3) gy.reset_angle(0) gy2.reset_angle(0) mB.reset_angle(0) mC.reset_angle(0) angle = 0 # global var holding accumulated turn angle runB = False # whether Motor B should be running right now runC = False
from pybricks import ev3brick as brick from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor) from pybricks.parameters import (Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align) from pybricks.tools import print, wait, StopWatch from pybricks.robotics import DriveBase import math # Write your program here brick.sound.beep() gyro = GyroSensor(Port.S2) motor1 = Motor(Port.B) motor2 = Motor(Port.C) robot = DriveBase(motor1, motor2, 56, 50) gyro.mode = "GYRO-CAL" gyro.reset_angle(0) #Must start robot in upright balanced position while True: angle = gyro.speed() print(angle) #This part actually implements if angle > 3: robot.drive_time(5*angle+100,0, 200) elif angle < -3: robot.drive_time(5*angle-100,0,200) else: robot.stop(Stop.HOLD)