Skip to content

Commit da1d88d

Browse files
CodeCreator3CodeCreator3
committed
more testing
1 parent edbe07b commit da1d88d

7 files changed

Lines changed: 112 additions & 95 deletions

File tree

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

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -174,13 +174,13 @@ public static final SparkFlexConfig MOTOR_CONFIG() {
174174
}
175175

176176
public class Extender {
177-
public static final boolean INVERT = false; // TODO: determine
178-
public static final Distance HOME_POSITION = Units.Meter.of(0); // TODO: determine
179-
public static final double GEAR_RATIO = 1 / 50.0; // TODO: determine (critical gear / motor)
177+
public static final boolean INVERT = true;
178+
public static final Distance HOME_POSITION = Units.Meter.of(0);
179+
public static final double GEAR_RATIO = 1 / 50.0;
180180
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(10); // TODO: determine
183+
public static final Distance MAX_LENGTH = Units.Inches.of(9.811); // TODO: determine
184184

185185
public static final SparkMaxConfig MOTOR_CONFIG() {
186186
SparkMaxConfig config = new SparkMaxConfig();
@@ -202,7 +202,7 @@ public class ShooterConstants {
202202
public static final boolean INVERT = true; // TODO: determine
203203
public static final double MOI = 0.0001;
204204
public static final Distance HORIZONTAL_OFFSET = Units.Inches.of(10); // TODO: determine
205-
public static final boolean FOLLOW_INVERT = false; // TODO: determine
205+
public static final boolean FOLLOW_INVERT = false;
206206

207207
public static final SparkFlexConfig MOTOR_CONFIG() {
208208
SparkFlexConfig config = new SparkFlexConfig();
@@ -227,6 +227,7 @@ public class HopperConstants {
227227
public static final double GEAR_RATIO = 1; // TODO: determine (mech / motor)
228228
public static final boolean INVERT = false; // TODO: determine
229229
public static final double MOI = 0.0001;
230+
public static final boolean FOLLOW_INVERT = true;
230231

231232
public static final SparkFlexConfig MOTOR_CONFIG() {
232233
SparkFlexConfig config = new SparkFlexConfig();

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

Lines changed: 32 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,8 @@ public class RobotContainer {
6767
private final TunableNumberGroup tempTunables = new TunableNumberGroup("TempTunables");
6868
private final LoggedTunableNumber tempTunableNumber =
6969
tempTunables.build("TempTunableNumber", 300);
70+
private final LoggedTunableNumber tempTunableNumber2 =
71+
tempTunables.build("TempTunableNumber2", 0.1);
7072

7173
private final SuperStateMachine superStateMachine;
7274

@@ -160,18 +162,35 @@ private void configureButtonBindings() {
160162
.onTrue(SuperStateMachine.setStateCommand(superStateMachine, SuperState.ClimbPrep))
161163
.onFalse(SuperStateMachine.setStateCommand(superStateMachine, SuperState.Climb));
162164

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-
}));
165+
// controller
166+
// .a()
167+
// .whileTrue(
168+
// Commands.run(
169+
// () -> {
170+
// System.out.println("RAN");
171+
// intake.setExtenderPos(Units.Inches.of(tempTunableNumber.get()));
172+
// }))
173+
// .onFalse(
174+
// Commands.runOnce(
175+
// () -> {
176+
// System.out.println("RAN B");
177+
// intake.setExtenderPercentOut(0);
178+
// }));
179+
180+
// controller
181+
// .b()
182+
// .whileTrue(
183+
// Commands.run(
184+
// () -> {
185+
// System.out.println("RAN");
186+
// intake.setExtenderPercentOut(tempTunableNumber2.get());
187+
// }))
188+
// .onFalse(
189+
// Commands.runOnce(
190+
// () -> {
191+
// System.out.println("RAN B");
192+
// intake.setExtenderPercentOut(0);
193+
// }));
175194

176195
SmartDashboard.putData(
177196
"Brake Mode",
@@ -217,7 +236,7 @@ public Command getAutonomousCommand() {
217236
}
218237

219238
public void robotPeriodic() {
220-
// superStateMachine.periodic();
239+
superStateMachine.periodic();
221240
}
222241

223242
public void onDisabled() {

‎src/main/java/frc/robot/stateMachines/ScoreStateMachine.java‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,6 @@
1313
import frc.robot.subsystems.SuperStructure;
1414
import frc.robot.subsystems.SuperStructure.StructureState;
1515
import frc.robot.subsystems.drive.Drive;
16-
import frc.robot.subsystems.drive.Drive.DriveState;
1716

1817
public class ScoreStateMachine extends StateMachine {
1918

@@ -35,13 +34,16 @@ public ScoreStateMachine(Drive drive, SuperStructure superStructure) {
3534

3635
private StateHandler scorePrep() {
3736
drive.setTargetAngle(calcDesiredRobotAngle());
38-
drive.setState(DriveState.RotateToAngle);
37+
// drive.setState(DriveState.RotateToAngle);
3938
superStructure.setState(StructureState.ScorePrep);
4039
boolean atRPM = superStructure.getShooter().isAtTarget();
4140
boolean atAngle = drive.isAtTargetAngle();
4241
shooterAtRPM.info(atRPM);
4342
robotAtAngle.info(atAngle);
44-
return atRPM && atAngle ? stateWithName("Score", () -> score()) : null;
43+
return atRPM
44+
// && atAngle
45+
? stateWithName("Score", () -> score())
46+
: null;
4547
}
4648

4749
private StateHandler score() {

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

Lines changed: 63 additions & 64 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
import frc.lib.team6328.LoggedTunableNumber;
1717
import frc.robot.Constants.ElevatorConstants;
1818
import frc.robot.Constants.FieldConstants;
19+
import frc.robot.Constants.IntakeConstants;
1920
import frc.robot.Constants.ShooterConstants;
2021
import frc.robot.Constants.SuperStructureConstants;
2122
import frc.robot.subsystems.elevator.Elevator;
@@ -97,70 +98,68 @@ public SuperStructure(
9798
@Override
9899
public void periodic() {
99100
switch (state) {
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;
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 fillingtree
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;
164163
default:
165164
break;
166165
}

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

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,6 @@
1313
import edu.wpi.first.units.measure.AngularVelocity;
1414
import frc.robot.Constants;
1515
import frc.robot.Constants.HopperConstants;
16-
import frc.robot.Constants.ShooterConstants;
1716

1817
public class HopperIOReal implements HopperIO {
1918

@@ -30,7 +29,7 @@ public class HopperIOReal implements HopperIO {
3029
// private final VL6180 timeOfFlight = new VL6180(Port.kOnboard);
3130

3231
public HopperIOReal() {
33-
followConfig.follow(leadMotor, ShooterConstants.FOLLOW_INVERT);
32+
followConfig.follow(leadMotor, HopperConstants.FOLLOW_INVERT);
3433
followMotor.configure(
3534
followConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
3635
}

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@ public class Intake extends SubsystemBase {
9090
backRollerkV.initDefault(0.0002);
9191
backRollerMaxAccelerationConfig.initDefault(1000);
9292

93-
extenderkP.initDefault(0);
93+
extenderkP.initDefault(0.5);
9494
extenderkD.initDefault(0);
9595
}
9696
}
@@ -140,12 +140,12 @@ public void setFrontPercentOut(double percent) {
140140
}
141141

142142
public void setBackPercentOut(double percent) {
143-
io.setBackVoltage(percent);
143+
io.setBackVoltage(percent * Constants.MAX_VOLTAGE.in(Units.Volts));
144144
// controlMode = ControlMode.OPEN_LOOP;
145145
}
146146

147147
public void setExtenderPercentOut(double percent) {
148-
io.setExtenderVoltage(percent);
148+
io.setExtenderVoltage(percent * Constants.MAX_VOLTAGE.in(Units.Volts));
149149
// controlMode = ControlMode.OPEN_LOOP;
150150
}
151151

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

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -93,10 +93,7 @@ public void setBackVel(AngularVelocity vel) {
9393

9494
@Override
9595
public void setExtenderPos(Distance length) {
96-
extender
97-
.getClosedLoopController()
98-
.setReference(
99-
length.in(Units.Inches) * ElevatorConstants.INCHES_TO_MOTOR_ROT, ControlType.kPosition);
96+
extender.getClosedLoopController().setReference(length.in(Units.Inches), ControlType.kPosition);
10097
}
10198

10299
@Override
@@ -131,7 +128,7 @@ public void setExtenderSensorPosition(Distance position) {
131128
public boolean setIdleMode(IdleMode value) {
132129
frontConfig.idleMode(value);
133130
backConfig.idleMode(value);
134-
extenderConfig.idleMode(value);
131+
extenderConfig.idleMode(IdleMode.kCoast);
135132
return frontRollers.configure(
136133
frontConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
137134
== REVLibError.kOk

0 commit comments

Comments
 (0)