diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index da81db4..9b5f03c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -257,7 +257,7 @@ public class Robot extends TimedRobot { if (selectedOdo == null) { selectedOdo = m_robotContainer.getOdometry(); } - m_robotContainer.resetOdometry(selectedOdo); + // m_robotContainer.resetOdometry(selectedOdo); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 46ac339..57d4a4f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -113,7 +113,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + public final Climber m_robotClimber = new Climber(m_robotMap.elbow); public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); @@ -235,7 +235,7 @@ public class RobotContainer { }, m_robotHood)); m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(0.0, getOperatorController().getRightY() * 0.5) + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getRightY() * 0.5) , m_robotClimber)); // m_robotTurret.setDefaultCommand( @@ -293,8 +293,11 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); @@ -359,7 +362,6 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) @@ -368,7 +370,6 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1db64ed..19a8264 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -49,6 +49,7 @@ public class RobotMap { configureExtenderMotors(); configureSerializerMotors(); configureStorageMotors(); + configureClimberMotors(); } /* LED Subsystem */ @@ -56,29 +57,30 @@ public class RobotMap { // void configureLEDMotorControllers() {} - /* Swerve Subsystem */ - public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); - public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); - public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); - public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); - public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); - public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); - public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); +/* Swerve Subsystem */ +public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); +public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); +public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); +public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); +public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); +public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); +public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); +public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); +public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); +public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); +public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); +public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - public SwerveModule leftFront; - public SwerveModule leftBack; - public SwerveModule rightFront; - public SwerveModule rightBack; +public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); - void configureSwerveMotorControllers() { +public SwerveModule leftFront; +public SwerveModule leftBack; +public SwerveModule rightFront; +public SwerveModule rightBack; +void configureSwerveMotorControllers() { + leftFrontSteerMotor.configFactoryDefault(); leftFrontWheelMotor.configFactoryDefault(); rightFrontSteerMotor.configFactoryDefault(); @@ -87,58 +89,58 @@ public class RobotMap { leftBackWheelMotor.configFactoryDefault(); rightBackSteerMotor.configFactoryDefault(); rightBackWheelMotor.configFactoryDefault(); - + leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + + leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + NeutralMode mode = NeutralMode.Coast; leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); leftFrontWheelMotor.setNeutralMode(mode);// Coast @@ -148,7 +150,7 @@ public class RobotMap { leftBackWheelMotor.setNeutralMode(mode);// Coast rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); rightBackWheelMotor.setNeutralMode(mode);// Coast - + // current limits leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); @@ -160,7 +162,7 @@ public class RobotMap { rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); - + leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); @@ -170,55 +172,56 @@ public class RobotMap { rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); - + leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, - SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); + SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, - SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, - SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, - SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); - + SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + // config cancoder as remote encoder for swerve steer motors leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); } - /* Climb Subsystem */ - public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO - public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO +/* Climb Subsystem */ +public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO - /* Hooks Subsystem */ -// public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless); -// public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless); - public final Servo leftClaw = new Servo(1); // TODO: find actual channel - public final Servo rightClaw = new Servo(2); // TODO: find actual channel +private void configureClimberMotors() { + elbow.configFactoryDefault(); + elbow.setNeutralMode(NeutralMode.Brake); +} +/* Hooks Subsystem */ +public final Servo leftClaw = new Servo(1); +public final Servo rightClaw = new Servo(2); - // Shooter Config - /* Boom Boom Subsystem */ - public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); - public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); +// Shooter Config +/* Boom Boom Subsystem */ +public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); +public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // turret subsystem - public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); +// turret subsystem +public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); - // hood subsystem - public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); - - // Create motor CANSparkMax - void configureShooterMotorControllers() { +// hood subsystem +public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); +// Create motor CANSparkMax +void configureShooterMotorControllers() { + // LEFT FALCON shooterFalconLeft.configFactoryDefault(); shooterFalconLeft.setNeutralMode(NeutralMode.Coast); @@ -227,14 +230,14 @@ public class RobotMap { shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - + ShooterConstants.SHOOTER_TIMEOUT_MS); + // RIGHT FALCON shooterFalconRight.configFactoryDefault(); shooterFalconRight.setNeutralMode(NeutralMode.Coast); @@ -244,59 +247,59 @@ public class RobotMap { // m_shooterFalconRight.configPeakOutputForward(0, // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.follow(shooterFalconLeft); - + // turret shooterTurret.restoreFactoryDefaults(); shooterTurret.setIdleMode(IdleMode.kBrake); shooterTurret.setInverted(true); - + // hood subsystem angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); angleAdjusterMotor.setInverted(true); - } +} - /* Serializer Subsystem */ - public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); - public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); - - /* Intake Subsystem */ - public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); - public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); +/* Serializer Subsystem */ +public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); +public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); - void configureIntakeMotors() { +/* Intake Subsystem */ +public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); +public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + +void configureIntakeMotors() { intakeMotor.configFactoryDefault(); intakeMotor.setInverted(false); intakeMotor.setNeutralMode(NeutralMode.Coast); - + intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE); intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE); - } +} - void configureExtenderMotors() { - extenderMotor.restoreFactoryDefaults(); - extenderMotor.setInverted(true); - extenderMotor.setIdleMode(IdleMode.kBrake); +void configureExtenderMotors() { + extenderMotor.restoreFactoryDefaults(); + extenderMotor.setInverted(true); + extenderMotor.setIdleMode(IdleMode.kBrake); } - + void configureSerializerMotors() { serializerBelt.restoreFactoryDefaults(); - } + } /* Storage Subsystem */ public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - + void configureStorageMotors() { storageMotor.restoreFactoryDefaults(); } - + } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 60c69e7..d55f4e8 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -106,8 +106,8 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - + // targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + targetAngle = 0; error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; // if (error > 180) { // error = 360 - error; // TODO: error - 360 diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 666f9be..d16f830 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,429 +4,20 @@ package frc4388.robot.subsystems; -import java.util.Arrays; -import java.util.HashMap; - -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.WPI_Pigeon2; -import org.opencv.core.Point; - -import edu.wpi.first.wpilibj.motorcontrol.Talon; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; - public class Climber extends SubsystemBase { - private static double[][] shoulderFeedMap; - private static double[][] elbowFeedMap; + private WPI_TalonFX elbow; - public static void configureFeedMaps() { - - } + /** Creates a new Climber */ + public Climber(WPI_TalonFX elbow) { this.elbow = elbow; } + + public void setEncoders(double value) { this.elbow.setSelectedSensorPosition(value); } + public double getCurrent() { return this.elbow.getSupplyCurrent(); } + public void setMotors(double elbowOutput) { this.elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); } - private WPI_TalonFX m_shoulder; - private WPI_TalonFX m_elbow; - private WPI_Pigeon2 m_gyro; - - private Point tPoint; - - private double[] tJointAngles; - private double[] tJointSpeeds; - - private boolean groundRelative; - private double shoulderSpeedLimiter; - private double elbowSpeedLimiter; - - /** Creates a new ClimberRewrite. */ - public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) { - m_shoulder = shoulder; - m_shoulder.configFactoryDefault(); - m_shoulder.setNeutralMode(NeutralMode.Brake); - - m_elbow = elbow; - m_elbow.configFactoryDefault(); - m_elbow.setNeutralMode(NeutralMode.Brake); - - // setClimberPositionGains(); - setClimberVelocityGains(); - useVelocityGains(); - - tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; - - // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); - // elbowStartPosition = m_elbow.getSelectedSensorPosition(); - // m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); - // m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); - - m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); - m_elbow.configForwardSoftLimitEnable(true); - // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); - // m_elbow.configReverseSoftLimitEnable(true); - - m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); - m_shoulder.configForwardSoftLimitEnable(true); - m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); - m_shoulder.configReverseSoftLimitEnable(false); - - m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - m_elbow.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - m_shoulder.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - - m_shoulder.overrideLimitSwitchesEnable(true); - m_elbow.overrideLimitSwitchesEnable(true); - - tPoint = new Point(); - - if(groundRelative) - m_gyro = gyro; - - groundRelative = _groundRelative; - this.elbowSpeedLimiter = 1.0; - } - - // public void setClimberPositionGains() { - // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - // m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - - // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - // m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - // } - - // public void usePositionGains() { - // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - // } - - public void setClimberVelocityGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_shoulder.config_kF(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kP(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kI(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kD(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - m_elbow.config_kF(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kP(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kI(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kD(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - } - - public void useVelocityGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - } - - // public void setClimberFeedForward(double shoulderF, double elbowF) { - // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - // m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, shoulderF, ClimberConstants.CLIMBER_TIMEOUT_MS); - - // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - // m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, elbowF, ClimberConstants.CLIMBER_TIMEOUT_MS); - // } - - // public void compensateFeedForArmAngles() { - // double shoulderPosition = m_shoulder.getSelectedSensorPosition(); - // double elbowPosition = m_elbow.getSelectedSensorPosition(); - - // double shoulderFeed = 0; - // double elbowFeed = 0; - - // for(int i = 1; i < shoulderFeedMap.length; i++) { - // if(shoulderFeedMap[i][0] > shoulderPosition) { - // double percentDifference = (shoulderPosition - shoulderFeedMap[i-1][0]) / (shoulderFeedMap[i][0] - shoulderFeedMap[i-1][0]); - // double feedDifference = shoulderFeedMap[i][1] - shoulderFeedMap[i-1][1]; - // shoulderFeed = (percentDifference * feedDifference) + shoulderFeedMap[i-1][1]; - // } - // } - - // for(int i = 1; i < elbowFeedMap.length; i++) { - // if(elbowFeedMap[i][0] > elbowPosition) { - // double percentDifference = (elbowPosition - elbowFeedMap[i-1][0]) / (elbowFeedMap[i][0] - elbowFeedMap[i-1][0]); - // double feedDifference = elbowFeedMap[i][1] - elbowFeedMap[i-1][1]; - // elbowFeed = (percentDifference * feedDifference) + elbowFeedMap[i-1][1]; - // } - // } - - // setClimberFeedForward(shoulderFeed, elbowFeed); - // } - - public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER );//* this.shoulderSpeedLimiter); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER);// * this.elbowSpeedLimiter); - } - - public double[] getJointAngles() { - return new double[] { - (m_shoulder.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.SHOULDER_GB_RATIO, - (m_elbow.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.ELBOW_GB_RATIO - }; - } - - public void setJointAngles(double[] angles) { - // System.out.println(Arrays.toString(angles)); - setJointAngles(angles[0], angles[1]); - } - - public void setJointAngles(double shoulderAngle, double elbowAngle) { - // shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle; - // elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle; - - // shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle; - // elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle; - - // // Convert radians to ticks - // System.out.println("angles: " + shoulderAngle + ", " + elbowAngle); - - // double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; - // double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; - - // shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO; - // elbowPosition *= ClimberConstants.ELBOW_GB_RATIO; - - // // shoulderPosition += m_shoulderOffset; - // // elbowPosition += m_elbowOffset; - - // m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); - // m_elbow.set(TalonFXControlMode.Position, elbowAngle); - } - - public void setJointSpeeds(double[] speeds) { - setJointSpeeds(speeds[0], speeds[1]); - } - /** - * Velocity PID set for joints - * @param shoulderSpeed - * @param elbowSpeed - */ - public void setJointSpeeds(double shoulderSpeed, double elbowSpeed) { - m_shoulder.set(TalonFXControlMode.Velocity, shoulderSpeed); - m_elbow.set(TalonFXControlMode.Velocity, elbowSpeed); - } - - boolean movingPrev = false; - boolean moving; - /** - * - * @param xInput Rate of change of X position of target point - * @param yInput Rate of change of Y position of target point - * @deprecated use controlJointsWithInput() instead - */ - @Deprecated - public void controlWithInput(double xInput, double yInput) { - moving = xInput != 0 || yInput != 0; - - if(movingPrev != moving) { - if(moving) { - useVelocityGains(); - SmartDashboard.putString("Climber Gains", "Velocity"); - } else { - // usePositionGains(); - tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; - SmartDashboard.putString("Climber Gains", "Position"); - } - } - - if(moving) { - double[] jointSpeeds = new double[] {xInput * ClimberConstants.MOVE_SPEED, yInput * ClimberConstants.MOVE_SPEED}; - setJointSpeeds(jointSpeeds); - } else { - setJointAngles(tJointAngles); - } - - movingPrev = moving; - } - - public void controlJointsWithInput(double shoulderInput, double elbowInput) { - tJointAngles[0] += shoulderInput * ClimberConstants.MOVE_SPEED * .02; - tJointAngles[1] += elbowInput * ClimberConstants.MOVE_SPEED * .02; - } - - int pCount = 0; @Override - public void periodic() { - SmartDashboard.putBoolean("Moving", moving); - SmartDashboard.putBoolean("MovingPrev", movingPrev); - SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); - SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); - // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - // if(pCount % 1 == 0) - // setJointAngles(tJointAngles); - - // pCount ++; - - // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). - double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); - double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); - double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT); - - if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { - this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); - } - - if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { - this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); - } - - if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) { - this.elbowSpeedLimiter = 1.0; - } - - // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). - double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition(); - double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); - double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); - - if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { - this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); - } - - if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { - this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); - } - - if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) { - this.shoulderSpeedLimiter = 1.0; - } - } - - /** - * Gets joint angles for climber arm - * {@code targetPoint.x} and {@code targetPoint.y} are set in the xy plane of the climber, accounting for the - * rotation of the robot. Both parameters should be in cm. - * Does not set the motors automatically - *

- * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 - * - * @param targetPoint Target xy point for the climber arm to go to - * @param tiltAngle The tilt of the robot - * @return [shoulderAngle, elbowAngle] in radians - * @deprecated - * */ - @Deprecated - public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) { - double [] angles = new double[2]; - - double mag = Math.hypot(targetPoint.x, targetPoint.y); - if(mag >= ClimberConstants.MAX_ARM_LENGTH) { - targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH; - targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH; - mag = ClimberConstants.MAX_ARM_LENGTH; - } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) { - targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH; - targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH; - mag = ClimberConstants.MIN_ARM_LENGTH; - } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { - targetPoint.x = ClimberConstants.MIN_ARM_LENGTH; - targetPoint.y = 0.d; - mag = ClimberConstants.MIN_ARM_LENGTH; - } - - // The angle to the target point - double theta; - if(targetPoint.x == 0.d) { - theta = Math.PI/2.d; // TODO rename variable - } else { - theta = Math.atan(targetPoint.y / targetPoint.x); - } - theta += tiltAngle; - - if(targetPoint.x < 0.d) - theta += Math.PI; - - - // Correct target position for tilt - targetPoint.x = Math.cos(theta) * mag; - targetPoint.y = Math.sin(theta) * mag; - - // Law and Order: Cosines edition - double shoulderAngle; - if(mag == 0) { - shoulderAngle = 0; - } else { - shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / - (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag)); - } - shoulderAngle = theta - shoulderAngle; - - double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) / - (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH)); - //elbowAngle = Math.PI - elbowAngle; - // System.out.println("sa1: " + shoulderAngle); - // System.out.println("ea1: " + elbowAngle); - - // If the climber is resting on the robot base, rotate the upper arm in the direction of the target - if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { - shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; - double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH; - // System.out.println("xDiff: " + xDiff); - - elbowAngle = Math.atan(-targetPoint.y / xDiff); - // System.out.println("ea2: " + elbowAngle); - if(elbowAngle < 0) { - elbowAngle = Math.PI - Math.abs(elbowAngle); - } - - if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) - elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; - // System.out.println("ea3: " + elbowAngle); - - // Deal with negative wraparound - // if(xDiff >= 0.d) - // elbowAngle += Math.PI; - // System.out.println("ea4: " + elbowAngle); - } - - angles[0] = shoulderAngle; - angles[1] = elbowAngle; - return angles; - } - /** - * Forward kinematics for climber - * @param shoulderAngle in radians - * @param elbowAngle in radians - * @return Point in 2d of end effector - */ - public static Point getClimberPosition(double shoulderAngle, double elbowAngle) { - Point climberPoint = new Point(0, 0); - - climberPoint.x += Math.sin(shoulderAngle); - climberPoint.y += Math.cos(shoulderAngle); - - climberPoint.x -= Math.sin(elbowAngle - shoulderAngle); - climberPoint.y += Math.cos(elbowAngle - shoulderAngle); - - return climberPoint; - } - - public static Point getClimberPosition(double[] jointAngles) { - return getClimberPosition(jointAngles[0], jointAngles[1]); - } - - public void setClimberSoftLimits(boolean set){ - m_elbow.configForwardSoftLimitEnable(set); - m_shoulder.configForwardSoftLimitEnable(set); - } - - public void setEncoders(double value){ - m_elbow.setSelectedSensorPosition(value); - m_shoulder.setSelectedSensorPosition(value); - } - - public double getCurrent(){ - return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); - } - -} + public void periodic() {} +} \ No newline at end of file