Ejemplo n.º 1
0
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()
Ejemplo n.º 2
0
    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)
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
    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)
Ejemplo n.º 6
0
    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)
Ejemplo n.º 7
0
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()
Ejemplo n.º 8
0
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")
Ejemplo n.º 9
0
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()
Ejemplo n.º 10
0
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