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_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)
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_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 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 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()
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()