# need to wait until angle set to zero, then recalibrated
# have not tested this...
while abs(gyro.angle()) != 0
gyro.reset_angle(0)
ev3.speaker.beep()
ev3.speaker.beep()
#################################################################################
ev3.screen.draw_text(50, 60, "Pigeons!")
recalibrateGyro()
while robot.distance() < 500:
robot.drive(1000, 0)
robot.reset()
gyro.reset_angle(0)
angle = gyro.angle()
print ( "gyro1 " + str(angle) )
while abs(angle) < 160:
print ( "gyro2 " + str(angle) )
print ( "drive " + str(robot.angle() ) )
robot.drive(10, -180)
angle = gyro.angle()
robot.reset()
while robot.distance() < 500:
robot.drive(1000, 0)
class twoWheelsController:
def __init__(self,
left_motor_output,
right_motor_output,
min_steering,
max_steering,
min_speed,
max_speed,
wheelDiameter,
axeDiameter,
steering_treshold=12,
prev_steering_size=10):
"""
Initializes a controller with two wheels.
:param left_motor_output: the output pin for the left motor
:param right_motor_output: the output pin for the right motor
:param min_steering: the minimum steering in [-100; 100]
:param max_steering: the maximum steering in [-100; 100]
:param min_speed: the minimum speed in [-100; 100]
:param max_speed: the maximum speed in [-100; 100]
"""
self._max_steering = max_steering
self._min_steering = min_steering
self._min_speed = min_speed
self._max_speed = max_speed
self._motor_left = Motor(left_motor_output)
self._motor_right = Motor(right_motor_output)
self._drive_base = DriveBase(self._motor_left, self._motor_right,
wheelDiameter, axeDiameter)
#internal variable to determine wich segment the robot is on currently
#self.prev_steering = numpy.zeros(shape = (20))
self.prev_steering = [0] * prev_steering_size
self.prev_angle = [0] * 10
self.action = 0
self.steering_treshold = steering_treshold
self.current_action = [
'going_straight', 'turning_right', 'turning_left'
]
self.current_angle = 0
def drive(self, steering, speed):
"""
Drives using the given steering and speed limited to defined bounds.
:param steering: the steering to apply in [-100; 100]
:param speed: the speed to apply in [-100; 100]
"""
apply_steering = min(self._max_steering,
max(self._min_steering, steering))
apply_speed = min(self._max_speed, max(self._min_speed, speed))
#self.prev_steering = np.append([apply_steering], self.prev_steering[0:-1])
self.prev_steering.pop(0)
self.prev_steering.append(apply_steering)
#self.update_action()
self.prev_angle.pop(0)
self.prev_angle.append(self.getAngle())
#print(self.prev_angle)
self.update_angle()
self.update_action()
self._drive_base.drive(apply_speed, apply_steering)
def stop(self):
""" Stops moving. """
self._drive_base.drive(0, 0)
def update_action(self):
#steering_mean = numpy.mean(self.prev_steering)
steering_mean = sum(self.prev_steering) / len(self.prev_steering)
if steering_mean > self.steering_treshold:
if self.action != 1:
self.action = 1
#print("controller",self.current_action[self.action])
elif steering_mean < -self.steering_treshold:
if self.action != 2:
self.action = 2
#print("controller",self.current_action[self.action])
else:
if self.action != 0:
self.action = 0
#print("controller",self.current_action[self.action])
def update_angle(self):
angle_mean = int(sum(self.prev_angle) / len(self.prev_angle))
self.current_angle = angle_mean
#print(self.current_angle)
def getAngle(self):
return self._drive_base.angle()
# Initialize the motors.
left_motor = Motor(Port.B)
right_motor = Motor(Port.C)
left_motor.reset_angle(0)
right_motor.reset_angle(0)
# Initialize the drive base.
robot = DriveBase(left_motor,
right_motor,
wheel_diameter=54.6,
axle_track=104.1)
#################################################################################
ev3.screen.draw_text(50, 60, "Pigeons!")
x = 0
while x < 8:
### straight (if run motor faster, must adjust distance)
robot.reset()
while robot.distance() < 250:
robot.drive(200, 0)
### turn (if run motor faster, must adjust angle)
robot.reset()
while abs(robot.angle()) < 90:
print("angle: " + str(abs(robot.angle())))
robot.drive(200, 150)
x = x + 1
### straight (if run motor faster, must adjust distance)
### turn (if run motor faster, must adjust angle)
ev3.screen.draw_text(50, 60, "Pigeons!")
## 1.
### straight
robot.reset()
while robot.distance() < 250:
robot.drive(200, 0)
print("gyro")
### turn
robot.reset()
while abs(robot.angle()) < 90:
robot.drive(200, 150)
## 2.
### straight
robot.reset()
while robot.distance() < 250:
robot.drive(200, 0)
print("gyro")
### turn
robot.reset()
while abs(robot.angle()) < 90:
robot.drive(200, 150)
