class Pid:
def __init__(self, kp, ki, kd):
"""Class that can be used to do PID calculations.
:param kp: Proportional multiplier for the error
:type kp: Number
:param ki: Multiplier for the intergral of the error
:type ki: Number
:param kd: Multiplier for the derivative of the error
:type kd: Number
"""
self.clock = StopWatch()
self.kp = kp
self.ki = ki
self.kd = kd
self.last_error = 0
self.total_error = 0
self.last_time = 0
def reset(self):
"""Reset parameters before start of PID loop
"""
self.clock.reset()
self.last_time = self.clock.time()
self.last_error = 0
self.total_error = 0
def compute_steering(self, error):
"""Computes and returns the corrections.
:param error: Difference between target and actual angles
:type error: Number
:return: Returns correction value
:rtype: Number
"""
current_time = self.clock.time()
elapsed_time = current_time - self.last_time
self.last_time = current_time
error_change = 0
if elapsed_time > 0:
error_change = (error - self.last_error) / elapsed_time
self.total_error = self.last_error * elapsed_time + self.total_error
self.last_error = error
steering = error * self.kp
steering += error_change * self.kd
steering += self.total_error * self.ki / 100
return steering
def follow_line():
# Return sensor values
black_area, white_area = calibration()
brick.display.text("Ready! Set! Go!")
wait(3000)
# Keeps track of time
timer = StopWatch()
# Loop to run for 20 seconds
while (timer.time() / 1000) < 20:
# The edge value is the mean of the black and white area readings
desired_value = (black_area + white_area) / 2
# Read sensor value
sensor_value = color_sensor.reflection()
# Magic constant
gain = 0.6
# Determine power for the motors
power = gain * (sensor_value - desired_value)
# One motor will speed up, the other will slow down
left_motor.dc(50 + power)
right_motor.dc(50 - power)
def walk_timed():
"""Makes the puppy walk a certain time in ms.
Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs.
Change walk_time to adjust the time the puppy walks in ms."""
#walk_time equals desired walk time in ms
walk_time = 6000
elapsed_time = StopWatch()
#puppy stands up
left_leg_motor.run_target(100, 25, wait=False)
right_leg_motor.run_target(100, 25)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
while not left_leg_motor.control.target_tolerances():
wait(100)
wait(500)
#puppy walks for specified time in ms
while elapsed_time.time() < walk_time:
left_leg_motor.run_target(-100, 25, wait=False)
right_leg_motor.run_target(-100, 25)
while not left_leg_motor.control.target_tolerances():
wait(300)
left_leg_motor.run_target(100, 65, wait=False)
right_leg_motor.run_target(100, 65)
while not left_leg_motor.control.target_tolerances():
wait(300)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
wait(100)
elapsed_time.reset()
def wall_follow():
# Keeps track of time
timer = StopWatch()
# Loop to run the program for 20 seconds
while (timer.time() / 1000) < 20:
sensor_value = ultra_sensor.distance() / 10
# Within reasonable distance from the wall
if sensor_value < 22 and sensor_value > 18:
robot.drive_time(80, 0, 500)
# Too close to wall
elif sensor_value < 19:
# centimeters
diff = 20 - sensor_value
robot.drive_time(0, -90, 1000)
robot.drive_time(10 * diff, 0, 1000)
robot.drive_time(0, 90, 1000)
# Too far to wall
else:
# centimeters
diff = sensor_value - 20
robot.drive_time(0, 90, 1000)
robot.drive_time(10 * diff, 0, 1000)
robot.drive_time(0, -90, 1000)
wait(200)
def straight_line(angle, speed, duration):
timer = StopWatch()
while timer.time() <= duration:
screen.clear()
fix_amount = p_controller(angle, gyro)
drive_base.drive(speed, -fix_amount)
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 run(self, angle: int):
"""
Start turing in place for a specified angle.
----------
angle : int – angle to turn (clockwise - positive).
"""
# Setup starting values
stop_watch = StopWatch()
self.pid.setpoint = angle
self.gyro_sensor.reset_angle(0)
last_angle = angle + 100
last_rotation = 0
while not abs(self.pid.setpoint -
last_angle) < TurnToAngleBehavior.ACCURACY_ANGLE:
angle = self.gyro_sensor.angle()
rotation = self.pid(angle)
if last_rotation != rotation:
self.bot.drive(0, rotation)
last_angle = angle
last_rotation = rotation
print(stop_watch.time(), ' -- angle:', angle, ', rotation: ',
rotation, ', error: ', (angle - self.pid.setpoint))
def drive_time(self, dist=100, time=1, ang=0):
stopwatch = StopWatch()
stopwatch.reset()
while True:
self.drive.drive(dist, ang)
if stopwatch.time() > time:
self.drive.drive.stop()
break
def follow_line_right(speed, duration):
timer = StopWatch()
while timer.time() <= duration:
deviation = right_color_sensor.reflection() - setpoint
turn_rate = K_P * deviation
screen.print(right_color_sensor.reflection())
drive_base.drive(speed, turn_rate)
wait(10)
def andarTempo(self, velocEsquerda, velocDireita, tempo):
cronometro = StopWatch() # Definimos um cronometro
cronometro.reset() # Resetamos o tempo marcado no cronometro
while cronometro.time() < tempo:
self.motorDireito.run(velocEsquerda)
self.motorEsquerdo.run(velocDireita)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def get_double_press(
self
): # used to indicate a user has finished their time with CHORE bot
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.touch_left.pressed() and self.touch_right.pressed():
return True
if stopwatch.time() > 3000:
self.ev3.light.off()
return False
def main():
coords = [0, 0]
left.reset_angle(0)
right.reset_angle(0)
Kp, Ki, Kd, i, last_error, target = 20, 0, 0, 0, 0, 5
conveyor_belt = Motor(Port.A)
directions_made = []
stopwatch = StopWatch()
stopwatch.resume()
driving_forward = False
while True:
if stopwatch.time() >= 100000: # OUR LAST HOPE FOR POINTS!!!!!
robot.stop()
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
break
if lightSensor.refelctivity() <= 500:
backward(1000)
if cSensor.color() == Color.BLUE:
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
conveyor_belt.run_time(-100, 1000, Stop.BRAKE, False)
foundvictim()
if LeftSensor.distance() >= 40:
wait(500)
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
leftturn()
directions_made.append("left turn")
robot.drive_time(-1000, 0, 1000)
directions_made.append(("forward", 1000))
elif ForwardSensor.distance() <= 300:
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
rightturn()
directions_made.append("right turn")
else:
robot.drive(-1000, 0)
driving_forward, start_time = True, stopwatch.time()
for direction in directions_made:
if direction == "left turn":
robot.stop()
rightturn()
elif direction == "right turn":
robot.stop()
leftturn()
else:
robot.drive_time(-1000, 0, direction[1])
def get_feedback(self):
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.touch_left.pressed():
self.ev3.light.off()
return -1
elif self.touch_right.pressed():
self.ev3.light.off()
return 1
if stopwatch.time() > 10000:
self.ev3.light.off()
return 0
def motoresDianteiros(motorBotton, motorTop):
ev3.speaker.say("Acionando motores")
cronometro = StopWatch()
cronometro.reset()
botoes = []
botoesAnt = botoes
while(True):
botoes = ev3.buttons.pressed()
if(len(botoes) == 0 or botoes != botoesAnt):
cronometro.reset()
motorBotton.brake()
motorTop.brake()
elif([Button.CENTER] == botoes and cronometro.time()>=TKEYPRESS):
break
elif([Button.UP] == botoes and cronometro.time()>=TKEYPRESS):
motorBotton.dc(100)
elif([Button.DOWN] == botoes and cronometro.time()>=TKEYPRESS):
motorBotton.dc(-100)
elif([Button.LEFT] == botoes and cronometro.time()>=TKEYPRESS):
motorTop.dc(-50)
elif([Button.RIGHT] == botoes and cronometro.time()>=TKEYPRESS):
motorTop.dc(50)
botoesAnt = botoes
def __init__(self, address):
try:
self.sock = get_bluetooth_rfcomm_socket(address, 1)
except OSError as e:
print("Turn on Bluetooth on the EV3 and on SPIKE.")
raise e
self._values = None
start_new_thread(self.reader, ())
watch = StopWatch()
while watch.time() < 2000:
if self.values() is not None:
return
wait(100)
raise IOError("No data received")
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 wait_until_bumped(self, wait_timer=500):
"""Wait until the TouchSensor is bumped
:param wait_timer: Time to wait (milliseconds) to consider a press and release a bump,
defaults to 500
:type wait_timer: int, float, optional
"""
if not isinstance(wait_timer, (int, float)):
return
my_watch = StopWatch()
while True:
self.wait_until_pressed()
my_watch.reset()
my_watch.resume()
self.wait_until_released()
my_watch.pause()
if my_watch.time() > wait_timer:
continue
return
def act_playful():
"""Makes the puppy act playful."""
playful_timer = StopWatch()
playful_bark_interval = None
ev3.screen.show_image(ImageFile.NEUTRAL)
#puppy stands up
left_leg_motor.run_target(100, 25, wait=False)
right_leg_motor.run_target(100, 25)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
while not left_leg_motor.control.target_tolerances():
wait(100)
playful_bark_interval = 0
if playful_timer.time() > playful_bark_interval:
ev3.speaker.play_file(SoundFile.DOG_BARK_2)
playful_timer.reset()
playful_bark_interval = randint(4, 8) * 1000
def proportional_derivative(calibrate_val):
# initialize local variables
watch = StopWatch()
prev_time = 0 # time when the previous value was taken
curr_time = 0 # time when the current value was taken
prev_left = 0 # previous left sensor value
prev_right = 0 # previous right sensor value
curr_left = 0 # current left sensor value
curr_right = 0 # current right sensor value
run = True
speed = 0 # values determined from testing to be the best four our robot
desired_r = 200 # desired right sensor value
desired_l = 200 # desired left sensor value
k_p = 2 # proportional constant
k_d = 2 # derivative constant
while run and (count < 1000):
curr_time = watch.time()
if curr_time == 0:
prev_time = curr_time
curr_left = left_sensor.read(
) + calibrate_val # left sensor was determined through testing to need a static offeset to match the right sensor's value
curr_right = right_sensor.read()
if prev_left == 0:
prev_left = curr_left
if prev_right == 0:
prev_right = curr_right
left_speed = prop_deriv_control(L0, L1, time0, time1, desired_l, k_p,
k_d, speed)
right_speed = prop_deriv_control(R0, R1, time0, time1, desired_r, k_p,
k_d, speed)
prev_time = curr_time
prev_left = curr_left
prev_right = curr_right
left_motor.run(left_speed)
right_motor.run(right_speed)
if Button.CENTER in brick.buttons():
# ends run loop
run = False
# ends main loop
return True
def get_color(self, time=1000):
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.color_sensor.color() == Color.BLUE:
self.ev3.light.off()
return Color.BLUE
if self.color_sensor.color() == Color.GREEN:
self.ev3.light.off()
return Color.GREEN
if self.color_sensor.color() == Color.YELLOW:
self.ev3.light.off()
return Color.YELLOW
if self.color_sensor.color() == Color.RED:
self.ev3.light.off()
return Color.RED
if stopwatch.time() > time:
self.ev3.light.off()
return None
def scan_rock(self, time):
# Stops the robot and moves the scan arm.
self.stop()
self.left_front_wheel.run(100)
# During the given duration, scan for rocks.
watch = StopWatch()
while watch.time() < time:
# If a rock color is detected, display it and make a sound.
if self.sherloc.color() in self.ROCK_COLORS:
self.hub.display.image(Icon.CIRCLE)
self.hub.speaker.beep()
else:
self.hub.display.off()
wait(10)
# Turn the arm motor and restore the heartbeat animation again.
self.hub.display.animate(self.HEART_BEAT, 30)
self.left_front_wheel.stop()
def tester():
powerL = 50
powerR = 48
# Time it takes for your robot to move forward in a straight line for
# approximately 15 cm when your left and right motors are set to
# powerL and powerR respectively.
time15cm = 0.887
# 15% faster
powerR15 = powerR * 1.15
# resets the rotation angle of both motors
left_motor.reset_angle(0)
right_motor.reset_angle(0)
timer = StopWatch()
# starts the left and right motors moving using the dc command at powers
# powerL and powerR15
while (timer.time() / 1000) < time15cm:
left_motor.dc(powerL)
right_motor.dc(powerR15)
# waits time15cm seconds
wait(time15cm)
# stops both motors moving
left_motor.dc(0)
right_motor.dc(0)
wait(1000)
# gets the rotation angle of both motors
left_rot_angle_after = left_motor.angle()
right_rot_angle_after = right_motor.angle()
computePoseChange(left_rot_angle_after, right_rot_angle_after)
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 wait_until_button_bumped(self, button, channel, wait_timer=500):
"""Waits until a specified button has been bumped
''NOTE: using a list, tuple or dict for buttons will make this function act the same as
wait_until_button_pressed if all of the buttons listed are pressed at once''
:param button: Button or Buttons to wait for
:type button: Button, list, tuple, dict
:param channel: Channel number of the remote
:type channel: int
"""
if not isinstance(wait_timer, (int, float)):
return
my_watch = StopWatch()
while True:
self.wait_until_button_pressed(button, channel)
my_watch.reset()
my_watch.resume()
self.wait_until_button_released(button, channel)
my_watch.pause()
if my_watch.time() > wait_timer:
continue
return
brick.display.image("Enemy.png", (200, 200), clear=False)
brick.display.image('SpaceWars.png')
brick.sound.file(SoundFile.T_REX_ROAR)
wait(2000)
loadImage()
while True:
brick.display.image("Player.png", (playerPositionX, playerPositionY),
clear=False)
brick.display.image("Enemy.png", (enemy1PositionX, enemy1PositionY),
clear=False)
brick.display.image("Enemy.png", (enemy2PositionX, enemy2PositionY),
clear=False)
brick.display.text(int(time1.time() / 1000), (150, 20))
if (playerPositionX + playerWidth) >= screenWidth:
playerPositionX = screenWidth - playerWidth
elif playerPositionX <= 0:
playerPositionX = 0
if enemy1PositionY > screenHeight:
enemy1PositionY = -enemyHeight
enemy1PositionX = randint(0, screenWidth - 60)
if enemy2PositionY > screenHeight:
enemy2PositionY = -enemyHeight
enemy2PositionX = randint(0, screenWidth - 90)
class BigRobot:
#---------------------- initialization-------------------------------------------------------
def __init__(self,dimensions,motors,GPS_number,Gyro,Ultra,Yanse,PID_parameters,time_step):
# define system
self.ev3 = ev3 = EV3Brick()
# define dimensions
self.a = dimensions['a']
self.b = dimensions['b']
self.r = dimensions['r']
# define motors
self.left_up_motor = motors['left_up_motor']
self.right_up_motor = motors['right_up_motor']
self.left_down_motor = motors['left_down_motor']
self.right_down_motor = motors['right_down_motor']
# define GPS
self.GPS_number = GPS_number
# define Sensors
self.Gyro = Gyro
self.Gyro.reset_angle(0)
self.Ultra = Ultra
self.Yanse = Yanse
# define PID parameters
self.Kp_x = PID_parameters['x']['Kp']
self.Ki_x = PID_parameters['x']['Ki']
self.Kd_x = PID_parameters['x']['Kd']
self.Kp_y = PID_parameters['y']['Kp']
self.Ki_y = PID_parameters['y']['Ki']
self.Kd_y = PID_parameters['y']['Kd']
self.Kp_theta = PID_parameters['theta']['Kp']
self.Ki_theta = PID_parameters['theta']['Ki']
self.Kd_theta = PID_parameters['theta']['Kd']
# define watch
self.watch = StopWatch()
# define time steo
self.time_step = time_step
# initialize position
self.position_x = 0
self.position_y = 0
#---------------------move in local reference -------------------------------------------------
def move(self,vx,vy,omega_deg):
# move in robot local reference
# vx, vy: speed, mm/s
# omega_deg: rotation speed, positive rotation:counterclockwise, deg/s
# calculate local angular speed
omega_rad = omega_deg/180*math.pi
angular_speed_left_up_motor = 1/r*(vx-vy-(self.a+self.b)*omega_rad)
angular_speed_right_up_motor = 1/r*(vx+vy+(self.a+self.b)*omega_rad)
angular_speed_left_down_motor = 1/r*(vx+vy-(self.a+self.b)*omega_rad)
angular_speed_right_down_motor = 1/r*(vx-vy+(self.a+self.b)*omega_rad)
# run motor
self.left_up_motor.run(angular_speed_left_up_motor*180/math.pi)
self.right_up_motor.run(angular_speed_right_up_motor*180/math.pi)
self.left_down_motor.run(angular_speed_left_down_motor*180/math.pi)
self.right_down_motor.run(angular_speed_right_down_motor*180/math.pi)
return self.watch.time()
#-------------------move in global reference---------------------------------------------------
def move_absolu(Vx,Vy,omega_deg,theta_deg):
# move in earth reference
# Vx, Vy: speed in earth reference
# omega_deg:deg/s
# reference transition
theta_rad = theta_deg/180*math.pi
vx = Vx*math.cos(theta_rad) + Vy*math.sin(theta_rad)
vy = -Vx*math.sin(theta_rad) + Vy*math.cos(theta_rad)
omega_rad = omega_deg/180*math.pi
angular_speed_left_up_motor = 1/r*(vx-vy-(self.a+self.b)*omega_rad)
angular_speed_right_up_motor = 1/r*(vx+vy+(self.a+self.b)*omega_rad)
angular_speed_left_down_motor = 1/r*(vx+vy-(self.a+self.b)*omega_rad)
angular_speed_right_down_motor = 1/r*(vx-vy+(self.a+self.b)*omega_rad)
self.left_up_motor.run(angular_speed_left_up_motor*180/math.pi)
self.right_up_motor.run(angular_speed_right_up_motor*180/math.pi)
self.left_down_motor.run(angular_speed_left_down_motor*180/math.pi)
self.right_down_motor.run(angular_speed_right_down_motor*180/math.pi)
return self.watch.time()
#-------------------------get ultrasonic distance----------------------------------------------
def get_obstacle_distance(self):
return self.Ultra.distance()
#-------------------------get gyro angle----------------------------------------------
def get_angle(self):
return self.Gyro.angle
#-------------------------get GPS position------------------------------------------------------
def get_GPS(self):
_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
_socket.setsockopt(socket.SOL_SOCKET,socket.SO_BROADCAST,1)
PORT = 1730
_socket.bind(('', PORT))
Position_XYZ,address = _socket.recvfrom(2048) #等待对方发送信息
Position_XYZ = Position_XYZ.decode('utf-8') #提取对方发送的信息
_socket.close()
#print(Position_XYZ_real)
a = 1
while a == 1:
Position_XYZ_real=Position_XYZ
if Position_XYZ_real[11] == str(self.GPS_number):
self_position = Position_XYZ_real
#print(self_position)
self.position_x = float(self_position[16:23])
for i in range(len(self_position)):
if self_position[i] == ',':
self.position_y = float(self_position[i+2:i+9])
break
a = 0
return self.position_x, self.position_y
# ------------------------get pid output values-------------------------------------------------------
def pid_output_x(self,error_present,error_last,error_integral):
proportion = self.Kp_x * error_present
integral = self.Ki_x * self.time_step*error_present + error_integral
derivative = self.Kd_x * (error_present-error_last)/self.time_step
output = proportion + integral + derivative
return output
def pid_output_y(self,error_present,error_last,error_integral):
proportion = self.Kp_y * error_present
integral = self.Ki_y * self.time_step*error_present + error_integral
derivative = self.Kd_y * (error_present-error_last)/self.time_step
output = proportion + integral + derivative
return output
def pid_output_theta(self,error_present,error_last,error_integral_last):
proportion = self.Kp_theta * error_present
integral = self.Ki_theta * self.time_step*error_present + error_integral
derivative = self.Kd_theta * (error_present-error_last)/self.time_step
output = proportion + integral + derivative
return output
#----------------------- define target distance ----------------------------------------------------------
def get_distance(self,target_x,target_y):
# get euclidien distance
distance = math.sqrt((self.position_x - target_x)**2 + (self.position_y - target_y)**2)
return distance
# -----------------------define get error in x,y and theta
def get_error(self,target_x,target_y,target_theta)
class Robot:
def __init__(self):
# micro controller
ev3 = EV3Brick()
ev3.speaker.beep()
# sensors
# low value = registers black tape
# high value = aluminum
self.sensorYellow = ColorSensor(Port.S1)
self.sensorRed = ColorSensor(Port.S3)
self.sensorBlue = ColorSensor(Port.S2)
# motor
left_motor = Motor(Port.A, gears=[40, 24])
right_motor = Motor(Port.B, gears=[40, 24])
axle_track = 205
wheel_diameter = 35
self.motors = DriveBase(left_motor, right_motor,
wheel_diameter, axle_track)
# constants
# intersection detection of side sensors
self.thresholdBlueSensor = 30
# value for making turns
self.thresholdSideSensors = 15
# timer
self.watch = StopWatch()
self.timer = 0
def drive(self, directions):
i = 0
while i < len(directions):
self.timer = self.watch.time()
if self.timer % 100 == 0:
print(self.timer)
self.correctPath()
if self.senseIntersection() == True and self.timer >= 500:
print('intersection')
self.motors.drive_time(0, 0, 1000) # reduce this when done
self.executeCommand(directions[i])
i += 1
self.motors.drive(-125, 0)
def executeCommand(self, cmd):
if cmd == 0:
print('straight')
self.driveStraight()
if cmd == 1:
print('right')
self.turnRight()
if cmd == 2:
print('left')
self.turnLeft()
if cmd == 3:
print('reverse')
self.reverse()
if cmd == 4:
print('stop')
# turning behaviours at intersection
def turnLeft(self):
self.motors.drive_time(-30, 44, 2000)
self.watch.reset()
def turnRight(self):
self.motors.drive_time(-30, -46, 2000)
self.watch.reset()
def driveStraight(self):
self.motors.drive_time(-60, 0, 1800)
self.watch.reset()
def reverse(self):
self.motors.drive_time(60, 0, 1800)
self.motors.drive_time(0, 94, 2000)
self.motors.drive_time(-60, 0, 800)
# intersection detection
def senseIntersection(self):
if self.sensorRed.reflection() < 2 or self.sensorYellow.reflection() < 2:
return True
# path correction
# completely aluminum = 23
# completely black tape = 1
def correctPath(self):
if self.sensorBlue.reflection() < 8:
self.adjustLeft()
if self.sensorBlue.reflection() > 16:
self.adjustRight()
# default: -125, angle
def adjustLeft(self):
angle = 12 - min(self.sensorBlue.reflection(), 10)
step = 125 + (12 - min(self.sensorBlue.reflection(), 10))
self.motors.drive(-step, angle)
def adjustRight(self):
angle = max(self.sensorBlue.reflection(), 14) -12
step = 125 + (max(self.sensorBlue.reflection(), 14) -12)
self.motors.drive(-step, -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 t = Thread(target=printAngle) tstart = watch.time() / 1000 t.start() # start angle monitor routine brick.sound.beep(400, 10) # initialization-complete sound # ========================================== speed = 100 # mm per second steer = 360/8 # degrees per second msec = 8000 # milliseconds pi = 3.14159265359 # roughly pi2 = pi * 2.0 # roughly angB = 0 angC = 0 oc = 0
while True:
# This timer measures how long a single loop takes. This will be used
# to help keep the loop time consistent, even when different actions
# are happening.
single_loop_timer.reset()
# This calculates the average control loop period. This is used in the
# control feedback calculation instead of the single loop time to
# filter out random fluctuations.
if control_loop_count == 0:
# The first time through the loop, we need to assign a value to
# avoid dividing by zero later.
average_control_loop_period = TARGET_LOOP_PERIOD / 1000
control_loop_timer.reset()
else:
average_control_loop_period = (control_loop_timer.time() / 1000 /
control_loop_count)
control_loop_count += 1
# calculate robot body angle and speed
gyro_sensor_value = gyro_sensor.speed()
gyro_offset *= (1 - GYRO_OFFSET_FACTOR) * gyro_offset
gyro_offset += GYRO_OFFSET_FACTOR * gyro_sensor_value
robot_body_rate = gyro_sensor_value - gyro_offset
robot_body_angle += robot_body_rate * average_control_loop_period
# calculate wheel angle and speed
left_motor_angle = left_motor.angle()
right_motor_angle = right_motor.angle()
previous_motor_sum = motor_position_sum
motor_position_sum = left_motor_angle + right_motor_angle
while any(brick.buttons()):
wait(10)
WaitForButton()
time = StopWatch()
ChaCha()
NewColor()
ChaCha(dir='backward')
NewColor()
TurnAround()
NewColor()
ChaCha(dir='backward')
NewColor()
ChaCha()
NewColor()
TurnAround(time=2 * 1717)
NewColor()
TurnAround(dir='ccw', time=2 * 1717)
NewColor()
print(time.time())
