Skip to content

Commit edbe07b

Browse files
CodeCreator3CodeCreator3
committed
testing changes
1 parent 37473d4 commit edbe07b

9 files changed

Lines changed: 115 additions & 97 deletions

File tree

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -140,8 +140,8 @@ public class IntakeConstants {
140140
public static final String ROOT_TABLE = "Intake";
141141

142142
public class FrontRollers {
143-
public static final double GEAR_RATIO = 1; // TODO: determine (mech / motor)
144-
public static final boolean INVERT = false; // TODO: determine
143+
public static final double GEAR_RATIO = 1;
144+
public static final boolean INVERT = true;
145145
public static final double MOI = 0.0001;
146146

147147
public static final SparkFlexConfig MOTOR_CONFIG() {
@@ -176,11 +176,11 @@ public static final SparkFlexConfig MOTOR_CONFIG() {
176176
public class Extender {
177177
public static final boolean INVERT = false; // TODO: determine
178178
public static final Distance HOME_POSITION = Units.Meter.of(0); // TODO: determine
179-
public static final double GEAR_RATIO = 1; // TODO: determine (critical gear / motor)
180-
public static final Distance PULLEY_RADIUS = Units.Inch.of(2); // TODO: determine
179+
public static final double GEAR_RATIO = 1 / 50.0; // TODO: determine (critical gear / motor)
180+
public static final Distance PULLEY_RADIUS = Units.Inch.of(.827); // TODO: determine
181181
public static final double INCHES_TO_MOTOR_ROT =
182182
1.0 / (PULLEY_RADIUS.in(Units.Inches) * 2 * Math.PI * GEAR_RATIO);
183-
public static final Distance MAX_LENGTH = Units.Inches.of(30); // TODO: determine
183+
public static final Distance MAX_LENGTH = Units.Inches.of(10); // TODO: determine
184184

185185
public static final SparkMaxConfig MOTOR_CONFIG() {
186186
SparkMaxConfig config = new SparkMaxConfig();
@@ -199,9 +199,9 @@ public static final SparkMaxConfig MOTOR_CONFIG() {
199199
public class ShooterConstants {
200200
public static final String ROOT_TABLE = "Shooter";
201201
public static final double GEAR_RATIO = 1; // TODO: determine (mech / motor)
202-
public static final boolean INVERT = false; // TODO: determine
202+
public static final boolean INVERT = true; // TODO: determine
203203
public static final double MOI = 0.0001;
204-
public static final Distance HORIZONTAL_OFFSET = Units.Inches.of(5); // TODO: determine
204+
public static final Distance HORIZONTAL_OFFSET = Units.Inches.of(10); // TODO: determine
205205
public static final boolean FOLLOW_INVERT = false; // TODO: determine
206206

207207
public static final SparkFlexConfig MOTOR_CONFIG() {

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 24 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,9 @@
2222
import edu.wpi.first.wpilibj2.command.Command;
2323
import edu.wpi.first.wpilibj2.command.Commands;
2424
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
25+
import frc.lib.team2930.TunableNumberGroup;
2526
import frc.lib.team2930.commands.RunsWhenDisabledInstantCommand;
27+
import frc.lib.team6328.LoggedTunableNumber;
2628
import frc.robot.autonomous.AutoManager;
2729
import frc.robot.stateMachines.SuperStateMachine;
2830
import frc.robot.stateMachines.SuperStateMachine.SuperState;
@@ -36,9 +38,11 @@
3638
import frc.robot.subsystems.elevator.ElevatorIOSim;
3739
import frc.robot.subsystems.hopper.Hopper;
3840
import frc.robot.subsystems.hopper.HopperIO;
41+
import frc.robot.subsystems.hopper.HopperIOReal;
3942
import frc.robot.subsystems.hopper.HopperIOSim;
4043
import frc.robot.subsystems.intake.Intake;
4144
import frc.robot.subsystems.intake.IntakeIO;
45+
import frc.robot.subsystems.intake.IntakeIOReal;
4246
import frc.robot.subsystems.intake.IntakeIOSim;
4347
import frc.robot.subsystems.shooter.*;
4448
import frc.robot.subsystems.usb_vision.*;
@@ -60,6 +64,9 @@ public class RobotContainer {
6064
private final Shooter shooter;
6165
private final USBVision vision;
6266
// private final Vision vision;
67+
private final TunableNumberGroup tempTunables = new TunableNumberGroup("TempTunables");
68+
private final LoggedTunableNumber tempTunableNumber =
69+
tempTunables.build("TempTunableNumber", 300);
6370

6471
private final SuperStateMachine superStateMachine;
6572

@@ -76,9 +83,9 @@ public RobotContainer() {
7683
case REAL: // Real robot, instantiate hardware IO implementations
7784
drive = new Drive(new DriveModules(true), new GyroIOPigeon2(), controller);
7885
elevator = new Elevator(new ElevatorIO() {});
79-
intake = new Intake(new IntakeIO() {});
80-
shooter = new Shooter(new ShooterIO() {});
81-
hopper = new Hopper(new HopperIO() {});
86+
intake = new Intake(new IntakeIOReal() {});
87+
shooter = new Shooter(new ShooterIOReal() {});
88+
hopper = new Hopper(new HopperIOReal() {});
8289
vision = new USBVision(new USBVisionIOReal() {});
8390
// vision =
8491
// new Vision(
@@ -153,6 +160,19 @@ private void configureButtonBindings() {
153160
.onTrue(SuperStateMachine.setStateCommand(superStateMachine, SuperState.ClimbPrep))
154161
.onFalse(SuperStateMachine.setStateCommand(superStateMachine, SuperState.Climb));
155162

163+
controller
164+
.a()
165+
.onTrue(
166+
Commands.run(
167+
() -> {
168+
intake.setExtenderPercentOut(tempTunableNumber.get());
169+
}))
170+
.onFalse(
171+
Commands.runOnce(
172+
() -> {
173+
intake.setExtenderPercentOut(0);
174+
}));
175+
156176
SmartDashboard.putData(
157177
"Brake Mode",
158178
new RunsWhenDisabledInstantCommand(
@@ -197,7 +217,7 @@ public Command getAutonomousCommand() {
197217
}
198218

199219
public void robotPeriodic() {
200-
superStateMachine.periodic();
220+
// superStateMachine.periodic();
201221
}
202222

203223
public void onDisabled() {

‎src/main/java/frc/robot/subsystems/SuperStructure.java‎

Lines changed: 64 additions & 63 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,6 @@
1616
import frc.lib.team6328.LoggedTunableNumber;
1717
import frc.robot.Constants.ElevatorConstants;
1818
import frc.robot.Constants.FieldConstants;
19-
import frc.robot.Constants.IntakeConstants;
2019
import frc.robot.Constants.ShooterConstants;
2120
import frc.robot.Constants.SuperStructureConstants;
2221
import frc.robot.subsystems.elevator.Elevator;
@@ -98,68 +97,70 @@ public SuperStructure(
9897
@Override
9998
public void periodic() {
10099
switch (state) {
101-
case Idle:
102-
elevator.setHeight(Units.Inches.of(stowElevatorHeightInches.get()));
103-
elevator.setServoPositions(elevatorServoUnactuatedPos.get());
104-
intake.setFrontVel(Units.RPM.of(0));
105-
intake.setBackVel(Units.RPM.of(0));
106-
intake.setExtenderPos(IntakeConstants.Extender.MAX_LENGTH);
107-
hopper.setVel(Units.RPM.of(0));
108-
shooter.setVel(Units.RPM.of(0));
109-
break;
110-
case Intake:
111-
elevator.setHeight(Units.Inches.of(stowElevatorHeightInches.get()));
112-
elevator.setServoPositions(elevatorServoUnactuatedPos.get());
113-
intake.setFrontVel(Units.RPM.of(intakeFrontVelRPM.get()));
114-
intake.setBackVel(Units.RPM.of(intakeBackVelRPM.get()));
115-
intake.setExtenderPos(IntakeConstants.Extender.MAX_LENGTH);
116-
hopper.setVel(Units.RPM.of(0));
117-
shooter.setVel(Units.RPM.of(0));
118-
break;
119-
case ScorePrep:
120-
elevator.setHeight(Units.Inches.of(stowElevatorHeightInches.get()));
121-
elevator.setServoPositions(elevatorServoUnactuatedPos.get());
122-
intake.setFrontVel(Units.RPM.of(0));
123-
intake.setBackVel(Units.RPM.of(0));
124-
intake.setExtenderPos(IntakeConstants.Extender.MAX_LENGTH);
125-
hopper.setVel(Units.RPM.of(0));
126-
shooter.setVel(
127-
Units.RPM.of(
128-
shooterVelRPM.get())); // TODO: replace with interpolating tree after filling tree
129-
break;
130-
case Score:
131-
elevator.setHeight(Units.Inches.of(stowElevatorHeightInches.get()));
132-
elevator.setServoPositions(elevatorServoUnactuatedPos.get());
133-
intake.setFrontVel(Units.RPM.of(0));
134-
intake.setBackVel(Units.RPM.of(0));
135-
intake.setExtenderPos(IntakeConstants.Extender.MAX_LENGTH);
136-
hopper.setVel(Units.RPM.of(hopperVelRPM.get()));
137-
shooter.setVel(
138-
Units.RPM.of(
139-
shooterVelRPM.get())); // TODO: replace with interpolating tree after filling tree
140-
break;
141-
case ClimbPrep:
142-
elevator.setHeight(Units.Inches.of(climbPrepElevatorHeightInches.get()));
143-
intake.setFrontVel(Units.RPM.of(0));
144-
intake.setBackVel(Units.RPM.of(0));
145-
intake.setExtenderPos(IntakeConstants.Extender.HOME_POSITION);
146-
if (intake.isExtenderAtTarget()) {
147-
elevator.setServoPositions(elevatorServoActuatedPos.get());
148-
} else {
149-
elevator.setServoPositions(elevatorServoUnactuatedPos.get());
150-
}
151-
hopper.setVel(Units.RPM.of(0));
152-
shooter.setVel(Units.RPM.of(0));
153-
break;
154-
case Climb:
155-
elevator.setHeight(Units.Inches.of(climbElevatorHeightInches.get()));
156-
elevator.setServoPositions(elevatorServoActuatedPos.get());
157-
intake.setFrontVel(Units.RPM.of(0));
158-
intake.setBackVel(Units.RPM.of(0));
159-
intake.setExtenderPos(IntakeConstants.Extender.HOME_POSITION);
160-
hopper.setVel(Units.RPM.of(0));
161-
shooter.setVel(Units.RPM.of(0));
162-
break;
100+
// case Idle:
101+
// elevator.setHeight(Units.Inches.of(stowElevatorHeightInches.get()));
102+
// elevator.setServoPositions(elevatorServoUnactuatedPos.get());
103+
// intake.setFrontVel(Units.RPM.of(0));
104+
// intake.setBackVel(Units.RPM.of(0));
105+
// intake.setExtenderPos(IntakeConstants.Extender.MAX_LENGTH);
106+
// hopper.setVel(Units.RPM.of(0));
107+
// shooter.setVel(Units.RPM.of(0));
108+
// break;
109+
// case Intake:
110+
// elevator.setHeight(Units.Inches.of(stowElevatorHeightInches.get()));
111+
// elevator.setServoPositions(elevatorServoUnactuatedPos.get());
112+
// intake.setFrontVel(Units.RPM.of(intakeFrontVelRPM.get()));
113+
// intake.setBackVel(Units.RPM.of(intakeBackVelRPM.get()));
114+
// intake.setExtenderPos(IntakeConstants.Extender.MAX_LENGTH);
115+
// hopper.setVel(Units.RPM.of(0));
116+
// shooter.setVel(Units.RPM.of(0));
117+
// break;
118+
// case ScorePrep:
119+
// elevator.setHeight(Units.Inches.of(stowElevatorHeightInches.get()));
120+
// elevator.setServoPositions(elevatorServoUnactuatedPos.get());
121+
// intake.setFrontVel(Units.RPM.of(0));
122+
// intake.setBackVel(Units.RPM.of(0));
123+
// intake.setExtenderPos(IntakeConstants.Extender.MAX_LENGTH);
124+
// hopper.setVel(Units.RPM.of(0));
125+
// shooter.setVel(
126+
// Units.RPM.of(
127+
// shooterVelRPM.get())); // TODO: replace with interpolating tree after filling
128+
// tree
129+
// break;
130+
// case Score:
131+
// elevator.setHeight(Units.Inches.of(stowElevatorHeightInches.get()));
132+
// elevator.setServoPositions(elevatorServoUnactuatedPos.get());
133+
// intake.setFrontVel(Units.RPM.of(0));
134+
// intake.setBackVel(Units.RPM.of(0));
135+
// intake.setExtenderPos(IntakeConstants.Extender.MAX_LENGTH);
136+
// hopper.setVel(Units.RPM.of(hopperVelRPM.get()));
137+
// shooter.setVel(
138+
// Units.RPM.of(
139+
// shooterVelRPM.get())); // TODO: replace with interpolating tree after filling
140+
// tree
141+
// break;
142+
// case ClimbPrep:
143+
// elevator.setHeight(Units.Inches.of(climbPrepElevatorHeightInches.get()));
144+
// intake.setFrontVel(Units.RPM.of(0));
145+
// intake.setBackVel(Units.RPM.of(0));
146+
// intake.setExtenderPos(IntakeConstants.Extender.HOME_POSITION);
147+
// if (intake.isExtenderAtTarget()) {
148+
// elevator.setServoPositions(elevatorServoActuatedPos.get());
149+
// } else {
150+
// elevator.setServoPositions(elevatorServoUnactuatedPos.get());
151+
// }
152+
// hopper.setVel(Units.RPM.of(0));
153+
// shooter.setVel(Units.RPM.of(0));
154+
// break;
155+
// case Climb:
156+
// elevator.setHeight(Units.Inches.of(climbElevatorHeightInches.get()));
157+
// elevator.setServoPositions(elevatorServoActuatedPos.get());
158+
// intake.setFrontVel(Units.RPM.of(0));
159+
// intake.setBackVel(Units.RPM.of(0));
160+
// intake.setExtenderPos(IntakeConstants.Extender.HOME_POSITION);
161+
// hopper.setVel(Units.RPM.of(0));
162+
// shooter.setVel(Units.RPM.of(0));
163+
// break;
163164
default:
164165
break;
165166
}

‎src/main/java/frc/robot/subsystems/hopper/Hopper.java‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,6 @@
44

55
package frc.robot.subsystems.hopper;
66

7-
import com.revrobotics.spark.SparkBase.PersistMode;
8-
import com.revrobotics.spark.SparkBase.ResetMode;
97
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
108
import edu.wpi.first.units.Units;
119
import edu.wpi.first.units.measure.AngularVelocity;
@@ -19,8 +17,6 @@
1917
import frc.robot.Constants;
2018
import frc.robot.Constants.HopperConstants;
2119
import frc.robot.Constants.Mode;
22-
import frc.robot.Constants.ShooterConstants;
23-
2420
import org.littletonrobotics.junction.Logger;
2521

2622
public class Hopper extends SubsystemBase {
@@ -55,10 +51,10 @@ public class Hopper extends SubsystemBase {
5551
maxAccelerationConfig.initDefault(0.0);
5652

5753
} else if (Constants.currentMode == Mode.REAL) {
58-
kP.initDefault(0.0);
54+
kP.initDefault(0.0002);
5955
kV.initDefault(0.0);
6056

61-
maxAccelerationConfig.initDefault(0.0);
57+
maxAccelerationConfig.initDefault(1000);
6258
}
6359
}
6460

‎src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java‎

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -21,15 +21,15 @@ public class HopperIOReal implements HopperIO {
2121
new SparkFlex(Constants.CanIDs.HOPPER_LEAD_CAN_ID, MotorType.kBrushless);
2222
private final SparkFlex followMotor =
2323
new SparkFlex(Constants.CanIDs.HOPPER_FOLLOW_CAN_ID, MotorType.kBrushless);
24-
private SparkFlexConfig config = Constants.ElevatorConstants.MOTOR_CONFIG();
24+
private SparkFlexConfig config = Constants.HopperConstants.MOTOR_CONFIG();
2525

2626
private final RelativeEncoder encoder = leadMotor.getEncoder();
2727

2828
private final SparkFlexConfig followConfig = new SparkFlexConfig();
2929

3030
// private final VL6180 timeOfFlight = new VL6180(Port.kOnboard);
3131

32-
public HopperIOReal(){
32+
public HopperIOReal() {
3333
followConfig.follow(leadMotor, ShooterConstants.FOLLOW_INVERT);
3434
followMotor.configure(
3535
followConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
@@ -69,8 +69,11 @@ public void configMotor(double kV, double kP, double maxAcceleration) {
6969
public boolean setIdleMode(IdleMode value) {
7070
config.idleMode(value);
7171
followConfig.idleMode(value);
72-
return leadMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
73-
== REVLibError.kOk && followMotor.configure(followConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
74-
== REVLibError.kOk;
72+
return leadMotor.configure(
73+
config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
74+
== REVLibError.kOk
75+
&& followMotor.configure(
76+
followConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
77+
== REVLibError.kOk;
7578
}
7679
}

‎src/main/java/frc/robot/subsystems/intake/Intake.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -83,12 +83,12 @@ public class Intake extends SubsystemBase {
8383
extenderkD.initDefault(0);
8484
} else if (Constants.currentMode == Mode.REAL) {
8585
frontRollerkP.initDefault(0.0);
86-
frontRollerkV.initDefault(0.0);
87-
frontRollerMaxAccelerationConfig.initDefault(0.0);
86+
frontRollerkV.initDefault(0.0002);
87+
frontRollerMaxAccelerationConfig.initDefault(1000);
8888

8989
backRollerkP.initDefault(0.0);
90-
backRollerkV.initDefault(0.0);
91-
backRollerMaxAccelerationConfig.initDefault(0.0);
90+
backRollerkV.initDefault(0.0002);
91+
backRollerMaxAccelerationConfig.initDefault(1000);
9292

9393
extenderkP.initDefault(0);
9494
extenderkD.initDefault(0);

‎src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java‎

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,6 @@
1414
import edu.wpi.first.units.Units;
1515
import edu.wpi.first.units.measure.AngularVelocity;
1616
import edu.wpi.first.units.measure.Distance;
17-
import edu.wpi.first.wpilibj.I2C.Port;
18-
import frc.lib.teamBSR.VL6180;
1917
import frc.robot.Constants;
2018
import frc.robot.Constants.ElevatorConstants;
2119
import frc.robot.Constants.IntakeConstants;
@@ -37,10 +35,10 @@ public class IntakeIOReal implements IntakeIO {
3735
private final RelativeEncoder backRollersEncoder = backRollers.getEncoder();
3836
private final RelativeEncoder extenderEncoder = extender.getEncoder();
3937

40-
private final VL6180 timeOfFlight = new VL6180(Port.kOnboard);
38+
// private final VL6180 timeOfFlight = new VL6180(Port.kOnboard);
4139

4240
public IntakeIOReal() {
43-
timeOfFlight.startContinuous((int) (Constants.defaultPeriod * 1000));
41+
// timeOfFlight.startContinuous((int) (Constants.defaultPeriod * 1000));
4442
}
4543

4644
@Override
@@ -67,7 +65,7 @@ public void setFrontVoltage(double volts) {
6765

6866
@Override
6967
public void setBackVoltage(double volts) {
70-
frontRollers.setVoltage(volts);
68+
backRollers.setVoltage(volts);
7169
}
7270

7371
@Override

‎src/main/java/frc/robot/subsystems/shooter/Shooter.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -56,9 +56,9 @@ public class Shooter extends SubsystemBase {
5656

5757
} else if (Constants.currentMode == Mode.REAL) {
5858
kP.initDefault(0.0);
59-
kV.initDefault(0.0);
59+
kV.initDefault(0.000162);
6060

61-
maxAccelerationConfig.initDefault(0.0);
61+
maxAccelerationConfig.initDefault(8000);
6262
}
6363
}
6464

‎src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ public class ShooterIOReal implements ShooterIO {
2020
new SparkFlex(Constants.CanIDs.SHOOTER_LEAD_CAN_ID, MotorType.kBrushless);
2121
private final SparkFlex followMotor =
2222
new SparkFlex(Constants.CanIDs.SHOOTER_FOLLOW_CAN_ID, MotorType.kBrushless);
23-
private SparkFlexConfig config = Constants.ElevatorConstants.MOTOR_CONFIG();
23+
private SparkFlexConfig config = ShooterConstants.MOTOR_CONFIG();
2424

2525
private final RelativeEncoder encoder = leadMotor.getEncoder();
2626

0 commit comments

Comments
 (0)