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