コード例 #1
0
ファイル: drive.py プロジェクト: frc1418/2020-robot
class Drive:
    """
    Handle robot drivetrain.
    All drive interaction must go through this class.
    """

    limelight: Limelight
    train: wpilib.drive.DifferentialDrive
    navx: navx.AHRS
    left_motors: wpilib.SpeedControllerGroup
    right_motors: wpilib.SpeedControllerGroup

    y = will_reset_to(0.0)
    rot = will_reset_to(0.0)
    aligning = will_reset_to(False)
    deadband = will_reset_to(0.1)

    auto = will_reset_to(False)
    left_voltage = will_reset_to(0)
    right_voltage = will_reset_to(0)

    speed_constant = will_reset_to(1.05)
    rotational_constant = will_reset_to(0.8)
    squared_inputs = False

    angle_p = ntproperty('/align/kp', 0.04)
    angle_i = ntproperty('/align/ki', 0)
    angle_d = ntproperty('/align/kd', 0.0)
    angle_reported = ntproperty('/align/angle', 0)
    angle_to = ntproperty('/align/angle_to', 0)

    def setup(self):
        """
        Run setup code on the injected variables (train)
        """
        self.angle_controller = PIDController(self.angle_p, self.angle_i,
                                              self.angle_d)
        self.angle_controller.setTolerance(2, 1)
        self.angle_controller.enableContinuousInput(0, 360)
        self.angle_setpoint = None
        self.calculated_pid = False

    def set_target(self, angle: float, relative=True):
        if relative and angle is not None:
            self.angle_setpoint = (self.angle + angle) % 360
            self.angle_to = self.angle_setpoint
        else:
            self.angle_setpoint = angle

        if angle is not None:
            self.angle_controller.setSetpoint(self.angle_setpoint)
        else:
            self.calculated_pid = False
            self.angle_controller.reset()

    def align(self):
        self.aligning = True
        self.deadband = 0

    def voltageDrive(self, left_voltage, right_voltage):
        self.left_voltage = left_voltage
        self.right_voltage = right_voltage
        self.auto = True
        self.deadband = 0

    def move(self, y: float, rot: float):
        """
        Move robot.
        :param y: Speed of motion in the y direction. [-1..1]
        :param rot: Speed of rotation. [-1..1]
        """
        self.y = y
        self.rot = rot

    @property
    def target_locked(self):
        # self.logger.info(f'SET {self.angle_controller.getSetpoint()} ANG {self.navx.getAngle()}')
        return self.angle_controller.atSetpoint() and self.calculated_pid

    @property
    def angle(self):
        raw = self.navx.getAngle()
        while raw < 0:
            raw += 360
        return raw % 360

    def execute(self):
        """
        Handle driving.
        """
        self.train.setDeadband(self.deadband)
        self.angle_reported = self.angle

        if self.aligning and self.angle_setpoint is not None:
            self.right_motors.setInverted(False)
            if self.angle_controller.atSetpoint() and self.calculated_pid:
                self.train.arcadeDrive(0, 0, squareInputs=False)
                return

            # Use new network tables variables for testing
            self.angle_controller.setP(self.angle_p)
            self.angle_controller.setD(self.angle_d)
            self.angle_controller.setI(self.angle_i)

            output = self.angle_controller.calculate(self.angle)
            self.logger.info(
                f'Output: {output}, Error: {self.angle_controller.getPositionError()}'
            )
            # Manual I-term zone (15 degrees)
            if abs(self.angle_controller.getPositionError()) <= 3:
                self.angle_controller.setI(self.angle_i)
                # Minumum and Maximum effect of integrator on output
                self.angle_controller.setIntegratorRange(-0.05, 0.05)
            else:
                self.angle_controller.setI(0)
                self.angle_controller.setIntegratorRange(0, 0)

            if output < 0:
                output = max(output, -0.35)
            else:
                output = min(output, 0.35)

            self.train.arcadeDrive(0, output, squareInputs=False)
            self.calculated_pid = True
        elif self.auto:
            self.right_motors.setInverted(True)
            self.right_motors.setVoltage(self.right_voltage)
            self.left_motors.setVoltage(self.left_voltage)
        else:
            self.right_motors.setInverted(False)
            self.train.arcadeDrive(
                self.speed_constant * self.y,
                self.rotational_constant * self.rot,
                squareInputs=self.squared_inputs,
            )

        if not self.limelight.targetExists():
            self.limelight.target_state = 0
        elif self.target_locked:
            self.limelight.target_state = 2
        else:
            self.limelight.target_state = 1
