class Pen:
def __init__(self, port_motor_move, port_motor_pen):
# Motor to move pen sideways
self.motor_move = LargeMotor(port_motor_move)
self.motor_move.reset()
self.motor_move.speed_sp = 400
self.motor_move.stop_action = 'brake'
self.motor_move_positions = (0, -60, -210, -350, -500, -640)
# Motor te move pen up or down
self.motor_pen = LargeMotor(port_motor_pen)
self.motor_pen.stop_action = 'hold'
# TouchSensor to indicate pen is up
self.ts_pen = TouchSensor('pistorms:BBS2')
# Move pen up
self.motor_pen.run_forever(speed_sp=-200)
self.ts_pen.wait_for_pressed()
self.motor_pen.stop()
@try_except
def pen_up(self):
''' Move pen up.'''
self.motor_pen.run_to_abs_pos(speed_sp=400, position_sp=-20)
wait_while_motors_running(self.motor_pen)
@try_except
def pen_down(self):
''' Move pen down.'''
self.motor_pen.run_to_abs_pos(speed_sp=400, position_sp=20)
wait_while_motors_running(self.motor_pen)
@try_except
def move_to_abs_pos(self, pos):
''' Move pen sidways to absolute position [0,..,5].'''
self.motor_move.run_to_abs_pos(
position_sp=self.motor_move_positions[pos])
class myRobot:
def __init__(self):
self.tankDrive = MoveTank(OUTPUT_A, OUTPUT_D)
self.motorA = LargeMotor(OUTPUT_A)
self.motorD = LargeMotor(OUTPUT_D)
self.myGyroSensor = GyroSensor()
self.myGripper = RobotGripper()
self.myUltrasonicSensor = UltrasonicSensor()
self.myColorSensor = ColorSensor()
self.Turning = False
self.Moving = False
self.myAngle = 0
self.positionX = 0
self.positionY = 0
self.myDefaultSpeed = 50
self.desiredBarCode = "BWWB"
self.myGPS = IGPS()
def getAverageDistance(self):
#Returns average distance in inches
average = (self.motorA.position + self.motorD.position) / 2
average = (average * 3.14 * (2.71 * 2))
return average
def moveStraight(self, distance):
self.motorA.reset()
self.motorD.reset()
self.tankDrive.reset()
#self.myGyroSensor.angle = 0
self.myGyroSensor.mode = 'GYRO-ANG'
currentDistance = 0
speedPercentLeft = .5
speedPercentRight = .5
while currentDistance < distance:
currentDistance = self.getAverageDistance()
if self.myGyroSensor.angle < 0:
#Turn left, left gain
gain = (-.00005 * (self.myGyroSensor.angle))
speedPercentLeft = speedPercentLeft - gain
speedPercentRight = speedPercentRight + gain
else:
#Turn moe right, right gain
gain = (.00005 * (self.myGyroSensor.angle))
speedPercentLeft = speedPercentLeft + gain
speedPercentRight = speedPercentRight - gain
self.tankDrive.on_for_degrees(self.myDefaultSpeed,
self.myDefaultSpeed,
10,
brake=False,
block=True)
sleep(.2)
#movesteer('rotations',steer = , pwr=50, rots=.1)
#Set to brake
def collisonDistance(self):
distance = self.myUltrasonicSensor.distance_inches_ping
return distance
def collisionAvoidance(self):
if self.Turning == False:
#Normal collision avoidance
print('Checking for collisions')
distance = self.collisonDistance()
if distance < 5:
self.motorA.stop()
self.motorD.stop()
print('Collision avoided')
else:
#Collision avoidance disabled
print('Not checking for collisions')
def turnToAngle(self, angle):
#self.myGyroSensor.reset()
if angle > 0:
print('Positive angle')
#Turns right
self.Turning = True
while self.myGyroSensor.value() < angle:
self.tankDrive.on_for_degrees((-.2 * self.myDefaultSpeed),
(.2 * self.myDefaultSpeed),
3,
brake=False,
block=True)
sleep(.2)
self.tankDrive.STOP_ACTION_BRAKE()
self.Turning = False
self.myAngle += self.myGyroSensor.value
self.myGyroSensor.reset
else:
#do another thing
print('Negative Angle')
self.Turning = True
while self.myGyroSensor.value() > angle:
self.tankDrive.on_for_degrees((.2 * self.myDefaultSpeed),
(-.2 * self.myDefaultSpeed),
2,
brake=False,
block=True)
sleep(.1)
self.tankDrive.STOP_ACTION_BRAKE()
self.Turning = False
self.myAngle -= self.myGyroSensor.value
self.myGyroSensor.reset
def readBarCode(self):
#String kmessage is 3 letters, 'W' denotates white, "B" denotates black
output = ""
ev3.Sound.speak('starting').wait()
for i in range(4):
self.moveStraight(.25)
self.myColorSensor.mode = 'COL-COLOR'
if self.myColorSensor.value == 1:
output = output + 'B'
ev3.Sound.speak('black').wait()
elif self.myColorSensor.value == 6:
output = output + 'W'
ev3.Sound.speak('white').wait()
sleep(1)
ev3.Sound.speak('Here is the barcode' + output).wait()
print(output)
sleep(5)
return output
# # Date Author Change Notes # 2/22/2020 Ron King Port from MindSensors Pistorms version # 3/1/2020 Ron King Fixed issues in speed change control logic and sequencing # 3/22/2020 Ron King v2.0 - change from regulated 'on', 'speed' to duty_cycle # Removed all traces of Pistorms version (issues and comments) import time, tty, sys from ev3dev2.motor import LargeMotor, OUTPUT_C, OUTPUT_D, SpeedPercent, SpeedDPS, MoveTank from ev3dev2.sensor import INPUT_1 tty.setcbreak(sys.stdin) mLift = LargeMotor(OUTPUT_C) time.sleep(0.5) mLift.reset() time.sleep(2) mLift.duty_cycle_sp = 0 mLiftDtyMax = 36 mLift.stop_action = 'coast' mLift.polarity = 'inversed' mLift.position = 0 mGrab = LargeMotor(OUTPUT_D) time.sleep(0.5) mGrab.reset() time.sleep(2) mGrab.duty_cycle_sp = 0 mGrabDtyMax = 36 mGrab.stop_action = 'coast' mGrab.polarity = 'inversed'
class Dinor3x:
FAST_WALK_SPEED = 80
NORMAL_WALK_SPEED = 40
SLOW_WALK_SPEED = 20
def __init__(self,
jaw_motor_port: str = OUTPUT_A,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.jaw_motor = MediumMotor(address=jaw_motor_port)
self.left_motor = LargeMotor(address=left_motor_port)
self.right_motor = LargeMotor(address=right_motor_port)
self.tank_driver = MoveTank(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=LargeMotor)
self.steer_driver = MoveSteering(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=LargeMotor)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.speaker = Sound()
self.roaring = False
self.walk_speed = self.NORMAL_WALK_SPEED
def roar_by_ir_beacon(self):
"""
Dinor3x roars when the Beacon button is pressed
"""
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.roaring = True
self.open_mouth()
self.roar()
elif self.roaring:
self.roaring = False
self.close_mouth()
def change_speed_by_color(self):
"""
Dinor3x changes its speed when detecting some colors
- Red: walk fast
- Green: walk normally
- White: walk slowly
"""
if self.color_sensor.color == ColorSensor.COLOR_RED:
self.speaker.speak(text='RUN!',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.walk_speed = self.FAST_WALK_SPEED
self.walk(speed=self.walk_speed)
elif self.color_sensor.color == ColorSensor.COLOR_GREEN:
self.speaker.speak(text='Normal',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.walk_speed = self.NORMAL_WALK_SPEED
self.walk(speed=self.walk_speed)
elif self.color_sensor.color == ColorSensor.COLOR_WHITE:
self.speaker.speak(text='slow...',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.walk_speed = self.SLOW_WALK_SPEED
self.walk(speed=self.walk_speed)
def walk_by_ir_beacon(self):
"""
Dinor3x walks or turns according to instructions from the IR Beacon
- 2 top/up buttons together: walk forward
- 2 bottom/down buttons together: walk backward
- Top Left / Red Up: turn left on the spot
- Top Right / Blue Up: turn right on the spot
- Bottom Left / Red Down: stop
- Bottom Right / Blue Down: calibrate to make the legs straight
"""
# forward
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right:
self.walk(speed=self.walk_speed)
# backward
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.walk(speed=-self.walk_speed)
# turn left on the spot
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel):
self.turn(speed=self.walk_speed)
# turn right on the spot
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.turn(speed=-self.walk_speed)
# stop
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel):
self.tank_driver.off(brake=True)
# calibrate legs
elif self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.calibrate_legs()
def calibrate_legs(self):
self.tank_driver.on(left_speed=10, right_speed=20)
self.touch_sensor.wait_for_released()
self.tank_driver.off(brake=True)
self.left_motor.on(speed=40)
self.touch_sensor.wait_for_pressed()
self.left_motor.off(brake=True)
self.left_motor.on_for_rotations(rotations=-0.2,
speed=50,
brake=True,
block=True)
self.right_motor.on(speed=40)
self.touch_sensor.wait_for_pressed()
self.right_motor.off(brake=True)
self.right_motor.on_for_rotations(rotations=-0.2,
speed=50,
brake=True,
block=True)
self.left_motor.reset()
self.right_motor.reset()
def walk(self, speed: float = 100):
self.calibrate_legs()
self.steer_driver.on(steering=0, speed=-speed)
def turn(self, speed: float = 100):
self.calibrate_legs()
if speed >= 0:
self.left_motor.on_for_degrees(degrees=180,
speed=speed,
brake=True,
block=True)
else:
self.right_motor.on_for_degrees(degrees=180,
speed=-speed,
brake=True,
block=True)
self.tank_driver.on(left_speed=speed, right_speed=-speed)
def close_mouth(self):
self.jaw_motor.on_for_seconds(speed=-20,
seconds=1,
brake=False,
block=False)
def open_mouth(self):
self.jaw_motor.on_for_seconds(speed=20,
seconds=1,
block=False,
brake=False)
def roar(self):
self.speaker.play_file(wav_file='T-rex roar.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.jaw_motor.on_for_degrees(speed=40,
degrees=-60,
block=True,
brake=True)
for i in range(12):
self.jaw_motor.on_for_seconds(speed=-40,
seconds=0.05,
block=True,
brake=True)
self.jaw_motor.on_for_seconds(speed=40,
seconds=0.05,
block=True,
brake=True)
self.jaw_motor.on_for_seconds(speed=20,
seconds=0.5,
brake=False,
block=True)
def main(self):
self.close_mouth()
while True:
self.roar_by_ir_beacon()
self.change_speed_by_color()
self.walk_by_ir_beacon()
print('turn1')
motor_control(40,-40)
time.sleep(1.6)
motor_control(0,0)
time.sleep(.1)
elif data == b'b':
print('turn2')
motor_control(40,-40)
time.sleep(3.3)
motor_control(0,0)
time.sleep(.1)
elif data == b'r':
print('turn3')
motor_control(40,-40)
time.sleep(4.9)
motor_control(0,0)
time.sleep(.1)
else:
motor_control(0, 0)
print(' ')
except:
LMotor.reset()
RMotor.reset()
break
client_socket.close()
class DriveTrain:
def __init__(self):
self.rc = RobotContainer.RobotContainer()
self.driveColorLeft = ColorSensor(self.rc.DRIVE_COLOR_LEFT)
self.driveColorRight = ColorSensor(self.rc.DRIVE_COLOR_RIGHT)
self.driveLeft = LargeMotor(self.rc.DRIVE_LEFT)
self.driveRight = LargeMotor(self.rc.DRIVE_RIGHT)
self.tank_drive = MoveTank(self.rc.DRIVE_LEFT, self.rc.DRIVE_RIGHT)
def followLine(self, speed, aggression, LineColor, distance):
def lineDrive():
leftColor = self.driveColorLeft.color_name
rightColor = self.driveColorRight.color_name
if leftColor in LineColor:
if rightColor in LineColor:
self.tank_drive.off()
else:
self.tank_drive.on(SpeedPercent(speed / aggression),
SpeedPercent(speed))
else:
if rightColor in LineColor:
self.tank_drive.on(SpeedPercent(speed),
SpeedPercent(speed / aggression))
else:
self.tank_drive.on(SpeedPercent(speed),
SpeedPercent(speed))
if distance == 0:
lineDrive()
else:
self.driveLeft.reset()
self.driveRight.reset()
motor1 = self.driveLeft.rotations
motor2 = self.driveRight.rotations
dist = (motor1 + motor2) / 2
rotations = distance / (self.rc.WHEEL_DIAMETER * 3.14159)
if dist <= 0:
dist *= -1
while rotations > dist:
motor1 = self.driveLeft.rotations
motor2 = self.driveRight.rotations
dist = (motor1 + motor2) / 2
if dist <= 0:
dist *= -1
lineDrive()
def driveToLine(self, speed, aggression, LineColor, StopColor):
states = self.getSensorStates(StopColor)
while states[0] != 1 or states[1] != 1:
states = self.getSensorStates(StopColor)
print(states)
self.followLine(-20, 9, LineColor, 0)
def driveForward(self, speed, distance):
rotations = distance / (self.rc.WHEEL_DIAMETER * 3.14159)
self.tank_drive.on_for_rotations(SpeedPercent(speed),
SpeedPercent(speed), rotations)
def turnAngle(self, speed, angle):
distance = (self.rc.WHEEL_DISTANCE * 3.14159 * angle) / 360
rotations = distance / (self.rc.WHEEL_DIAMETER * 3.14159)
self.tank_drive.on_for_rotations(SpeedPercent(speed),
SpeedPercent(-speed), rotations)
def getSensorStates(self, colors):
values = [0, 0]
sensor_values = [
self.driveColorLeft.color_name, self.driveColorRight.color_name
]
for i in range(len(sensor_values)):
if sensor_values[i] in colors:
values[i] = 1
return values
tachospeed_max = 0.0
goal_DPS = 0
goal_diff = 99999.99
best_goal_diff = 999999.99
optimal_speed_i = 0
start_pos = 0
end_pos = 0
start_time = 0.0
end_time = 0.0
initial_power = b1.measured_voltage
mA = LargeMotor(OUTPUT_A)
mB = LargeMotor(OUTPUT_B)
time.sleep(0.1)
mA.reset()
time.sleep(0.1)
start_time = time.time()
print("starting motors")
while (True):
print("ramping motors up")
for i in range(0, 1001, 5):
mA.on(SpeedDPS(i))
mB.on(SpeedDPS(i))
time.sleep(0.05)
print("ramping motors down")
for i in range(1000, 0, -5):
mA.on(SpeedDPS(i))
mB.on(SpeedDPS(i))
#!/usr/bin/env python3
#
# uses ev3dev2-based code
#
import time
from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, SpeedDPS, MoveTank
from ev3dev2.sensor import INPUT_1
print("stopping all motors")
mA = LargeMotor(OUTPUT_A)
mB = LargeMotor(OUTPUT_B)
mC = LargeMotor(OUTPUT_C)
mD = LargeMotor(OUTPUT_D)
time.sleep(0.1)
mA.reset()
mC.reset()
mB.reset()
mD.reset()
mA.off(brake=False)
mC.off(brake=False)
mB.off(brake=False)
mD.off(brake=False)
time.sleep(0.1)
print("all motors should now be stopped, brake = False")
class BalancerLearner(object):
"""
Base class for a robot that stands on two wheels and uses a gyro sensor.
Robot will keep its balance using reinforcement learning
"""
def __init__(self,
gain_gyro_angle=1700,
gain_gyro_rate=120,
gain_motor_angle=7,
gain_motor_angular_speed=9,
gain_motor_angle_error_accumulated=3,
power_voltage_nominal=8.0,
pwr_friction_offset_nom=3,
timing_loop_msec=30,
motor_angle_history_length=5,
gyro_drift_compensation_factor=0.05,
left_motor_port=OUTPUT_D,
right_motor_port=OUTPUT_A,
debug=False):
"""Create GyroBalancer."""
# Gain parameters
self.gain_gyro_angle = gain_gyro_angle
self.gain_gyro_rate = gain_gyro_rate
self.gain_motor_angle = gain_motor_angle
self.gain_motor_angular_speed = gain_motor_angular_speed
self.gain_motor_angle_error_accumulated =\
gain_motor_angle_error_accumulated
# Power parameters
self.power_voltage_nominal = power_voltage_nominal
self.pwr_friction_offset_nom = pwr_friction_offset_nom
# Timing parameters
self.timing_loop_msec = timing_loop_msec
self.motor_angle_history_length = motor_angle_history_length
self.gyro_drift_compensation_factor = gyro_drift_compensation_factor
# Power supply setup
self.power_supply = PowerSupply()
# Gyro Sensor setup
self.gyro = GyroSensor()
#self.gyro.mode = self.gyro.MODE_GYRO_RATE
self.gyro.mode = self.gyro.MODE_GYRO_G_A
# Touch Sensor setup
self.touch = TouchSensor()
# IR Buttons setup
# self.remote = InfraredSensor()
# self.remote.mode = self.remote.MODE_IR_REMOTE
# Configure the motors
self.motor_left = LargeMotor(left_motor_port)
self.motor_right = LargeMotor(right_motor_port)
# Sound setup
self.sound = Sound()
# Open sensor and motor files
self.gyro_angle_file = open(self.gyro._path + "/value0", "rb")
self.gyro_rate_file = open(self.gyro._path + "/value1", "rb")
self.touch_file = open(self.touch._path + "/value0", "rb")
self.encoder_left_file = open(self.motor_left._path + "/position",
"rb")
self.encoder_right_file = open(self.motor_right._path + "/position",
"rb")
self.dc_left_file = open(self.motor_left._path + "/duty_cycle_sp", "w")
self.dc_right_file = open(self.motor_right._path + "/duty_cycle_sp",
"w")
# Drive queue
self.drive_queue = queue.Queue()
# Stop event for balance thread
self.stop_balance = threading.Event()
# Debugging
self.debug = debug
# Handlers for SIGINT and SIGTERM
signal.signal(signal.SIGINT, self.signal_int_handler)
signal.signal(signal.SIGTERM, self.signal_term_handler)
def shutdown(self):
"""Close all file handles and stop all motors."""
self.stop_balance.set() # Stop balance thread
self.motor_left.stop()
self.motor_right.stop()
self.gyro_angle_file.close()
self.gyro_rate_file.close()
self.touch_file.close()
self.encoder_left_file.close()
self.encoder_right_file.close()
self.dc_left_file.close()
self.dc_right_file.close()
def _fast_read(self, infile):
"""Function for fast reading from sensor files."""
infile.seek(0)
return(int(infile.read().decode().strip()))
def _fast_write(self, outfile, value):
"""Function for fast writing to motor files."""
outfile.truncate(0)
outfile.write(str(int(value)))
outfile.flush()
def _set_duty(self, motor_duty_file, duty, friction_offset,
voltage_comp):
"""Function to set the duty cycle of the motors."""
# Compensate for nominal voltage and round the input
duty_int = int(round(duty*voltage_comp))
# Add or subtract offset and clamp the value between -100 and 100
if duty_int > 0:
duty_int = min(100, duty_int + friction_offset)
elif duty_int < 0:
duty_int = max(-100, duty_int - friction_offset)
# Apply the signal to the motor
self._fast_write(motor_duty_file, duty_int)
def signal_int_handler(self, signum, frame):
"""Signal handler for SIGINT."""
log.info('"Caught SIGINT')
self.shutdown()
raise GracefulShutdown()
def signal_term_handler(self, signum, frame):
"""Signal handler for SIGTERM."""
log.info('"Caught SIGTERM')
self.shutdown()
raise GracefulShutdown()
def balance(self):
"""Run the _balance method as a thread."""
balance_thread = threading.Thread(target=self._balance)
balance_thread.start()
def _balance(self):
"""Make the robot balance."""
while True and not self.stop_balance.is_set():
# Reset the motors
self.motor_left.reset() # Reset the encoder
self.motor_right.reset()
self.motor_left.run_direct() # Set to run direct mode
self.motor_right.run_direct()
# Initialize variables representing physical signals
# (more info on these in the docs)
# The angle of "the motor", measured in raw units,
# degrees for the EV3).
# We will take the average of both motor positions as
# "the motor" angle, which is essentially how far the middle
# of the robot has travelled.
motor_angle_raw = 0
# The angle of the motor, converted to RAD (2*pi RAD
# equals 360 degrees).
motor_angle = 0
# The reference angle of the motor. The robot will attempt to
# drive forward or backward, such that its measured position
motor_angle_ref = 0
# equals this reference (or close enough).
# The error: the deviation of the measured motor angle from the
# reference. The robot attempts to make this zero, by driving
# toward the reference.
motor_angle_error = 0
# We add up all of the motor angle error in time. If this value
# gets out of hand, we can use it to drive the robot back to
# the reference position a bit quicker.
motor_angle_error_acc = 0
# The motor speed, estimated by how far the motor has turned in
# a given amount of time.
motor_angular_speed = 0
# The reference speed during manouvers: how fast we would like
# to drive, measured in RAD per second.
motor_angular_speed_ref = 0
# The error: the deviation of the motor speed from the
# reference speed.
motor_angular_speed_error = 0
# The 'voltage' signal we send to the motor.
# We calculate a new value each time, just right to keep the
# robot upright.
motor_duty_cycle = 0
# The raw value from the gyro sensor in rate mode.
gyro_rate_raw = 0
# The angular rate of the robot (how fast it is falling forward
# or backward), measured in RAD per second.
gyro_rate = 0
# The gyro doesn't measure the angle of the robot, but we can
# estimate this angle by keeping track of the gyro_rate value
# in time.
gyro_est_angle = 0
# Over time, the gyro rate value can drift. This causes the
# sensor to think it is moving even when it is perfectly still.
# We keep track of this offset.
gyro_offset = 0
# Start
log.info("Hold robot upright. Press touch sensor to start.")
self.sound.speak("Press touch sensor to start balancing")
self.touch.wait_for_bump()
# Read battery voltage
voltage_idle = self.power_supply.measured_volts
voltage_comp = self.power_voltage_nominal / voltage_idle
# Offset to limit friction deadlock
friction_offset = int(round(self.pwr_friction_offset_nom *
voltage_comp))
# Timing settings for the program
# Time of each loop, measured in seconds.
loop_time_target = self.timing_loop_msec / 1000
loop_count = 0 # Loop counter, starting at 0
# A deque (a fifo array) which we'll use to keep track of
# previous motor positions, which we can use to calculate the
# rate of change (speed)
motor_angle_hist =\
deque([0], self.motor_angle_history_length)
# The rate at which we'll update the gyro offset (precise
# definition given in docs)
gyro_drift_comp_rate =\
self.gyro_drift_compensation_factor *\
loop_time_target * RAD_PER_SEC_PER_RAW_GYRO_UNIT
# Calibrate Gyro
log.info("-----------------------------------")
log.info("Calibrating...")
# As you hold the robot still, determine the average sensor
# value of 100 samples
gyro_calibrate_count = 100
for i in range(gyro_calibrate_count):
gyro_data = self._fast_read(self.gyro_rate_file)
#log.info(gyro_data)
#gyro_other_data = self._fast_read(self.gyro_other)
#log.info(gyro_other_data)
gyro_offset = gyro_offset + gyro_data
#self.gyro_data_file.write(str(gyro_data), "\n")
#self.gyro_data_file.flush()
time.sleep(0.01)
gyro_offset = gyro_offset / gyro_calibrate_count
#gyro_offset = 0
# Print the result
log.info("gyro_offset: " + str(gyro_offset))
log.info("-----------------------------------")
log.info("GO!")
log.info("-----------------------------------")
log.info("Press Touch Sensor to re-start.")
log.info("-----------------------------------")
self.sound.beep()
# Remember start time
prog_start_time = time.time()
if self.debug:
# Data logging
data = OrderedDict()
loop_times = OrderedDict()
data['loop_times'] = loop_times
gyro_angles = OrderedDict()
data['gyro_angles'] = gyro_angles
gyro_rates_raw = OrderedDict()
data['gyro_rates_raw'] = gyro_rates_raw
gyro_est_angles = OrderedDict()
data['gyro_est_angles'] = gyro_est_angles
gyro_rates = OrderedDict()
data['gyro_rates'] = gyro_rates
motor_angle_errors = OrderedDict()
data['motor_angle_errors'] = motor_angle_errors
motor_angular_speed_errors = OrderedDict()
data['motor_angular_speed_errors'] = motor_angular_speed_errors
motor_angle_errors_acc = OrderedDict()
data['motor_angle_errors_acc'] = motor_angle_errors_acc
motor_duty_cycles = OrderedDict()
data['motor_duty_cycles'] = motor_duty_cycles
# Initial fast read touch sensor value
touch_pressed = False
# Driving and Steering
speed, steering = (0, 0)
# Record start time of loop
loop_start_time = time.time()
# Balancing Loop
while not touch_pressed and not self.stop_balance.is_set():
loop_count += 1
# Check for drive instructions and set speed / steering
try:
speed, steering = self.drive_queue.get_nowait()
self.drive_queue.task_done()
except queue.Empty:
pass
# Read the touch sensor (the kill switch)
touch_pressed = self._fast_read(self.touch_file)
# Read the Motor Position
motor_angle_raw = ((self._fast_read(self.encoder_left_file) +
self._fast_read(self.encoder_right_file)) /
2.0)
motor_angle = motor_angle_raw * RAD_PER_RAW_MOTOR_UNIT
# Read the Gyro
gyro_rate_raw = self._fast_read(self.gyro_rate_file)
#log.info(gyro_rate_raw)
# Busy wait for the loop to reach target time length
loop_time = 0
while(loop_time < loop_time_target):
loop_time = time.time() - loop_start_time
time.sleep(0.001)
# Calculate most recent loop time
loop_time = time.time() - loop_start_time
# Set start time of next loop
loop_start_time = time.time()
#if self.debug:
# Log gyro data and loop time
#time_of_sample = time.time() - prog_start_time
#gyro_angles[time_of_sample] = self._fast_read(self.gyro_angle_file)
#gyro_rates[time_of_sample] = gyro_rate_raw
#loop_times[time_of_sample] = loop_time * 1000.0
# Calculate gyro rate
gyro_rate = (gyro_rate_raw - gyro_offset) *\
RAD_PER_SEC_PER_RAW_GYRO_UNIT
# Calculate Motor Parameters
motor_angular_speed_ref =\
speed * RAD_PER_SEC_PER_PERCENT_SPEED
motor_angle_ref = motor_angle_ref +\
motor_angular_speed_ref * loop_time_target
motor_angle_error = motor_angle - motor_angle_ref
# Compute Motor Speed
motor_angular_speed =\
((motor_angle - motor_angle_hist[0]) /
(self.motor_angle_history_length * loop_time_target))
motor_angular_speed_error = motor_angular_speed
motor_angle_hist.append(motor_angle)
# Compute the motor duty cycle value
motor_duty_cycle =\
(self.gain_gyro_angle * gyro_est_angle +
self.gain_gyro_rate * gyro_rate +
self.gain_motor_angle * motor_angle_error +
self.gain_motor_angular_speed *
motor_angular_speed_error +
self.gain_motor_angle_error_accumulated *
motor_angle_error_acc)
# Apply the signal to the motor, and add steering
self._set_duty(self.dc_right_file, motor_duty_cycle + steering,
friction_offset, voltage_comp)
self._set_duty(self.dc_left_file, motor_duty_cycle - steering,
friction_offset, voltage_comp)
# Update angle estimate and gyro offset estimate
gyro_est_angle = gyro_est_angle + gyro_rate *\
loop_time_target
gyro_offset = (1 - gyro_drift_comp_rate) *\
gyro_offset + gyro_drift_comp_rate * gyro_rate_raw
# Update Accumulated Motor Error
motor_angle_error_acc = motor_angle_error_acc +\
motor_angle_error * loop_time_target
if self.debug:
# Log gyro data and loop time
time_of_sample = time.time() - prog_start_time
gyro_angles[time_of_sample] = self._fast_read(self.gyro_angle_file)
gyro_rates_raw[time_of_sample] = gyro_rate_raw
loop_times[time_of_sample] = loop_time * 1000.0
motor_duty_cycles[time_of_sample] = motor_duty_cycle
gyro_est_angles[time_of_sample] = gyro_est_angle
gyro_rates[time_of_sample] = gyro_rate
motor_angle_errors[time_of_sample] = motor_angle_error
motor_angular_speed_errors[time_of_sample] = motor_angular_speed_error
motor_angle_errors_acc[time_of_sample] = motor_angle_error_acc
# Closing down & Cleaning up
# Loop end time, for stats
prog_end_time = time.time()
# Turn off the motors
self._fast_write(self.dc_left_file, 0)
self._fast_write(self.dc_right_file, 0)
# Wait for the Touch Sensor to be released
while self.touch.is_pressed:
time.sleep(0.01)
# Calculate loop time
avg_loop_time = (prog_end_time - prog_start_time) / loop_count
log.info("Loop time:" + str(avg_loop_time * 1000) + "ms")
# Print a stop message
log.info("-----------------------------------")
log.info("STOP")
log.info("-----------------------------------")
if self.debug:
# Dump logged data to file
with open("data.txt", 'w') as data_file:
json.dump(data, data_file)
def _move(self, speed=0, steering=0, seconds=None):
"""Move robot."""
self.drive_queue.put((speed, steering))
if seconds is not None:
time.sleep(seconds)
self.drive_queue.put((0, 0))
self.drive_queue.join()
def move_forward(self, seconds=None):
"""Move robot forward."""
self._move(speed=SPEED_MAX, steering=0, seconds=seconds)
def move_backward(self, seconds=None):
"""Move robot backward."""
self._move(speed=-SPEED_MAX, steering=0, seconds=seconds)
def rotate_left(self, seconds=None):
"""Rotate robot left."""
self._move(speed=0, steering=STEER_MAX, seconds=seconds)
def rotate_right(self, seconds=None):
"""Rotate robot right."""
self._move(speed=0, steering=-STEER_MAX, seconds=seconds)
def stop(self):
"""Stop robot (balancing will continue)."""
self._move(speed=0, steering=0)
class Printer:
colors = ('unknown', 'black', 'blue', 'green', 'yellow', 'red', 'white',
'brown')
def __init__(self, pixel_size):
self._touch = TouchSensor(INPUT_1)
self._color = ColorSensor(INPUT_4)
self._color.mode = 'COL-COLOR'
self._fb_motor = LargeMotor(OUTPUT_C)
self._lr_motor = LargeMotor(OUTPUT_B)
self._ud_motor = Motor(OUTPUT_A)
self._x_res = utilities.MAX_X_RES / int(pixel_size)
self._y_res = utilities.MAX_Y_RES / int(pixel_size)
self._padding_left = utilities.MAX_PADDING_LEFT / int(pixel_size)
self._padding_right = utilities.MAX_PADDING_RIGHT / int(pixel_size)
self._is_pen_up = True
self._pen_calibrated = False
self._ud_ratio = 5
self._fb_ratio = 4
self._lr_ratio = 1
self._pen_up_val = -3 * self._ud_ratio
self._pen_down_val = -self._pen_up_val
self._pen_up_down_speed = 10
self._pen_left_speed = 20
self._pen_right_speed = 20
self._paper_scroll_speed = 20
self._pixel_size = pixel_size
self._p_codes = []
self._current_line = 0
def _pen_up(self, val):
print("{} {}".format('PEN_UP', val))
if val > 0:
self._ud_motor.on_for_degrees(self._pen_up_down_speed,
val * self._pen_up_val)
self._is_pen_up = True
def _pen_down(self, val):
print("{} {}".format('PEN_DOWN', val))
if val > 0:
self._ud_motor.on_for_degrees(self._pen_up_down_speed,
val * self._pen_down_val)
self._is_pen_up = False
def _pen_left(self, val):
print("{} {}".format('PEN_LEFT', val))
if val > 0:
self._lr_motor.on_for_degrees(
self._pen_left_speed,
int(self._pixel_size) * val * self._lr_ratio)
def _pen_right(self, val):
print("{} {}".format('PEN_RIGHT', val))
if val > 0:
self._lr_motor.on_for_degrees(
self._pen_right_speed,
-int(self._pixel_size) * val * self._lr_ratio)
def _paper_scroll(self, val):
print("{} {}".format('SCROLL', val))
if val > 0:
if val == 1:
self._current_line += val
print("-------------- Line {} --------------".format(
self._current_line))
self._fb_motor.on_for_degrees(
self._paper_scroll_speed,
int(self._pixel_size) * val * self._fb_ratio)
def _finish_calibration(self):
self._pen_calibrated = True
def calibrate(self, quick_calibration):
speaker = Sound()
if quick_calibration:
speaker.speak("Quick calibration")
print("Quick calibration...")
else:
speaker.speak("Calibrating")
print("Calibrating...")
btn = Button()
self._lr_motor.reset()
self._ud_motor.reset()
self._fb_motor.reset()
self._lr_motor.on_for_degrees(
self._pen_left_speed,
self._x_res * self._pixel_size * self._lr_ratio)
self._lr_motor.reset()
self._ud_motor.on(-15)
self._touch.wait_for_pressed()
self._ud_motor.stop()
time.sleep(1)
self._ud_motor.on(15)
self._touch.wait_for_released()
self._ud_motor.on_for_degrees(10, 40)
self._ud_motor.stop()
time.sleep(1)
speaker.speak(
"Insert calibration paper and press the touch sensor")
self._touch.wait_for_pressed()
speaker.speak("Adjust pen height")
print("Adjust pen height...")
while not self._pen_calibrated:
self._lr_motor.on_for_degrees(
self._pen_right_speed,
-100 * self._pixel_size * self._lr_ratio)
self._lr_motor.on_for_degrees(
self._pen_left_speed,
100 * self._pixel_size * self._lr_ratio)
time.sleep(1)
if btn.up:
self._pen_up(1)
elif btn.down:
self._pen_down(1)
elif btn.right:
self._finish_calibration()
elif btn.left:
self._pen_down(4)
self._lr_motor.reset()
if not self._is_pen_up:
self._pen_up(1)
self._pen_left(self._x_res)
for _ in range(2):
self._pen_right(self._x_res)
self._pen_left(self._x_res)
for _ in range(8):
self._pen_right(self._padding_left)
for _ in range(int(self._x_res)):
self._pen_right(1)
self._pen_left(self._x_res + self._padding_left)
self._lr_motor.reset()
speaker.speak(
"Insert a blank piece of paper and press the touch sensor")
self._touch.wait_for_pressed()
self._fb_motor.on(5)
val = 0
print("Scrolling the piece of paper to its starting position...")
while self.colors[val] == 'unknown':
val = self._color.value()
self._fb_motor.reset()
print("Calibration done")
def _interpret_p_codes(self, p_codes):
btn = Button()
speaker = Sound()
self._current_line = 0
abort = False
print("---------- p_codes:----------")
print("-------------- Line {} --------------".format(
self._current_line))
for x in p_codes:
if btn.down:
speaker.speak("Aborting")
print("\nAborting...")
abort = True
break
if x[0] == utilities.Command.PEN_UP:
if not self._is_pen_up:
self._pen_up(x[1])
elif x[0] == utilities.Command.PEN_DOWN:
if self._is_pen_up:
self._pen_down(x[1])
elif x[0] == utilities.Command.PEN_RIGHT:
self._pen_right(x[1])
elif x[0] == utilities.Command.PEN_LEFT:
self._pen_left(x[1])
elif x[0] == utilities.Command.SCROLL:
self._paper_scroll(x[1])
if not self._is_pen_up:
self._pen_up(1)
self._ud_motor.stop()
return abort
def draw(self, path=None, multicolor=False):
speaker = Sound()
if path is not None:
if multicolor:
speaker.speak("Printing multi color image")
print("\nPrinting multi color image...")
else:
speaker.speak("Printing single color image")
print("\nPrinting single color image...")
binarized, img_x, img_y = utilities.binarize_image(
path, self._x_res, self._y_res, multicolor)
else:
binarized, img_x, img_y = utilities.generate_and_binarize_test_image(
self._pixel_size)
speaker.speak("Printing test page")
print("\nPrinting test page...")
quick_calibration = False
for layer, i in zip(binarized, range(1, utilities.NUM_OF_COLORS)):
speaker.speak("Insert a {} pen and press the touch sensor".format(
utilities.palette_names[i]))
self._touch.wait_for_pressed()
self.calibrate(quick_calibration)
p_codes = utilities.binarized_image_to_p_codes(
layer, img_x, img_y, self._padding_left)
if self._interpret_p_codes(p_codes):
break
self._paper_scroll(self._y_res)
self._fb_motor.reset()
self._ud_motor.reset()
self._lr_motor.reset()
quick_calibration = True
speaker = Sound()
speaker.speak("Printing finished")
print("Printing finished")
class myRobot():
def __init__(self,
motorPort1,
motorPort2,
modulePort,
colorPort1,
colorPort2,
gyro1=None,
gyro2=None,
motorDiff=1,
colMode="COL-COLOR",
moduleSensor=None,
printErrors=True,
enableConsole=False,
modulePort2=None):
if motorPort1 and motorPort2:
self.ms = MoveSteering(
motorPort1,
motorPort2) # If defined in parameters, define MoveSteering
if motorPort1 and motorPort2:
self.mt = MoveTank(
motorPort1,
motorPort2) # If defined in parameters, define MoveTank
if motorPort1:
self.m1 = LargeMotor(
motorPort1) # If defined in parameters, define Left Motor
if motorPort2:
self.m2 = LargeMotor(
motorPort2) # If defined in parameters, define Right Motor
if modulePort:
self.mmt = Motor(modulePort)
# If defined in parameters, define module motor
if modulePort2:
self.mmt2 = Motor(modulePort2)
# If defined in parameters, define module motor
if enableConsole: self.resetMotors()
if colorPort1 != None: # If defined in parameters, define Left Color sensor
self.csLeft = ColorSensor(colorPort1)
self.csLeft.mode = colMode # Set color mode to the one in the parameters
if colorPort2 != None: # If defined in parameters, define Right Color sensor
self.csRight = ColorSensor(colorPort2)
self.csRight.mode = colMode # Set color mode to the one in the parameters
if moduleSensor != None: # If defined in parameters, define module sensor
self.moduleSensor = ColorSensor(moduleSensor)
self.moduleSensor.mode = "COL-COLOR"
try:
self.gs = GyroSensor(gyro1)
self.gs.mode = "GYRO-RATE"
self.gs.mode = "GYRO-ANG"
except:
print("Gyro 1 can't be defined!"
) # Define gyro if possible, otherwise show error
try:
self.gs2 = GyroSensor(gyro2)
self.gs2.mode = "GYRO-RATE"
self.gs2.mode = "TILT-ANG"
except:
print("Gyro 2 can't be defined!"
) # Define gyro if possible, otherwise show error
self.using2ndGyro = False # Set default gyro to the 1st one
self.motorDiff = motorDiff # Set motor difference to the one defined
self.leds = Leds() # Setup brick leds
self.bt = Button() # Setup brick buttons
if enableConsole:
self.console = Console(
font="Lat15-Terminus32x16") # Enable console access if needed
try:
with open("/home/robot/sappers2/pid.txt", "w") as pid:
pid.write(str(os.getpid()))
except:
pass # Write PID into file for other applications.
if printErrors: print("Successfully Initialized. ")
self.timerStart = time.time()
def prepareForRun(self): # Reset gyro and motors
self.ms.reset()
self.gyroReset()
def changeGyro(self, newGyroPort): # Change gyro to 2nd one
try:
self.gs = GyroSensor(newGyroPort)
self.using2ndGyro = True
except:
print("Can't change Gyro!")
def attachNew(self,
isMotor=True,
port="outA",
largeMotor=False): # Attach new motor
if isMotor:
self.m3 = Motor(port)
else:
self.m3 = LargeMotor(port)
def followLine(self,
timeOfGoing,
speed,
multiplier=5,
debug=False): # Simple line follower
currentTime = time.time()
while time.time() - currentTime < timeOfGoing:
colorLeft = int(self.csLeft.value() / 1)
colorRight = int(self.csRight.value() / 1)
print(colorLeft, colorRight)
if colorLeft != 6 and colorRight != 6:
tempSteering = 0
elif colorLeft != 6:
tempSteering = -multiplier
elif colorRight != 6:
tempSteering = multiplier
else:
tempSteering = 0
self.ms.on(tempSteering, speed * self.motorDiff)
def goByWay(self,
waySteering,
timeToGo,
speed=80,
debug=False): # MoveSteering but for time
currentTime = time.time()
while time.time() - currentTime < timeToGo:
self.ms.on(waySteering, speed * self.motorDiff)
def goUntilLine(self,
speed,
lineColor,
multiplier=1,
debug=False
): # Goes forward with gyro, but stops when detecting line
pastGyro = self.gs.value()
while (self.csLeft.value() not in lineColor) and (self.csRight.value()
not in lineColor):
self.ms.on((self.gs.value() - pastGyro) * multiplier,
speed * self.motorDiff)
if debug:
print(self.gs.value(), self.csLeft.value(),
self.csRight.value())
print(self.gs.value(), self.csLeft.value(), self.csRight.value())
def goWithGyro(self, timeToGo, speed=70, multiplier=2, debug=False):
# Forward with gyro for time
pastGyro = self.gs.value()
currentTime = time.time()
while time.time() - currentTime < timeToGo:
self.ms.on(self.max100((self.gs.value() - pastGyro) * multiplier),
speed * self.motorDiff)
if debug: print(self.gs.value())
def moveModule(self,
length,
speed=30,
stopMoving=True,
useLarge=False,
waitAfter=False): # Module motor access
if useLarge:
try:
self.m3.on(speed)
time.sleep(length)
if stopMoving: self.m3.reset()
except:
print("not working bruh you should fix it like rn")
else:
while True:
try:
self.mmt.on(speed * -1)
time.sleep(length)
if stopMoving: self.mmt.reset()
if waitAfter: time.sleep(waitAfter)
break
except Exception as e:
print(e)
break
def moveModuleByRot(self,
rotations,
speed,
block=True): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=block)
def moveModuleByRot2(self,
rotations,
speed,
block=True): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=block)
def moveBothModules(self,
rotations,
speed,
block=True,
speed2=False): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=False)
self.mmt2.on_for_rotations((speed2 if speed2 else -speed),
rotations,
block=block)
def resetMotors(self): # Motor reset
self.ms.reset()
self.mmt.on(100)
time.sleep(1)
self.mmt.reset()
def gyroReset(self): # Gyro reset
self.gs.mode = "GYRO-RATE"
self.gs.mode = "GYRO-ANG"
try:
self.gs2.mode = "GYRO-RATE"
self.gs2.mode = "TILT-ANG"
except:
print("Gyro2 not found!")
def resetGyro(self, gyroObject, desiredMode="TILT-ANG"):
gyroObject.mode = "GYRO-RATE"
gyroObject.mode = desiredMode
def fullStop(self, brakeOrNo=True): # Stops the robot, breaks if defined.
self.ms.stop(brake=brakeOrNo)
self.mmt.reset()
try:
self.mmt2.reset()
except:
pass
def turnWithGyro(
self,
degreesToTurn=90,
speedToTurnAt=20): # Turns with gyro, and saves past value
gyroPast = self.gs.value()
self.ms.on(90, speedToTurnAt)
while gyroPast - self.gs.value() <= degreesToTurn:
print(gyroPast - self.gs.value())
self.ms.stop(brake=True)
print(self.gs.value())
def justGo(self, speed,
timeOfGoing): # Goes forward without gyro, for time
self.ms.on(0, speed * self.motorDiff)
time.sleep(timeOfGoing)
def justTurn(self, speed,
timeOfGoing): # Turns withot gyro, useful for quick turns.
self.ms.on(90, speed * self.motorDiff)
time.sleep(timeOfGoing)
def absoluteTurn(self,
speed,
absoluteLoc,
rotWay=-1,
giveOrTake=1,
debug=False,
ver2=False): # Turns using absolute value from gyro
self.ms.on(90 * rotWay, speed * self.motorDiff)
# while self.gs.value() > absoluteLoc+giveOrTake or self.gs.value() < absoluteLoc+giveOrTake :
# if debug: print(self.gs.value())
# pass
while self.gs.value() > absoluteLoc + giveOrTake or self.gs.value(
) < absoluteLoc - giveOrTake:
if debug: print(self.gs.value())
pass
def isPositive(self, number):
if number > 0:
return 1
elif number < 0:
return -1
else:
return 0
def absolute(self, speed, location, giveOrTake=2, debug=False):
"""
Absolute turning
"""
while self.gs.value() > location + giveOrTake or self.gs.value(
) < location - giveOrTake:
self.ms.on(90 * self.isPositive(self.gs.value() - location),
speed * self.motorDiff)
if debug: print(self.gs.value())
pass
def turnForMore(
self,
speed,
minDeg,
rotWay=-1,
debug=False,
ver2=False
): # Turns with gyro, but stops quicker than absolute turn (useful for big turns)
if debug: print(self.gs.value())
self.ms.on(90 * rotWay, speed * self.motorDiff)
if ver2:
currentDeg = int(self.gs.value())
while minDeg - 1 <= currentDeg <= minDeg + 1:
if debug: print(self.gs.value())
else:
if rotWay < 0:
while int(self.gs.value()) < int(minDeg):
if debug: print(self.gs.value())
pass
else:
while int(self.gs.value()) > int(minDeg):
if debug: print(self.gs.value())
pass
def goWithShift(self,
timeToGo,
speed=70,
multiplier=2,
shift=1): # Goes with gyro using shift
pastGyro = self.gs.value()
currentTime = time.time()
while time.time() - currentTime < timeToGo:
if self.gs.value() == 0:
gsValue = shift
else:
gsValue = self.gs.value()
self.ms.on((gsValue - pastGyro) * multiplier,
speed * self.motorDiff)
def goWithGyroRot(self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False
): # Goes using gyro using one motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = self.m1.position
steerDiff = -1 if speed < 0 else 1
while abs((currentRot - self.m1.position) / 360) < rotations:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on((self.gs.value() - pastGyro) * multiplier * steerDiff,
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
def waitForColor(self,
color=[1, 2, 3, 4, 5, 6],
amountOfTime=0.5,
debug=False,
waitingTime=0.5): # Waits until color detected
colorArray = []
while True:
if self.moduleSensor.value() in color:
colorArray.append(self.moduleSensor.value())
time.sleep(amountOfTime / 10)
if debug: print(colorArray, end="\r")
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
elif self.bt.down == True:
print("awaiting input")
return False
else:
colorArray = []
if debug: print("waiting", self.moduleSensor.value(), end="\r")
self.leds.set_color("LEFT", "RED")
self.leds.set_color("RIGHT", "RED")
if len(colorArray) == 9:
break
self.leds.set_color("LEFT", "ORANGE")
self.leds.set_color("RIGHT", "AMBER")
time.sleep(waitingTime)
if debug: print(max(set(colorArray), key=colorArray.count), end="\n")
return max(set(colorArray), key=colorArray.count)
def turnWithGyro2(self,
degreesToTurn=90,
speedToTurnAt=20,
debug=False): # Deprecated turn with gyro
gyroPast = self.gs.value()
while gyroPast - self.gs.value() <= degreesToTurn:
self.ms.on(90, ((degreesToTurn - speedToTurnAt) + abs(
(degreesToTurn - speedToTurnAt))) / 2)
if debug: print(gyroPast - self.gs.value())
def startRun(self,
toExec,
resetGyro=True,
colorArray=[3]): # Starts run using magic wand
self.waitForColor([3], 1, True)
self.gyroReset()
exec(toExec)
def gyroSlowLinearly(
self,
rotations,
startSpeed,
multiplier,
minSpeed=5,
debug=False,
startValue=None,
stopAtEnd=False
): # Goes with gyro and slows down as it nears to destination
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = self.m1.position
steerDiff = -1 if startSpeed < 0 else 1
while abs((currentRot - self.m1.position) / 360) < rotations:
currSpeed = 1 - (
(int(abs((currentRot - self.m1.position) / 360) * 100) / 100) /
rotations)
if debug:
print(
currSpeed,
round(
(startSpeed * currSpeed + minSpeed) * self.motorDiff))
self.ms.on(
(self.gs.value() - pastGyro) * multiplier * steerDiff,
round((startSpeed * currSpeed + minSpeed) * self.motorDiff))
if stopAtEnd: self.ms.stop(brake=True)
def goWithGyroRot2(
self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False,
timeout=100000): # Goes forward using both motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
startTime = time.time()
while abs((currentRot - (self.m1.position + self.m1.position) / 2) /
360) < rotations or time.time() - startTime >= timeout:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on((self.gs.value() - pastGyro) * multiplier * steerDiff,
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
def returnButtons(self, pack2=False): # Returns pressed buttons for menu
if pack2:
packable = list(sys.stdin.readline().replace("\x1b[", "").rstrip())
return [''.join(x) for x in zip(packable[0::2], packable[1::2])]
else:
return sys.stdin.readline().replace("\x1b[", "").rstrip()
def goWithGyroForLevel(
self,
startRots,
speed,
multiplier,
debug=False,
startValue=None,
levelNeeded=5,
overrideDegrees=0
): # Goes forward with gyro until water-level is detected
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
self.goWithGyroRot2(startRots, speed, multiplier, False, startValue,
False)
print(abs(self.gs2.value()))
while abs(self.gs2.value()) > levelNeeded:
if debug:
print(
(abs(self.gs2.value()), self.gs.value(),
((self.gs.value() - pastGyro) * multiplier * steerDiff) +
overrideDegrees))
self.ms.on(
self.max100((
(self.gs.value() - pastGyro) * multiplier * steerDiff) +
overrideDegrees), speed * self.motorDiff)
def playSound(self, mscCommand): # Plays a sound
try:
os.system(mscCommand)
except:
pass
def max100(self, value): # Sets value to a max of 100
if value > 100:
return 100
elif value < -100:
return -100
else:
return value
def sleep(self, seconds): # sleep.
time.sleep(seconds)
def stopAtLine(
self,
speed,
color,
correctionSpeed=5): # Goes forward and stops when detecting lines
pastGyro = self.gs.value()
while True:
if self.csLeft.value() == color and self.csRight.value() == color:
break
elif self.csRight.value() != color and self.csLeft.value(
) == color:
self.mt.on(0, correctionSpeed * self.motorDiff)
elif self.csLeft.value() != color and self.csRight.value(
) == color:
self.mt.on(correctionSpeed * self.motorDiff, 0)
elif self.csLeft.value() != color and self.csRight.value(
) != color:
self.ms.on((self.gs.value() - pastGyro),
speed * self.motorDiff)
print({
"right": self.csRight.value(),
"left": self.csLeft.value(),
"gyro": self.gs.value()
})
print(
"final", {
"right": self.csRight.value(),
"left": self.csLeft.value(),
"gyro": self.gs.value()
})
def stayInPlace(self, multiplier
): # Using gyro and motor values, keeps the robot in place
motor1 = self.m1.position
motor2 = self.m2.position
while True:
corrig1 = -(self.m1.position - motor1) * multiplier
corrig2 = -(self.m2.position - motor2) * multiplier
if round(corrig1, 2) == 0 and round(corrig2, 2) == 0:
self.mt.stop(brake=False)
else:
self.mt.on(corrig1, corrig2)
print(corrig1, corrig2)
# print(corrig1, corrig2)
def moveModuleRepeating(self, raiseTime, downTime, speed1, speed2,
iterations): # Module motor access
for i in range(iterations):
self.moveModule(raiseTime, speed1, False)
self.moveModule(downTime, speed2, True)
def section(self, sectionName, resetTimer=False):
"""
Provides timing functionality, and makes it easier to see sections of runs in logs.
`resetTimer` parameter optional, it will reset the main runTimer.
"""
if resetTimer:
self.timerStart = time.time()
print(sectionName)
else:
print("%s, current run time: %i" %
(sectionName, self.timerStart - time.time()))
def goWithGyroRot2Maxed(
self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False,
timeout=100000): # Goes forward using both motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
startTime = time.time()
while abs((currentRot - (self.m1.position + self.m1.position) / 2) /
360) < rotations or time.time() - startTime >= timeout:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on(
self.max100(
(self.gs.value() - pastGyro) * multiplier * steerDiff),
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
