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)