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