def teleopPeriodic(self): """ Process inputs from the driver station here. This is run each iteration of the control loop before magicbot components are executed. """ if self.joystick.getRawButtonPressed(10): self.chassis.odometry_x = 0.0 self.chassis.odometry_y = 0.0 self.motion.set_waypoints( np.array([[0, 0, 0, 1], [1, 0, 0, 1], [1, 1, 0, 1], [2, 1, 0, 1], [2, 1, 0, 0]])) if self.joystick.getRawButtonPressed(9): self.motion.disable() self.chassis.field_oriented = True if self.joystick.getRawButtonPressed(8): print("Heading sp set") self.chassis.set_heading_sp(self.bno055.getAngle() + math.pi) # this is where the joystick inputs get converted to numbers that are sent # to the chassis component. we rescale them using the rescale_js function, # in order to make their response exponential, and to set a dead zone - # which just means if it is under a certain value a 0 will be sent # TODO: Tune these constants for whatever robot they are on vx = -rescale_js( self.joystick.getY(), deadzone=0.05, exponential=1.2, rate=4) vy = -rescale_js( self.joystick.getX(), deadzone=0.05, exponential=1.2, rate=4) vz = -rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=15.0, rate=self.spin_rate) self.chassis.set_inputs(vx, vy, vz)
def teleopPeriodic(self): """This is run each iteration of the control loop before magicbot components are executed.""" # this is where the joystick inputs get converted to numbers that are sent # to the chassis component. we rescale them using the rescale_js function, # in order to make their response exponential, and to set a dead zone - # which just means if it is under a certain value a 0 will be sent # TODO: Tune these constants forvwhatever robot they are on vx = -rescale_js( self.joystick.getY(), deadzone=0.05, exponential=1.2, rate=4) vy = -rescale_js( self.joystick.getX(), deadzone=0.05, exponential=1.2, rate=4) vz = -rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=15.0, rate=self.spin_rate) self.chassis.set_inputs(vx, vy, vz)
def testPeriodic(self): self.vision.execute() # Keep the time offset calcs running joystick_vx = -rescale_js( self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=0.5 ) for button, module in zip((5, 3, 4, 6), self.chassis.modules): if self.joystick.getRawButton(button): module.store_steer_offsets() module.steer_motor.set(ctre.ControlMode.PercentOutput, joystick_vx) if self.joystick.getTriggerPressed(): module.steer_motor.set( ctre.ControlMode.Position, module.steer_motor.getSelectedSensorPosition(0) + self.offset_rotation_rate, ) if self.joystick.getRawButtonPressed(2): module.steer_motor.set( ctre.ControlMode.Position, module.steer_motor.getSelectedSensorPosition(0) - self.offset_rotation_rate, ) if self.joystick.getRawButtonPressed(8): for module in self.chassis.modules: module.drive_motor.set(ctre.ControlMode.PercentOutput, 0.3) if self.joystick.getRawButtonPressed(12): for module in self.chassis.modules: module.steer_motor.set( ctre.ControlMode.Position, module.steer_enc_offset ) if self.gamepad.getStartButton(): self.climber.retract_all() self.climber.execute() if self.gamepad.getPOV() != -1: speed = 0.1 azimuth = math.radians(-self.gamepad.getPOV()) for module in self.chassis.modules: module.set_velocity( speed * math.cos(azimuth), speed * math.sin(azimuth), absolute_rotation=True, ) if self.gamepad.getTriggerAxis(self.gamepad.Hand.kLeft) > 0.5: self.hatch_enable_piston.set(wpilib.DoubleSolenoid.Value.kReverse)
def testPeriodic(self): self.vision.execute() # Keep the time offset calcs running joystick_vx = -rescale_js( self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=0.5) self.sd.putNumber("joy_vx", joystick_vx) for button, module in zip((5, 3, 4, 6), self.chassis.modules): if self.joystick.getRawButton(button): module.store_steer_offsets() module.steer_motor.set(ctre.ControlMode.PercentOutput, joystick_vx) if self.joystick.getTriggerPressed(): module.steer_motor.set( ctre.ControlMode.Position, module.steer_motor.getSelectedSensorPosition(0) + self.offset_rotation_rate, ) if self.joystick.getRawButtonPressed(2): module.steer_motor.set( ctre.ControlMode.Position, module.steer_motor.getSelectedSensorPosition(0) - self.offset_rotation_rate, ) if self.joystick.getRawButtonPressed(8): for module in self.chassis.modules: module.drive_motor.set(ctre.ControlMode.PercentOutput, 0.3) if self.joystick.getRawButtonPressed(12): for module in self.chassis.modules: module.steer_motor.set(ctre.ControlMode.Position, module.steer_enc_offset) if self.gamepad.getStartButtonPressed(): self.climber.retract_all() if self.gamepad.getBackButtonPressed(): self.climber.stop_all()
def teleopPeriodic(self): """ Process inputs from the driver station here. This is run each iteration of the control loop before magicbot components are executed. """ if self.gamepad.getBumperPressed( wpilib.interfaces.GenericHID.Hand.kLeft): self.intake.push(not self.intake.push_on) if self.gamepad.getBumperPressed( wpilib.interfaces.GenericHID.Hand.kRight): self.intake.toggle_arms() if self.gamepad.getBButtonPressed(): self.intake.toggle_clamp() if self.joystick.getTrigger(): self.cubeman.start_intake(force=True) if self.joystick.getRawButtonPressed(4) or self.gamepad.getTriggerAxis( wpilib.interfaces.GenericHID.Hand.kRight) > 0.5: self.cubeman.eject(force=True) # if self.joystick.getRawButtonPressed(2) or self.gamepad.getStartButtonPressed(): if self.gamepad.getStartButtonPressed(): self.cubeman.panic() if self.joystick.getRawButtonPressed(3) or self.gamepad.getTriggerAxis( wpilib.interfaces.GenericHID.Hand.kLeft) > 0.5: self.cubeman.deposit_exchange(force=True) if self.gamepad.getAButtonPressed(): self.cubeman.lift_to_scale(force=True) if self.gamepad.getXButtonPressed(): self.cubeman.lift_to_switch(force=True) if self.gamepad.getBackButtonPressed(): self.cubeman.reset(force=True) if self.joystick.getRawButtonPressed(10): self.imu.resetHeading() self.chassis.set_heading_sp(0) throttle = (1 - self.joystick.getThrottle()) / 2 # this is where the joystick inputs get converted to numbers that are sent # to the chassis component. we rescale them using the rescale_js function, # in order to make their response exponential, and to set a dead zone - # which just means if it is under a certain value a 0 will be sent # TODO: Tune these constants for whatever robot they are on joystick_vx = -rescale_js(self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=4 * throttle) joystick_vy = -rescale_js(self.joystick.getX(), deadzone=0.1, exponential=1.5, rate=4 * throttle) joystick_vz = -rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=20.0, rate=self.spin_rate) joystick_hat = self.joystick.getPOV() if joystick_vx or joystick_vy or joystick_vz: self.chassis.set_inputs( joystick_vx, joystick_vy, joystick_vz, field_oriented=not self.joystick.getRawButton(6)) elif self.gamepad.getStickButton(self.gamepad.Hand.kLeft): # TODO Tune these constants for the gamepad. gamepad_vx = -rescale_js(self.gamepad.getY( self.gamepad.Hand.kRight), deadzone=0.1, exponential=1.5, rate=0.5) gamepad_vy = -rescale_js(self.gamepad.getX( self.gamepad.Hand.kRight), deadzone=0.1, exponential=1.5, rate=0.5) self.chassis.set_inputs(gamepad_vx, gamepad_vy, 0, field_oriented=True) else: self.chassis.set_inputs(0, 0, 0) if joystick_hat != -1: constrained_angle = -constrain_angle(math.radians(joystick_hat)) self.chassis.set_heading_sp(constrained_angle)
def teleopPeriodic(self): """Allow the drivers to control the robot.""" # self.chassis.heading_hold_off() throttle = max(0.1, (1 - self.joystick.getThrottle()) / 2) # min 10% self.sd.putNumber("joy_throttle", throttle) # this is where the joystick inputs get converted to numbers that are sent # to the chassis component. we rescale them using the rescale_js function, # in order to make their response exponential, and to set a dead zone - # which just means if it is under a certain value a 0 will be sent # TODO: Tune these constants for whatever robot they are on joystick_vx = -rescale_js(self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=4 * throttle) joystick_vy = -rescale_js(self.joystick.getX(), deadzone=0.1, exponential=1.5, rate=4 * throttle) joystick_vz = -rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=20.0, rate=self.spin_rate) joystick_hat = self.joystick.getPOV() self.sd.putNumber("joy_vx", joystick_vx) self.sd.putNumber("joy_vy", joystick_vy) self.sd.putNumber("joy_vz", joystick_vz) # Allow big stick movements from the driver to break out of an automation if abs(joystick_vx) > 0.5 or abs(joystick_vy) > 0.5: self.hatch_intake.done() self.hatch_deposit.done() self.cargo_deposit.done() if not self.chassis.automation_running: if joystick_vx or joystick_vy or joystick_vz: self.chassis.set_inputs( joystick_vx, joystick_vy, joystick_vz, field_oriented=not self.joystick.getRawButton(6), ) else: self.chassis.set_inputs(0, 0, 0) if joystick_hat != -1: if self.intake.has_cargo: constrained_angle = -constrain_angle( math.radians(joystick_hat) + math.pi) else: constrained_angle = -constrain_angle( math.radians(joystick_hat)) self.chassis.set_heading_sp(constrained_angle) if self.joystick.getRawButtonPressed(4): self.hatch.punch() if self.joystick.getTrigger(): angle = FieldAngle.closest(self.imu.getAngle()) self.logger.info("closest field angle: %s", angle) if angle is FieldAngle.LOADING_STATION: self.hatch_intake.engage() else: self.hatch_deposit.engage() self.chassis.set_heading_sp(angle.value) if self.joystick.getRawButton(2): self.chassis.set_heading_sp(FieldAngle.LOADING_STATION.value) self.hatch_intake.engage() if self.joystick.getRawButtonPressed(5): self.hatch.clear_to_retract = True if self.joystick.getRawButtonPressed(3): if self.chassis.hold_heading: self.chassis.heading_hold_off() else: self.chassis.heading_hold_on() if self.gamepad.getStartButtonPressed(): self.climb_automation.start_climb_lv3() if self.gamepad.getBackButtonPressed(): self.climb_automation.done()
def teleopPeriodic(self): """Allow the drivers to control the robot.""" throttle = max(0.1, (1 - self.joystick.getThrottle()) / 2) # min 10% # this is where the joystick inputs get converted to numbers that are sent # to the chassis component. we rescale them using the rescale_js function, # in order to make their response exponential, and to set a dead zone - # which just means if it is under a certain value a 0 will be sent # TODO: Tune these constants for whatever robot they are on joystick_vx = -rescale_js( self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=4 * throttle ) joystick_vy = -rescale_js( self.joystick.getX(), deadzone=0.1, exponential=1.5, rate=4 * throttle ) joystick_vz = -rescale_js( self.joystick.getZ(), deadzone=0.2, exponential=20.0, rate=self.spin_rate ) joystick_hat = self.joystick.getPOV() # Allow big stick movements from the driver to break out of an automation if abs(joystick_vx) > 0.5 or abs(joystick_vy) > 0.5: self.hatch_intake.done() self.hatch_deposit.done() self.cargo_deposit.done() if not self.chassis.automation_running: if joystick_vx or joystick_vy or joystick_vz: self.chassis.set_inputs( joystick_vx, joystick_vy, joystick_vz, field_oriented=not self.joystick.getRawButton(6), ) else: self.chassis.set_inputs(0, 0, 0) if joystick_hat != -1: if self.cargo_component.has_cargo or self.cargo.is_executing: constrained_angle = -constrain_angle( math.radians(joystick_hat) + math.pi ) else: constrained_angle = -constrain_angle(math.radians(joystick_hat)) self.chassis.set_heading_sp(constrained_angle) # Starts Hatch Alignment and Cargo State Machines if ( self.joystick.getTrigger() or self.gamepad.getTriggerAxis(self.gamepad.Hand.kLeft) > 0.5 or self.gamepad.getTriggerAxis(self.gamepad.Hand.kRight) > 0.5 ): angle = FieldAngle.closest(self.imu.getAngle()) self.logger.info("closest field angle: %s", angle) if self.cargo_component.has_cargo: self.cargo_deposit.engage() else: if angle is FieldAngle.LOADING_STATION: self.hatch_intake.engage() else: self.hatch_deposit.engage() self.chassis.set_heading_sp(angle.value) # Hatch Manual Outake/Intake if self.joystick.getRawButtonPressed(5) or self.gamepad.getBumperPressed(6): angle = FieldAngle.closest(self.imu.getAngle()) self.logger.info("closest field angle: %s", angle) if angle is not FieldAngle.LOADING_STATION: self.hatch_automation.outake() else: self.hatch_automation.grab() if self.gamepad.getXButtonPressed(): self.hatch.retract_fingers() self.hatch.retract() # Stops Cargo Intake Motor if self.gamepad.getBButtonPressed(): self.cargo.outake_cargo_ship(force=True) # Toggles the Heading Hold if self.joystick.getRawButtonPressed(8): if self.chassis.hold_heading: self.chassis.heading_hold_off() else: self.chassis.heading_hold_on() # Resets the IMU's Heading if self.joystick.getRawButtonPressed(7): self.imu.resetHeading() self.chassis.set_heading_sp(0) # Start Button starts Climb State Machine if self.gamepad.getStartButtonPressed() and self.gamepad.getRawButtonPressed(5): self.climb_automation.start_climb_lv3() # Back Button Ends Climb State Machine if self.gamepad.getBackButtonPressed(): if self.gamepad.getRawButtonPressed(5): self.climb_automation.abort() else: self.climb_automation.done() # Cargo Floor Intake if self.gamepad.getAButtonPressed(): self.cargo.intake_floor(force=True) # Cargo Loading Station Intake if self.gamepad.getYButtonPressed(): self.cargo.intake_loading(force=True) self.chassis.set_heading_sp( FieldAngle.CARGO_FRONT.value ) # Reversed side of robot if self.gamepad.getPOV() != -1: speed = 0.65 azimuth = math.radians(-self.gamepad.getPOV()) if self.cargo_component.has_cargo: azimuth += math.pi self.chassis.set_inputs( speed * math.cos(azimuth), speed * math.sin(azimuth), 0, field_oriented=False, )
def teleopPeriodic(self): """ Process inputs from the driver station here. This is run each iteration of the control loop before magicbot components are executed. """ if self.joystick.getRawButtonPressed(12): self.intake.clamp_on = not self.intake.clamp_on if self.joystick.getRawButtonPressed(11): self.intake.push_on = not self.intake.push_on if self.joystick.getTrigger(): self.intake_automation.engage(initial_state="intake_cube") if self.joystick.getRawButtonPressed(4) or self.gamepad.getTriggerAxis( wpilib.interfaces.GenericHID.Hand.kRight) > 0.5: self.intake_automation.engage(initial_state="eject_cube") if self.joystick.getRawButtonPressed( 2) or self.gamepad.getStartButtonPressed(): self.intake_automation.engage(initial_state="stop", force=True) if self.joystick.getRawButtonPressed(3) or self.gamepad.getTriggerAxis( wpilib.interfaces.GenericHID.Hand.kLeft) > 0.5: self.intake_automation.engage(initial_state="exchange") if self.gamepad.getAButtonPressed(): self.lifter_automation.engage(initial_state="move_upper_scale", force=True) if self.gamepad.getBButtonPressed(): self.lifter_automation.engage(initial_state="move_balanced_scale", force=True) if self.gamepad.getYButtonPressed(): self.lifter_automation.engage(initial_state="move_lower_scale", force=True) if self.gamepad.getXButtonPressed(): self.lifter_automation.engage(initial_state="move_switch", force=True) if self.gamepad.getBackButtonPressed(): self.lifter_automation.engage(initial_state="reset", force=True) if self.joystick.getRawButtonPressed(10): self.imu.resetHeading() self.chassis.set_heading_sp_current() if self.joystick.getRawButtonPressed(8): print("Heading sp set") self.chassis.set_heading_sp(self.imu.getAngle() + math.pi) # this is where the joystick inputs get converted to numbers that are sent # to the chassis component. we rescale them using the rescale_js function, # in order to make their response exponential, and to set a dead zone - # which just means if it is under a certain value a 0 will be sent # TODO: Tune these constants for whatever robot they are on vx = -rescale_js( self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=4) vy = -rescale_js( self.joystick.getX(), deadzone=0.1, exponential=1.5, rate=4) vz = -rescale_js(self.joystick.getZ(), deadzone=0.4, exponential=10.0, rate=self.spin_rate) self.chassis.set_inputs(vx, vy, vz) self.loop_timer.measure()
def teleopPeriodic(self): """ Process inputs from the driver station here. This is run each iteration of the control loop before magicbot components are executed. """ if self.joystick.getRawButtonPressed(10): self.imu.resetHeading() self.chassis.set_heading_sp(0) throttle = (1 - self.joystick.getThrottle()) / 2 # this is where the joystick inputs get converted to numbers that are sent # to the chassis component. we rescale them using the rescale_js function, # in order to make their response exponential, and to set a dead zone - # which just means if it is under a certain value a 0 will be sent # TODO: Tune these constants for whatever robot they are on joystick_vx = -rescale_js(self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=4 * throttle) joystick_vy = -rescale_js(self.joystick.getX(), deadzone=0.1, exponential=1.5, rate=4 * throttle) joystick_vz = -rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=20.0, rate=self.spin_rate) joystick_hat = self.joystick.getPOV() if joystick_vx or joystick_vy or joystick_vz: self.chassis.set_inputs( joystick_vx, joystick_vy, joystick_vz, field_oriented=not self.joystick.getRawButton(6)) elif self.gamepad.getStickButton(self.gamepad.Hand.kLeft): # TODO Tune these constants for the gamepad. gamepad_vx = -rescale_js(self.gamepad.getY( self.gamepad.Hand.kRight), deadzone=0.1, exponential=1.5, rate=0.5) gamepad_vy = -rescale_js(self.gamepad.getX( self.gamepad.Hand.kRight), deadzone=0.1, exponential=1.5, rate=0.5) self.chassis.set_inputs(gamepad_vx, gamepad_vy, 0, field_oriented=True) else: self.chassis.set_inputs(0, 0, 0) if joystick_hat != -1: constrained_angle = -constrain_angle(math.radians(joystick_hat)) self.chassis.set_heading_sp(constrained_angle)
def teleopPeriodic(self): """ Process inputs from the driver station here. This is run each iteration of the control loop before magicbot components are executed. """ if self.joystick.getRawButtonPressed(3): self.intake_automation.engage(initial_state="intake_cube") if self.joystick.getRawButtonPressed(4): self.intake_automation.engage(initial_state='eject_cube') if self.joystick.getRawButtonPressed(5): self.intake_automation.engage(initial_state="stop", force=True) if self.joystick.getRawButtonPressed(6): self.intake_automation.engage(initial_state="deposit") if self.gamepad.getAButtonPressed(): self.lifter_automation.engage(initial_state="move_upper_scale", force=True) if self.gamepad.getBButtonPressed(): self.lifter_automation.engage(initial_state="move_balanced_scale", force=True) if self.gamepad.getXButtonPressed(): self.lifter_automation.engage(initial_state="move_lower_scale", force=True) if self.gamepad.getYButtonPressed(): self.lifter_automation.engage(initial_state="move_switch", force=True) if self.joystick.getRawButtonPressed(10): self.chassis.odometry_x = 0.0 self.chassis.odometry_y = 0.0 self.motion.set_waypoints( np.array([[0, 0, 0, 1], [1, 0, 0, 1], [1, 1, 0, 1], [2, 1, 0, 1], [2, 1, 0, 0]])) if self.joystick.getRawButtonPressed(9): self.motion.disable() self.chassis.field_oriented = True if self.joystick.getRawButtonPressed(8): print("Heading sp set") self.chassis.set_heading_sp(self.imu.getAngle() + math.pi) # this is where the joystick inputs get converted to numbers that are sent # to the chassis component. we rescale them using the rescale_js function, # in order to make their response exponential, and to set a dead zone - # which just means if it is under a certain value a 0 will be sent # TODO: Tune these constants for whatever robot they are on vx = -rescale_js( self.joystick.getY(), deadzone=0.05, exponential=1.2, rate=4) vy = -rescale_js( self.joystick.getX(), deadzone=0.05, exponential=1.2, rate=4) vz = -rescale_js(self.joystick.getZ(), deadzone=0.4, exponential=15.0, rate=self.spin_rate) self.chassis.set_inputs(vx, vy, vz)