class Shoot(Command): def __init__(self): super().__init__() self.timer = Timer() self.phase = 0 def on_start(self): self.phase = 0 RobotMap.shooter_component.set_active() RobotMap.shooter_component.disable_intake() RobotMap.shooter_component.engage_launcher() self.timer.reset() self.timer.start() def execute(self): if self.phase == 0: if self.timer.hasPeriodPassed(0.5): print("first phase") RobotMap.shooter_component.retract_launcher() RobotMap.shooter_component.enable_intake() self.phase = 1 elif self.phase == 1: if self.timer.hasPeriodPassed(1.0): print("second phase") RobotMap.shooter_component.disable_intake() self.finished() return def on_end(self): self.timer.stop() self.timer.reset() RobotMap.shooter_component.set_inactive()
def new_new_swerve(self): # if thr is None: throttle = -config.controller.getRawAxis(XboxAxis.R_Y) # else: # throttle = thr # if rot is None: rot = config.controller.getRawAxis(XboxAxis.L_X) # else: # self.target = rot # deadband zone if throttle < 0.25 and throttle > -0.25: throttle = 0 if rot < 0.25 and rot > -0.25: rot = 0 self.enc_ += rot self.target += self.seek_to(self.enc_) if self.target > self.max: self.target = self.max elif self.target < self.min: self.target = self.min while_timer = Timer() for_timer = Timer() current = [ self.seek_to(config.encoders[enc].get()) for enc in range(4) ] motor_values = self.setMotorValues(current[0]) while_timer.start() for_timer.start() while self.notInPosition( current, motor_values) and while_timer.hasPeriodPassed(.2): for enc in range(4): config.steering_motors[enc].set(0.2 * motor_values[enc]) if for_timer.hasPeriodPassed(.03): for enc in range(4): config.steering_motors[enc].set(0) for_timer.reset() current = [ self.seek_to(config.encoders[enc].get()) for enc in range(4) ] for_timer.stop() while_timer.stop() for mtr in config.driving_motors: mtr.set(throttle)
def new_new_swerve(self): # if thr is None: throttle = -config.controller.getRawAxis(XboxAxis.R_Y) # else: # throttle = thr # if rot is None: rot = config.controller.getRawAxis(XboxAxis.L_X) # else: # self.target = rot # deadband zone if throttle < 0.25 and throttle > -0.25: throttle = 0 if rot < 0.25 and rot > -0.25: rot = 0 self.enc_ += rot self.target += self.seek_to(self.enc_) if self.target > self.max: self.target = self.max elif self.target < self.min: self.target = self.min while_timer = Timer() for_timer = Timer() current = [self.seek_to(config.encoders[enc].get()) for enc in range(4)] motor_values = self.setMotorValues(current[0]) while_timer.start() for_timer.start() while self.notInPosition(current, motor_values) and while_timer.hasPeriodPassed(.2): for enc in range(4): config.steering_motors[enc].set(0.2 * motor_values[enc]) if for_timer.hasPeriodPassed(.03): for enc in range(4): config.steering_motors[enc].set(0) for_timer.reset() current = [self.seek_to(config.encoders[enc].get()) for enc in range(4)] for_timer.stop() while_timer.stop() for mtr in config.driving_motors: mtr.set(throttle)
class Shoot(Command): def __init__(self): super.__init__() self.timer = Timer() def on_start(self): RobotMap.shooter_component.disable_intake() RobotMap.shooter_component.engage_launcher() self.timer.start() def execute(self): if self.timer.hasPeriodPassed(1.5): RobotMap.shooter_component.enable_intake() self.finished() elif self.timer.hasPeriodPassed(1.0): RobotMap.shooter_component.retract_launcher() def on_end(self): self.timer.stop() self.timer.reset()
def new_swerve(self): throttle = -config.controller.getRawAxis(XboxAxis.R_Y) rot = config.controller.getRawAxis(XboxAxis.L_X) # deadband zone if throttle < 0.25 and throttle > -0.25: throttle = 0 if rot < 0.25 and rot > -0.25: rot = 0 logging.write_log([self.target, rot]) self.target += rot * 10 inverse = False stop = False t = Timer() t.start() for m in range(len(config.steering_motors)): t.reset() logging.write_log("Turning") while (not config.encoders[m].get() < self.target + 15 or \ not config.encoders[m].get() > self.target - 15) and \ not stop: if not inverse: config.steering_motors[m].set( (1 * -copysign(1, rot)) * (self.target - config.encoders[m].get())) else: config.steering_motors[m].set( (1 * copysign(1, rot)) * (config.encoders[m].get() - self.target)) logging.write_log(config.encoders[m].get()) if t.hasPeriodPassed(1.5): inverse = True stop = True break config.steering_motors[m].set(0) t.stop() for i in range(len(config.driving_motors)): if (i % 2): config.driving_motors[i].set(throttle) else: config.driving_motors[i].set(-throttle)
class SuckFast(Command): def __init__(self): super().__init__() self.timer = Timer() def on_start(self): self.timer.start() def execute(self): RobotMap.gripper_component.set_motor_speeds(1, 1) if self.timer.hasPeriodPassed(0.4): RobotMap.gripper_component.set_motor_speeds(0, 0) self.finished() def on_end(self): pass
def new_swerve(self): throttle = -config.controller.getRawAxis(XboxAxis.R_Y) rot = config.controller.getRawAxis(XboxAxis.L_X) # deadband zone if throttle < 0.25 and throttle > -0.25: throttle = 0 if rot < 0.25 and rot > -0.25: rot = 0 logging.write_log([self.target, rot]) self.target += rot * 10 inverse = False stop = False t = Timer() t.start() for m in range(len(config.steering_motors)): t.reset() logging.write_log("Turning") while (not config.encoders[m].get() < self.target + 15 or \ not config.encoders[m].get() > self.target - 15) and \ not stop: if not inverse: config.steering_motors[m].set((1 * -copysign(1, rot)) * (self.target - config.encoders[m].get())) else: config.steering_motors[m].set((1 * copysign(1, rot)) * (config.encoders[m].get() - self.target)) logging.write_log(config.encoders[m].get()) if t.hasPeriodPassed(1.5): inverse = True stop = True break config.steering_motors[m].set(0) t.stop() for i in range(len(config.driving_motors)): if (i % 2): config.driving_motors[i].set(throttle) else: config.driving_motors[i].set(-throttle)
class DriveByDistance(Command): def __init__(self, inches: float, speed: float, timeout: float = 0): super().__init__() angular_gains = (0.02, 0.0001, 0.02, 0.0) linear_gains = (0.02, 0.0, 0.0, 0.0) self._target_distance = inches self._speed = speed self._angular = 0 self.angular_controller = PIDController( *angular_gains, RobotMap.driver_component.driver_gyro, output=self) self.angular_controller.setInputRange(-360, 360) self.angular_controller.setOutputRange(-1, 1) self.angular_controller.setAbsoluteTolerance(0.5) self.angular_controller.setSetpoint(0) self._timeout = timeout self.timer = Timer() def pidWrite(self, output): self._angular = output def on_start(self): self.timer.start() RobotMap.driver_component.reset_drive_sensors() self.angular_controller.enable() def execute(self): if abs(RobotMap.driver_component.current_distance) >= abs( self._target_distance) or (self._timeout != 0 and self.timer.hasPeriodPassed( self._timeout)): RobotMap.driver_component.drive_train.curvatureDrive(0, 0, False) RobotMap.driver_component.neutralMotors() self.finished() return RobotMap.driver_component.drive_train.curvatureDrive( self._speed, self._angular, False) def on_end(self): self.timer.stop() self.angular_controller.disable()
class SpitFast(Command): def __init__(self, speed=1): super().__init__() self.timer = Timer() self._speed = -speed def on_start(self): self.timer.start() print("started spit") def execute(self): RobotMap.gripper_component.set_motor_speeds(self._speed, self._speed) if self.timer.hasPeriodPassed(1): RobotMap.gripper_component.set_motor_speeds(0, 0) self.finished() def on_end(self): self.timer.stop() self.timer.reset() print("ended spit")
class DriveByTime(Command): def __init__(self, seconds: float, speed: float): super().__init__() self._target_seconds = seconds self._speed = speed self.timer = Timer() def on_start(self): print("start driving forward by time") self.timer.start() def execute(self): RobotMap.driver_component.set_curve( self._speed, -RobotMap.driver_component.driver_gyro.getAngle() * 0.2) if self.timer.hasPeriodPassed(self._target_seconds): RobotMap.driver_component.set_curve(0, 0) self.finished() def on_end(self): print("end driving forward by time") self.timer.stop() self.timer.reset()
class RespondToController(Command): """ This command will respond to the controller's buttons. """ def __init__(self): super().__init__("Respond to Controller") self.sd = NetworkTables.getTable("SmartDashboard") self.logger = logging.getLogger("robot") oi.start_btn.whenPressed(SwitchCamera()) self.timer = Timer() self.right_bumper_last = False self.left_bumper_last = False def initialize(self): self.timer.start() def execute(self): if self.timer.hasPeriodPassed(0.05): # for xbox controller # piston control if oi.controller.getAButton(): # open ControlGearMech(False).start() elif oi.controller.getBButton(): # close ControlGearMech(True).start() # reversing control # if oi.controller.getStartButton(): # reverse camera, motors, and sonar # if self.sd.containsKey("camera/dev"): # if self.sd.getNumber("camera/dev") == 1: # self.sd.putNumber("camera/dev", 2) # else: # self.sd.putNumber("camera/dev", 1) # else: # self.sd.putNumber("camera/dev", 1) # cur_direct = self.sd.getNumber("direction") # if cur_direct == 1: # subsystems.motors.reverseDirection() # else: # subsystems.motors.forwardDirection() # self.sd.putNumber("direction", -cur_direct) # slow mode if oi.controller.getBumper(GenericHID.Hand.kRight): if self.right_bumper_last: pass elif oi.divider != 1: self.sd.putBoolean("slowmode", False) else: self.sd.putBoolean("slowmode", True) self.right_bumper_last = True else: self.right_bumper_last = False # lock on if oi.controller.getBumper(GenericHID.Hand.kLeft): if self.left_bumper_last: pass elif not self.sd.getBoolean("lockonRunning"): LockOn().start() else: self.logger.critical("Returning control to the controller!") self.sd.putBoolean("lockonRunning", False) RumbleController(0.5).start() FollowJoystick().start() self.left_bumper_last = True else: self.left_bumper_last = False