mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
climber subsystem 3.0
This commit is contained in:
aarav18
@@ -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
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
* <p><p>
|
||||
* 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() {}
|
||||
}
|
||||
Reference in New Issue
Block a user