コード例 #2
0
class SwerveModule:
    # Get the motors, encoder and config from injection
    driveMotor: ctre.WPI_VictorSPX
    rotateMotor: ctre.WPI_VictorSPX

    encoder: wpilib.AnalogInput

    cfg: ModuleConfig

    def setup(self):
        """
        Called after injection
        """
        # Config
        self.sd_prefix = self.cfg.sd_prefix or 'Module'
        self.encoder_zero = self.cfg.zero or 0
        self.inverted = self.cfg.inverted or False
        self.allow_reverse = self.cfg.allow_reverse or True

        # SmartDashboard
        self.sd = NetworkTables.getTable('SmartDashboard')
        self.debugging = self.sd.getEntry('drive/drive/debugging')

        # Motor
        self.driveMotor.setInverted(self.inverted)

        self._requested_voltage = 0
        self._requested_speed = 0

        # PID Controller
        # kP = 1.5, kI = 0.0, kD = 0.0
        self._pid_controller = PIDController(1.5, 0.0, 0.0)
        self._pid_controller.enableContinuousInput(
            0.0, 5.0)  # Will set the 0 and 5 as the same point
        self._pid_controller.setTolerance(
            0.05, 0.05)  # Tolerance where the PID will be accpeted aligned

    def get_voltage(self):
        """
        :returns: the voltage position after the zero
        """
        return self.encoder.getAverageVoltage() - self.encoder_zero

    def flush(self):
        """
        Flush the modules requested speed and voltage.
        Resets the PID controller.
        """
        self._requested_voltage = self.encoder_zero
        self._requested_speed = 0
        self._pid_controller.reset()

    @staticmethod
    def voltage_to_degrees(voltage):
        """
        Convert a given voltage value to degrees.

        :param voltage: a voltage value between 0 and 5
        :returns: the degree value between 0 and 359
        """
        deg = (voltage / 5) * 360

        if deg < 0:
            deg += 360

        return deg

    @staticmethod
    def voltage_to_rad(voltage):
        """
        Convert a given voltage value to rad.

        :param voltage: a voltage value between 0 and 5
        :returns: the radian value betwen 0 and 2pi
        """
        return (voltage / 5) * 2 * math.pi

    @staticmethod
    def degree_to_voltage(degree):
        """
        Convert a given degree to voltage.

        :param degree: a degree value between 0 and 360
        :returns" the voltage value between 0 and 5
        """
        return (degree / 360) * 5

    def _set_deg(self, value):
        """
        Round the value to within 360. Set the requested rotate position (requested voltage).
        Intended to be used only by the move function.
        """
        self._requested_voltage = (
            (self.degree_to_voltage(value) + self.encoder_zero) % 5)

    def move(self, speed, deg):
        """
        Set the requested speed and rotation of passed.

        :param speed: requested speed of wheel from -1 to 1
        :param deg: requested angle of wheel from 0 to 359 (Will wrap if over or under)
        """
        deg %= 360  # Prevent values past 360

        if self.allow_reverse:
            """
            If the difference between the requested degree and the current degree is
            more than 90 degrees, don't turn the wheel 180 degrees. Instead reverse the speed.
            """
            if abs(deg - self.voltage_to_degrees(self.get_voltage())) > 90:
                speed *= -1
                deg += 180
                deg %= 360

        self._requested_speed = speed
        self._set_deg(deg)

    def debug(self):
        """
        Print debugging information about the module to the log.
        """
        print(self.sd_prefix, '; requested_speed: ', self._requested_speed,
              ' requested_voltage: ', self._requested_voltage)

    def execute(self):
        """
        Use the PID controller to get closer to the requested position.
        Set the speed requested of the drive motor.

        Called every robot iteration/loop.
        """
        # Calculate the error using the current voltage and the requested voltage.
        # DO NOT use the #self.get_voltage function here. It has to be the raw voltage.
        error = self._pid_controller.calculate(self.encoder.getVoltage(),
                                               self._requested_voltage)

        # Set the output 0 as the default value
        output = 0
        # If the error is not tolerable, set the output to the error.
        # Else, the output will stay at zero.
        if not self._pid_controller.atSetpoint():
            # Use max-min to clamped the output between -1 and 1.
            output = max(min(error, 1), -1)

        # Put the output to the dashboard
        self.sd.putNumber('drive/%s/output' % self.sd_prefix, output)
        # Set the output as the rotateMotor's voltage
        self.rotateMotor.set(output)

        # Set the requested speed as the driveMotor's voltage
        self.driveMotor.set(self._requested_speed)

        self.update_smartdash()

    def update_smartdash(self):
        """
        Output a bunch on internal variables for debugging purposes.
        """
        self.sd.putNumber('drive/%s/degrees' % self.sd_prefix,
                          self.voltage_to_degrees(self.get_voltage()))

        if self.debugging.getBoolean(False):
            self.sd.putNumber('drive/%s/requested_voltage' % self.sd_prefix,
                              self._requested_voltage)
            self.sd.putNumber('drive/%s/requested_speed' % self.sd_prefix,
                              self._requested_speed)
            self.sd.putNumber('drive/%s/raw voltage' % self.sd_prefix,
                              self.encoder.getVoltage()
                              )  # DO NOT USE self.get_voltage() here
            self.sd.putNumber('drive/%s/average voltage' % self.sd_prefix,
                              self.encoder.getAverageVoltage())
            self.sd.putNumber('drive/%s/encoder_zero' % self.sd_prefix,
                              self.encoder_zero)

            self.sd.putNumber('drive/%s/PID Setpoint' % self.sd_prefix,
                              self._pid_controller.getSetpoint())
            self.sd.putNumber('drive/%s/PID Error' % self.sd_prefix,
                              self._pid_controller.getPositionError())
            self.sd.putBoolean('drive/%s/PID isAligned' % self.sd_prefix,
                               self._pid_controller.atSetpoint())

            self.sd.putBoolean('drive/%s/allow_reverse' % self.sd_prefix,
                               self.allow_reverse)