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)
# Initialize the EV3 Brick ev3 = EV3Brick() # Set volume to 100% and make a beep to signify program has started ev3.speaker.set_volume(100) ev3.speaker.beep() # Initialize EV3 touch sensor and motors motor = Motor(Port.A) gyro = GyroSensor(Port.S1) # Hold the gyro sensor still. THe program will display the current gyro # speed. If the speed is not close to zero unplug the gyro sensor from the # EV3 brick, hold it still and plug is back in. print("Speed: ", gyro.speed()) # Create a loop to react to buttons while True: # Check for center button events if Button.CENTER in ev3.buttons.pressed(): motor.stop() break # Rotate the gyro sensor using the red dot as the center. The angle will # range from -90 to 90. This is close to perfect as the motor dc method # requires a value from -100 to 100. motor.dc(gyro.angle()) wait(10)
class Robot: def __init__(self, left_motor_port, right_motor_port, med_motor_port, gyro_port, wheel_diameter, wheel_base): self.left_motor = Motor(left_motor_port) self.right_motor = Motor(right_motor_port) # self.med_motor = Motor(med_motor_port) self.robot = DriveBase(self.left_motor, self.right_motor, wheel_diameter, wheel_base) self.gyro = GyroSensor(gyro_port) # Function definitions # Gyro sensor reset, waits until the gyro has come to rest and then resets the value to zero # use at beginning of mission programs def resetGyro(self): brick.light(Color.RED) speed = self.gyro.speed() angle = self.gyro.angle() while speed != 0: wait(100) speed = self.gyro.speed() angle = self.gyro.angle() self.gyro.reset_angle(0) brick.light(Color.GREEN) # Drive the robot straight using the GyroSensor # def driveStraight(self, rotations, speed): distance = rotations * 360 # convert wheel rotations to degrees self.gyro.reset_angle(0) self.left_motor.reset_angle(0) self.right_motor.reset_angle(0) # set our amount to correct back towards straight correction = -2 if speed < 0: correction = 2 # start the robot driving self.robot.drive(speed, 0) # loop until the robot has travelled the distance we want # updating the steering angle of the drive based on the gyro measured drift and correction while abs(self.left_motor.angle()) <= distance and abs( self.right_motor.angle()) <= distance: drift = self.gyro.angle() print("Drift: " + str(drift)) steering = drift * correction #print("Steering: " + str(steering)) self.robot.drive(speed, steering) self.robot.stop(Stop.BRAKE) # Turn the robot an exact amount using the GryoSensor def turnDegrees(self, degrees): self.gyro.reset_angle(0) initial_power = 300 end_power = 50 left_motor_power = initial_power right_motor_power = initial_power * -1 if degrees < 0: left_motor_power = initial_power * -1 right_motor_power = initial_power initial_turn = abs(degrees * .75) self.left_motor.run(left_motor_power) self.right_motor.run(right_motor_power) angle = self.gyro.angle() print("Angle: " + str(angle)) while abs(angle) < initial_turn: wait(10) angle = self.gyro.angle() print("Angle: " + str(angle)) left_motor_power = end_power right_motor_power = end_power * -1 if degrees < 0: left_motor_power = end_power * -1 right_motor_power = end_power self.left_motor.run(left_motor_power) self.right_motor.run(right_motor_power) end_degrees = (abs(degrees) - 1) angle = self.gyro.angle() print("Angle: " + str(angle)) while abs(angle) < end_degrees: wait(10) angle = self.gyro.angle() print("Angle: " + str(angle)) self.left_motor.stop(Stop.BRAKE) self.right_motor.stop(Stop.BRAKE) print("Final Angle: " + str(self.gyro.angle())) def oldDrive(self, speed, turn): while True: self.robot.drive(speed, turn)
# plug in # S1 = touch sensor # S2 = light sensor # S3 = gyro sensor # S4 = ultrasonic sensor touch = TouchSensor(Port.S1) while not touch.pressed(): wait (10) light = ColorSensor(Port.S2) light.rgb() wait(100) light.color() # returns Color.BLACK, Color.BLUE, Color.GREEN, Color.YELLOW, Color.RED, Color.WHITE, Color.BROWN or None. wait(100) while not (light.color() == Color.YELLOW): wait(100) light.reflection() wait(100) light.ambient() wait(100) gyro = GyroSensor(Port.S3) gyro.reset_angle(0) # define your starting angle while gyro.angle() < 90: wait(100) gyro.speed()
class Robot(): def __init__(self): self.brick = EV3Brick() self.frontMotor = Motor(Port.D) self.rearMotor = Motor(Port.A) self.leftMotor = Motor(Port.C) self.rightMotor = Motor(Port.B) if path.exists('sensorpoints.py'): import sensorpoints self.leftSensor = LightSensor(Port.S3, sensorpoints.leftLow, sensorpoints.leftHigh) self.rightSensor = LightSensor(Port.S2, sensorpoints.rightLow, sensorpoints.rightHigh) else: self.leftSensor = LightSensor(Port.S3, 10, 105) self.rightSensor = LightSensor(Port.S2, 20, 160) self.gyroSensor = GyroSensor(Port.S1) wait(100) self.gyroSensor.speed() self.gyroSensor.angle() wait(500) self.gyroSensor.reset_angle(0.0) wait(200) def calibrate(self): rightHigh = 40 rightLow = 70 leftHigh = 40 leftLow = 70 timer = StopWatch() self.moveSteering(0, 125) while timer.time() < 5000: if self.rightSensor.light() > rightHigh: rightHigh = self.rightSensor.light() if self.rightSensor.light() < rightLow: rightLow = self.rightSensor.light() if self.leftSensor.light() > leftHigh: leftHigh = self.leftSensor.light() if self.leftSensor.light() < leftLow: leftLow = self.leftSensor.light() self.stop() # write results to file with open('sensorpoints.py', 'w') as myFile: myFile.write('leftLow = ') myFile.write(str(leftLow)) myFile.write('\nrightLow = ') myFile.write(str(rightLow)) myFile.write('\nleftHigh = ') myFile.write(str(leftHigh)) myFile.write('\nrightHigh = ') myFile.write(str(rightHigh)) def moveSteering(self, steering, speed): leftMotorSpeed = speed * min(1, 0.02 * steering + 1) rightMotorSpeed = speed * min(1, -0.02 * steering + 1) self.leftMotor.run(leftMotorSpeed) self.rightMotor.run(rightMotorSpeed) def drive(self, distance, speed, time=8, kSteering=1): # Startup for gyro #kSteering=5 # Steering coefficient startDegrees = self.gyroSensor.angle() # Startup for ramp speed if distance < 0: distance = abs(distance) speed = -speed rotation = distance * 51.9 self.rightMotor.reset_angle(0) # Loop while abs(self.rightMotor.angle()) < abs(rotation): # Do the gyro steering stuff currentDegrees = self.gyroSensor.angle() errorGyro = currentDegrees - startDegrees # Do the ramp speed stuff rampSpeed = min( sin(abs(self.rightMotor.angle()) / rotation * 3.14), abs(speed) - 100) self.moveSteering(errorGyro * kSteering * copysign(1, speed), rampSpeed * speed + copysign(100, speed)) # Exit self.stop() def turn(self, angle, speed, time=5): # Startup steering = 100 kTurn = 0.01 offset = 20 timer = StopWatch() # Loop while (abs(self.gyroSensor.angle() - angle) > 0) & (timer.time() < time * 1000): error = self.gyroSensor.angle() - angle #if error > 0 : # steering = 100 #else: # steering = -100 self.moveSteering(steering, speed * error * kTurn + copysign(offset, error)) # Exit self.stop(Stop.HOLD) print("turning to: ", angle, " gyro: ", self.gyroSensor.angle()) def lineFollow2Line(self, speed, rightSide=True, rightFollow=True): # Startup if rightFollow: followSensor = self.rightSensor stopSensor = self.leftSensor else: followSensor = self.leftSensor stopSensor = self.rightSensor if rightSide: kSide = 1 else: kSide = -1 lastError = 0 # Loop while not stopSensor.isWhite(): error = followSensor.line - followSensor.light() pCorrection = error * 0.25 dError = lastError - error dCorrection = dError * 1.25 self.moveSteering((pCorrection - dCorrection) * kSide, speed) lastError = error self.stop() def lineFollow4Time(self, speed, time, rightSide=True, rightFollow=True): # Startup if rightFollow: followSensor = self.rightSensor else: followSensor = self.leftSensor if rightSide: kSide = 1 else: kSide = -1 timer = StopWatch() lastError = 0 # Loop while timer.time() < time * 1000: # Experimental settings: kp = 0.2, kd = 0.4 error = followSensor.line - followSensor.light() pCorrection = error * 0.25 # Used to be 0.25 dError = lastError - error dCorrection = dError * 1.2 # Used to be 1.25 self.moveSteering((pCorrection - dCorrection) * kSide, speed) lastError = error #wait(10) self.stop() def turn2Line(self, speed, rightStop=True, time=5): if rightStop: stopSensor = self.rightSensor else: stopSensor = self.leftSensor self.moveSteering(100, speed) stopSensor.waitForLine() self.stop() def drive2Line(self, speed, distanceBefore, distanceAfter, rightStop=True): # Startup if rightStop: stopSensor = self.rightSensor else: stopSensor = self.leftSensor self.drive(distanceBefore, speed) # Loop #while self.rightSensor.rgb() < 90: # self.moveSteering(0, 70) # Start Driving # *** OLD ONE *** self.moveSteering(0, 250) self.moveSteering(0, 0.5 * speed) # execute function wait until we detect a line stopSensor.waitForLine() self.stop(Stop.HOLD) # Exit self.drive(distanceAfter, speed) def stop(self, brakeType=Stop.HOLD): # 3 options: Stop.BRAKE, Stop.COAST, Stop.HOLD self.rightMotor.stop(brakeType) self.leftMotor.stop(brakeType) def wait4Button(self): self.brick.speaker.beep() while not any(self.brick.buttons.pressed()): pass def gyroSet(self, newAngle=0): #while not self.gyroCheck(): # pass startAngle = self.gyroSensor.angle() wait(100) #self.gyroSensor.speed() #self.gyroSensor.angle() wait(500) self.gyroSensor.reset_angle(newAngle) wait(500) print("Gyro Start: ", startAngle, "Gyro Reset. Goal: ", newAngle, " Actual: ", self.gyroSensor.angle()) def gyroCheck(self): angle1 = self.gyroSensor.angle() wait(1000) if self.gyroSensor.angle() != angle1: print("drift detected") self.brick.speaker.say("grace forgot her necklace") return False else: print("absence of drift") self.brick.speaker.say("grace remebered her necklace") return True
# Initialize the EV3 brick. ev3 = EV3Brick() # Initialize the gyro sensor. It is used to provide feedback for balancing the # robot. gyro_sensor = GyroSensor(Port.S2) # The following (UPPERCASE names) are constants that control how the program # behaves. GYRO_CALIBRATION_LOOP_COUNT = 200 GYRO_OFFSET_FACTOR = 0.0005 # Calibrate the gyro offset. This makes sure that the robot is perfectly # still by making sure that the measured rate does not fluctuate more than # 2 deg/s. Gyro drift can cause the rate to be non-zero even when the robot # is not moving, so we save that value for use later. while True: gyro_minimum_rate, gyro_maximum_rate = 440, -440 gyro_sum = 0 for _ in range(GYRO_CALIBRATION_LOOP_COUNT): gyro_sensor_value = gyro_sensor.speed() gyro_sum += gyro_sensor_value if gyro_sensor_value > gyro_maximum_rate: gyro_maximum_rate = gyro_sensor_value if gyro_sensor_value < gyro_minimum_rate: gyro_minimum_rate = gyro_sensor_value wait(5) if gyro_maximum_rate - gyro_minimum_rate < 2: break gyro_offset = gyro_sum / GYRO_CALIBRATION_LOOP_COUNT