diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 7d19bf5..5964eae 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -54,7 +54,7 @@ public void teleopExecute() { if (zgain != 0) { new TurretMotorConstant(Math.pow(Math.abs(zgain), 0.8) * -0.1 * Math.signum(zgain)).schedule(); } - + // if (RobotMap.HumanInput.Operator.joystick.getAxis(3) < -0.95) { // new ClimberDown().schedule(); // } else if (RobotMap.HumanInput.Operator.joystick.getAxis(3) > 0.95) { diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index b0d55e0..8a00e1a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import org.usfirst.frc4904.robot.commands.shooter.SetDefaultShooterVelocity; import org.usfirst.frc4904.robot.commands.shooter.ShooterBrake; import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; import org.usfirst.frc4904.robot.subsystems.Indexer; @@ -76,6 +77,8 @@ public static class CANMotor { public static final int CLIMBER_MOTOR = 9; // TODO: set port public static final int SHOOTER_MOTOR = 8; // TODO: set port + + } public static class PWM { @@ -186,6 +189,8 @@ public static class Component { public static Motor climberMotor; public static Climber climber; public static Shooter shooter; + + public static VelocitySensorMotor shooterVSM; } public static class NetworkTables { @@ -242,10 +247,11 @@ public RobotMap() { Component.shooterTalon = new CANTalonFX(Port.CANMotor.SHOOTER_MOTOR); Component.shooterMotor = new Motor("ShooterMotor", true, Component.shooterTalon); + Component.shooterEncoder = new CANTalonEncoder(Component.shooterTalon); // Component.shooterEncoder = new CANTalonEncoder(Component.shooterTalon); // Component.shooterPID = new CustomPIDController(PID.Shooter.P, PID.Shooter.I, PID.Shooter.D, PID.Shooter.F, Component.shooterEncoder); - // VelocitySensorMotor shooterVSM = new VelocitySensorMotor("Turret", Component.shooterPID, Component.shooterTalon); //TODO: cringe (zach) - // Component.shooter = new Shooter(shooterVSM, Component.shooterEncoder); + Component.shooterVSM = new VelocitySensorMotor("Turret", Component.shooterPID, Component.shooterTalon); //TODO: cringe (zach) + Component.shooter = new Shooter(Component.shooterVSM, Component.shooterEncoder, new SetDefaultShooterVelocity(Component.shooterVSM)); // Component.shifter = new SolenoidShifters(Port.Pneumatics.SHIFTER.buildDoubleSolenoid()); diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/shooter/GetShooterVelocity.java b/src/main/java/org/usfirst/frc4904/robot/commands/shooter/GetShooterVelocity.java new file mode 100644 index 0000000..3e37132 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/commands/shooter/GetShooterVelocity.java @@ -0,0 +1,18 @@ +package org.usfirst.frc4904.robot.commands.shooter; + +import org.usfirst.frc4904.robot.RobotMap; +import org.usfirst.frc4904.standard.LogKitten; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class GetShooterVelocity extends CommandBase { + @Override + public void initialize() { + super.initialize(); + } + + public void getRate() { + double rate = RobotMap.Component.shooterEncoder.getRate(); + LogKitten.wtf(rate); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/shooter/SetDefaultShooterVelocity.java b/src/main/java/org/usfirst/frc4904/robot/commands/shooter/SetDefaultShooterVelocity.java new file mode 100644 index 0000000..f9812a1 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/commands/shooter/SetDefaultShooterVelocity.java @@ -0,0 +1,14 @@ +package org.usfirst.frc4904.robot.commands.shooter; + +import org.usfirst.frc4904.robot.Robot; +import org.usfirst.frc4904.robot.RobotMap; +import org.usfirst.frc4904.robot.RobotMap.Component; +import org.usfirst.frc4904.standard.commands.motor.MotorVelocitySet; +import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; + + +public class SetDefaultShooterVelocity extends MotorVelocitySet { + public SetDefaultShooterVelocity(VelocitySensorMotor motor) { + super(motor, 0.2); + } +} diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/shooter/ShooterSetSpeed.java b/src/main/java/org/usfirst/frc4904/robot/commands/shooter/ShooterSetSpeed.java index 107988d..d106082 100644 --- a/src/main/java/org/usfirst/frc4904/robot/commands/shooter/ShooterSetSpeed.java +++ b/src/main/java/org/usfirst/frc4904/robot/commands/shooter/ShooterSetSpeed.java @@ -14,6 +14,6 @@ public class ShooterSetSpeed extends MotorVelocitySet { */ public ShooterSetSpeed(double velocity) { - super(RobotMap.Component.shooter.shooterMotor, velocity); + super(RobotMap.Component.shooterVSM, velocity); } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 490d366..0e7a246 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -10,8 +10,10 @@ import org.usfirst.frc4904.robot.commands.indexerIntakeTurret.StoreBall; import org.usfirst.frc4904.robot.commands.intake.ExtendIntake; import org.usfirst.frc4904.robot.commands.intake.RetractIntake; +import org.usfirst.frc4904.robot.commands.shooter.GetShooterVelocity; import org.usfirst.frc4904.robot.commands.shooter.ShooterSetSpeed; import org.usfirst.frc4904.robot.commands.turret.TurnTurret; +import org.usfirst.frc4904.robot.subsystems.Shooter; import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.commands.RunFor; import org.usfirst.frc4904.standard.commands.chassis.ChassisShift; @@ -51,7 +53,14 @@ public void bindCommands() { RobotMap.HumanInput.Driver.xbox.a.whenPressed(new RunFor(new StoreBall(), 3.0)); RobotMap.HumanInput.Driver.xbox.rb.whenPressed(new ExtendIntake()); RobotMap.HumanInput.Driver.xbox.lb.whenPressed(new RetractIntake()); +<<<<<<< HEAD + // RobotMap.HumanInput.Driver.xbox.x.whenPressed(new RunFor(new BallReject(), 3.0)); + RobotMap.HumanInput.Driver.xbox.x.whenPressed(new ShooterSetSpeed(Shooter.SHOOT_VELOCITY)); +======= RobotMap.HumanInput.Driver.xbox.x.whenPressed(new RunFor(new BallReject(), 3.0)); + RobotMap.HumanInput.Driver.xbox.x.whenPressed(new ShooterSetSpeed()); +>>>>>>> 67fc2e7589a82473e0e196146f08b029f58e2334 + RobotMap.HumanInput.Driver.xbox.b.whenPressed(new GetShooterVelocity()); // RobotMap.HumanInput.Driver.xbox.x.whenPressed(new MotorBrake(RobotMap.Component.shooterTalon)); // RobotMap.HumanInput.Driver.xbox.dPad.up.whenPressed(new TurnTurret(Math.PI / 2, RobotMap.Component.turret)); @@ -85,4 +94,5 @@ public double getTurnSpeed() { return turnSpeed + precisionTurnSpeed + operatorControlTurnSpeed; // return turnSpeed; } -} \ No newline at end of file + +} diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/Shooter.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/Shooter.java index a1c39ae..aa51863 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/Shooter.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/Shooter.java @@ -5,12 +5,14 @@ package org.usfirst.frc4904.robot.subsystems; +import org.usfirst.frc4904.standard.commands.Noop; import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonFX; import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; import org.usfirst.frc4904.standard.subsystems.motor.Motor; import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Shooter extends SubsystemBase { @@ -18,11 +20,16 @@ public class Shooter extends SubsystemBase { public VelocitySensorMotor shooterMotor; public CANTalonEncoder shooterEncoder; - public Shooter(VelocitySensorMotor shooterMotor, CANTalonEncoder shooterEncoder) { + public Shooter(VelocitySensorMotor shooterMotor, CANTalonEncoder shooterEncoder, Command defaultCommand) { super(); + super.setDefaultCommand(defaultCommand); this.shooterMotor = shooterMotor; this.shooterEncoder = shooterEncoder; } + public Shooter(VelocitySensorMotor shooterMotor, CANTalonEncoder shooterEncoder) { + this(shooterMotor, shooterEncoder, new Noop()); + } + }