#code for all the missions mentioned are up #-------------------------------------------------------------------------------------------------------------------------------------------- #This is where the movement happens. the function "ColorChecking" is a function to decide what to do based on color. #--------------- failsafe ------------- def failsafe(): sys.exit() #-------------------------------------- #this is when the code accually starts Sound_.play_tone( frequency=400, duration=0.5, volume=50 ) #there is a 15-20 second lag when we start a program so this tells us that master has alreafy started by beeping start = time.time( ) #this makes it so that when we call start it is equal to what ever far the robot is in the code (IE: 00:12 if it was 12 seconds in) btn.on_backspace = failsafe while True: #the code that is indented repetes forever untill we stop the program if (btn.down): #if the down button is pressed: break #leave the loop pass btn.wait_for_released( 'enter') #waits untill the enter(middle) button is pressed ColorChecking() #calls the function color checking see above # Beepity beep! Sound_.play_tone(frequency=400, duration=0.5, volume=50)
class Robot: def __init__(self): self.sound = Sound() self.direction_motor = MediumMotor(OUTPUT_D) self.swing_motorL = LargeMotor(OUTPUT_A) self.swing_motorC = LargeMotor(OUTPUT_B) self.swing_motorR = LargeMotor(OUTPUT_C) self.swing_motors = [ self.swing_motorL, self.swing_motorC, self.swing_motorR ] self.touch_sensor = TouchSensor(INPUT_1) self.console = Console(DEFAULT_FONT) self.buttons = Button() self.beeps_enabled = True def beep(self, frequency=700, wait_for_comeplete=False): if not self.beeps_enabled: return play_type = Sound.PLAY_WAIT_FOR_COMPLETE if wait_for_comeplete else Sound.PLAY_NO_WAIT_FOR_COMPLETE self.sound.beep("-f %i" % frequency, play_type=play_type) def calibrate_dir(self): ''' Calibrate direction motor ''' # Run motor with 25% power, and wait until it stops running self.direction_motor.on(SpeedPercent(-10), block=False) # while (not self.direction_motor.STATE_OVERLOADED): # print(self.direction_motor.duty_cycle) self.direction_motor.wait_until(self.direction_motor.STATE_OVERLOADED) self.direction_motor.stop_action = Motor.STOP_ACTION_COAST self.direction_motor.stop() time.sleep(.5) # Reset to straight # self.direction_motor.on_for_seconds(SpeedPercent(10), .835, brake=True, block=True) self.direction_motor.on_for_degrees(SpeedPercent(10), 127, brake=True, block=True) self.direction_motor.reset() print("Motor reset, position: " + str(self.direction_motor.position)) time.sleep(.5) def calibrate_swing(self): for m in self.swing_motors: m.stop_action = m.STOP_ACTION_HOLD m.on(SpeedPercent(6)) self.swing_motorC.wait_until(self.swing_motorC.STATE_OVERLOADED, 2000) for m in self.swing_motors: m.stop_action = m.STOP_ACTION_HOLD m.on_for_degrees(SpeedPercent(5), -15, brake=True, block=False) self.swing_motorC.wait_while('running') for m in self.swing_motors: m.reset() m.stop_action = m.STOP_ACTION_HOLD m.stop() print("Ready Angle: %i" % self.swing_motorC.position) def ready_swing(self, angle: int): right_angle = -(angle / 3) # adjust motors to target angle for m in self.swing_motors: m.stop_action = Motor.STOP_ACTION_HOLD m.on_for_degrees(SpeedPercent(2), right_angle, brake=True, block=False) self.swing_motorC.wait_while('running') for m in self.swing_motors: m.stop_action = Motor.STOP_ACTION_HOLD m.stop() print("Swing Angle: %i" % self.swing_motorC.position) def set_direction(self, direction): print("Setting direction to: " + str(direction)) #direction = self.__aim_correction(direction) self.direction_motor.on_for_degrees(SpeedPercent(10), round(direction * 3)) print("Direction set to: " + str(self.direction_motor.position)) # # def __aim_correction(self, direction): # x = direction # y = -0.00000000429085685725*x**6 + 0.00000004144345630728*x**5 + 0.00001219331494759860*x**4 + 0.00020702946527870400*x**3 + 0.00716486965517779000*x**2 + 1.29675836037884000000*x + 0.27064829453014400000 # # return y def shoot(self, power): print("SHOOT, power: %i" % power) for m in self.swing_motors: m.duty_cycle_sp = -power for m in self.swing_motors: m.run_direct() time.sleep(.5) self.swing_motorC.wait_until_not_moving() for m in self.swing_motors: m.reset() def wait_for_button(self): self.touch_sensor.wait_for_bump() def __set_display(self, str): self.console.set_font(font=LARGER_FONT, reset_console=True) self.console.text_at(str, column=1, row=1, reset_console=True) def wait_for_power_select(self, power=0, angle=0, steps=1): self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) def left(): power -= steps if power < 0: power = 0 self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) self.buttons.wait_for_released(buttons=['left'], timeout_ms=150) def right(): power += steps if power > 100: power = 100 self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) self.buttons.wait_for_released(buttons=['right'], timeout_ms=150) def up(): angle += steps if angle > 110: angle = 110 self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) self.buttons.wait_for_released(buttons=['up'], timeout_ms=150) def down(): angle -= steps if angle < 0: angle = 0 self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) self.buttons.wait_for_released(buttons=['down'], timeout_ms=150) while not self.touch_sensor.is_pressed: if self.buttons.left: left() elif self.buttons.right: right() elif self.buttons.up: up() elif self.buttons.down: down() self.console.set_font(font=DEFAULT_FONT, reset_console=True) return (power, angle) def select_connection_mode(self): self.__set_display("Enable Connection\nLeft: True - Right: False") enabled = True while not self.touch_sensor.is_pressed: if self.buttons.left: enabled = True self.buttons.wait_for_released(buttons=['left']) break elif self.buttons.right: enabled = False self.buttons.wait_for_released(buttons=['right']) break self.console.set_font(font=DEFAULT_FONT, reset_console=True) return enabled def print(self, string): print(string)
def cmToRotations(cm): return cm/wheelCircumference_cm # inches to millimeters: def inToMillimeters(inches): return inches * 25.4 # centimeters to millimeters: def cmToMillimeters(cm): #hhm it works... questionable -- no syntax errors! return cm * 10 # Yay, no syntax errors! def drive_cm(power, cm): rt = cmToRotations(cm) tank_drive.on_for_rotations(SpeedPercent(power), SpeedPercent(power), int(rt) ) def drive_cm_new(power, cm): rt = cmToRotations(cm) tank_drive.on_for_rotations(SpeedPercent(power), SpeedPercent(power), rt) #--------------------------------------------------------------------------------------------------------------------------------------------- while True: if (btn.down): break pass btn.wait_for_released('enter') finish = time.time() Display_.clear() Display_.draw.text((0,0), str(finish - start), font=fonts.load('helvR24')) Display_.update() time.sleep(5) drive_cm_new(50, 50) time.sleep(5) drive_cm_new(-50, 50) start = time.time()
def ColorChecking(): if color.color == color.COLOR_YELLOW: #if yellow swing_and_safety() elif color.color == color.COLOR_GREEN: #if green big_design_and_build() elif color.color == color.COLOR_RED: #if red design_and_build_one() elif color.color == color.COLOR_BLUE: #if blue crane() def failsafe(): sys.exit() #This is where the movement happens. the function "ColorChecking" is a function to decide what to do based on color. #not whenever we touch enter (or the mibble button) then it will call ColorChecking(). Not just when this line happens Sound_.play_tone(frequency=400, duration=0.5, volume=50) start = time.time() btn.on_backspace = failsafe while True: #this code essentially color checks for 30 seconds if (btn.down): break pass btn.wait_for_released('right') ColorChecking() #hello # Sound_.play_tone(frequency=400, duration=0.5, volume=50)