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)
