Skip to content

Commit 4a0ce9d

Browse files
charlesdnguyen03charlesdnguyen03
committed
3.5.2020
1 parent 6b8619b commit 4a0ce9d

14 files changed

+66
-182
lines changed

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

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,9 @@
1212
* constants are needed, to reduce verbosity.
1313
*/
1414
public final class Constants {
15-
public static final double hoodFromTrench = 4000;
16-
public static final double hoodClose = 4000;
15+
public static final double hoodFromTrench = 600;
16+
public static final double hoodTriangle = 800;
17+
public static final double hoodAutoLine = 767;
1718
public static final double turretFromTrench = 6;
1819
public static final double turretClose = 0;
1920
public static final double ticksPerRevolutionLow = 19;

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

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -232,12 +232,16 @@ public void Smartdashboarding(){
232232
// SmartDashboard.putNumber("EncoderVelocity", DriveSub.encoder.getVelocity());
233233
// SmartDashboard.putBoolean("Exceeded RPM limit gear one", RobotContainer.driveSub.getRotationSpeed(RobotContainer.driveSub.getCurrentGear()) > DriveSub.limitRotSpdGear1 ? true : false);
234234

235+
SmartDashboard.putNumber("RPM of flywheel", RobotContainer.flywheelSub.getFlywheelSpeed());
236+
235237
SmartDashboard.putBoolean("Turret limit 1", RobotContainer.turretMotor.getSensorCollection().isFwdLimitSwitchClosed());
236238
SmartDashboard.putBoolean("Turret limit 2", RobotContainer.turretMotor.getSensorCollection().isRevLimitSwitchClosed());
237239

238240
SmartDashboard.putBoolean("Intake 1 touch", RobotContainer.touchOne.get());
239241
SmartDashboard.putBoolean("Intake 2 touch", RobotContainer.touchTwo.get());
240242
SmartDashboard.putBoolean("Intake 3 touch", RobotContainer.touchThree.get());
243+
SmartDashboard.putBoolean("Intake cycling ball", IntakeSub.cyclingBall);
244+
241245
//hood is 4
242246
//position three is ball 3 is digi 1intake 2 is digi two digi three is intake one
243247
}
@@ -283,11 +287,13 @@ public void configRobot(){
283287
RobotContainer.r2.setClosedLoopRampRate(ramprate);
284288
RobotContainer.r2.setOpenLoopRampRate(ramprate);
285289
RobotContainer.flywheelMotor.setSmartCurrentLimit(current);
286-
RobotContainer.flywheelMotor.setClosedLoopRampRate(ramprate);
287-
RobotContainer.flywheelMotor.setOpenLoopRampRate(ramprate);
290+
RobotContainer.flywheelMotor.setClosedLoopRampRate(ramprate+.75);
291+
RobotContainer.flywheelMotor.setOpenLoopRampRate(ramprate+.75);
288292
RobotContainer.flywheelMotor2.setSmartCurrentLimit(current);
289-
RobotContainer.flywheelMotor2.setClosedLoopRampRate(ramprate);
290-
RobotContainer.flywheelMotor2.setOpenLoopRampRate(ramprate);
293+
RobotContainer.flywheelMotor2.setClosedLoopRampRate(ramprate+.75);
294+
RobotContainer.flywheelMotor2.setOpenLoopRampRate(ramprate+.75);
295+
296+
RobotContainer.intakePistons.set(Value.kForward);
291297

292298
// RobotContainer.turretMotor.overrideLimitSwitchesEnable(false);
293299
// RobotContainer.turretMotor.overrideSoftLimitsEnable(false);
@@ -310,8 +316,8 @@ public void resetTheSpaghet(){
310316
}
311317

312318
public void shooter(){
313-
if(RobotContainer.manipulator.getPOV() == 90 || RobotContainer.manipulator.getPOV() == 180){
314-
pt.schedule();
319+
if(RobotContainer.manipulator.getPOV() == 90 || RobotContainer.manipulator.getPOV() == 180 || RobotContainer.manipulator.getPOV() == 0){
320+
// pt.schedule();
315321
ph.schedule();
316322
}
317323
}

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

Lines changed: 4 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,6 @@
1616
import frc.robot.commands.HoodCommand;
1717
import frc.robot.commands.IntakeCommand;
1818
import frc.robot.commands.PIDTurret;
19-
import frc.robot.commands.TurretCommand;
2019
import frc.robot.commands.climb.ClimbCommand;
2120
import frc.robot.commands.climb.ClimbSequence1;
2221
import frc.robot.commands.climb.ClimbSequence2;
@@ -132,7 +131,6 @@ public RobotContainer() {
132131
driveSub.setDefaultCommand(new DriveCommand());
133132
intakeSub.setDefaultCommand(new IntakeCommand());
134133
hoodSub.setDefaultCommand(new HoodCommand());
135-
turretSub.setDefaultCommand(new TurretCommand());
136134
flywheelSub.setDefaultCommand(new FlywheelCommand());
137135
climbSub.setDefaultCommand(new ClimbCommand());
138136
configureButtonBindings();
@@ -147,23 +145,11 @@ public RobotContainer() {
147145

148146
private void configureButtonBindings() {
149147

150-
da.whileHeld(new PIDTurret());
148+
// ma.whileHeld(new PIDTurret());
151149

152-
ta.whileHeld(new ClimbSequence1());
153-
tb.whileHeld(new ClimbSequence2());
154-
tb.whenReleased(new LockClimb());
155-
156-
// mb.whileHeld(
157-
// new AimCommand()
158-
// );
159-
160-
// mx.whileHeld(
161-
// new ParallelCommandGroup(
162-
// new TurretFeeder(),
163-
// new PIDFlywheel()
164-
// )
165-
// );
166-
150+
da.whileHeld(new ClimbSequence1());
151+
db.whileHeld(new ClimbSequence2());
152+
db.whenReleased(new LockClimb());
167153

168154
}
169155

‎src/main/java/frc/robot/commands/FlywheelCommand.java

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,11 @@ public void initialize() {
2727
// Called every time the scheduler runs while the command is scheduled.
2828
@Override
2929
public void execute() {
30-
RobotContainer.flywheelMotor.set(RobotContainer.manipulator.getRawAxis(3));
31-
RobotContainer.flywheelMotor2.set(RobotContainer.manipulator.getRawAxis(3));
30+
if(RobotContainer.manipulator.getRawAxis(3) > 0.5){
31+
RobotContainer.flywheelSub.spinFlywheels(1);
32+
}else{
33+
RobotContainer.flywheelSub.spinFlywheels(0);
34+
}
3235
}
3336

3437
// Called once the command ends or is interrupted.

‎src/main/java/frc/robot/commands/IntakeCommand.java

Lines changed: 10 additions & 89 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,8 @@ public void execute() {
2121

2222
if(RobotContainer.touchOne.get() && RobotContainer.touchThree.get()){
2323
IntakeSub.starWheelOff = true;
24-
}else if(RobotContainer.manipulator.getRawAxis(3) > 0.1){
24+
}
25+
if(RobotContainer.manipulator.getRawAxis(3) > 0.1){
2526
IntakeSub.starWheelOff = false;
2627
}
2728

@@ -37,13 +38,13 @@ public void execute() {
3738

3839
if(RobotContainer.ma.get()){
3940
System.out.println("1");
40-
// RobotContainer.intakeSub.intakeOut();
41-
if(!IntakeSub.starWheelOff && !RobotContainer.mb.get()){
41+
RobotContainer.intakeSub.intakeOut();
42+
if(IntakeSub.starWheelOff){
4243
System.out.println("2");
43-
RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed);
44+
RobotContainer.intakeSub.runStarWheelAndCPM(0);
4445
}else{
4546
System.out.println("3");
46-
RobotContainer.intakeSub.runStarWheelAndCPM(0);
47+
RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.intakeSpeed);
4748
}
4849

4950
if(RobotContainer.mb.get()){
@@ -60,104 +61,24 @@ public void execute() {
6061
// RobotContainer.intakeSub.intakeIn();
6162
}
6263

63-
64-
65-
// if(RobotContainer.ma.get() && RobotContainer.mb.get()){
66-
// RobotContainer.intakeSub.runIntake(-0.5);
67-
// }else if(RobotContainer.ma.get()){
68-
// RobotContainer.intakeSub.intakeOut();
69-
// RobotContainer.intakeSub.runIntake(IntakeSub.intakeSpeed);
70-
// if (RobotContainer.touchOne.get() && RobotContainer.touchThree.get()) {
71-
// RobotContainer.intakeSub.runStarWheelAndCPM(0);
72-
// } else {
73-
// RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed);
74-
// }
75-
// }else{
76-
// RobotContainer.intakeSub.intakeIn();
77-
// RobotContainer.intakeSub.runIntake(0);
78-
// RobotContainer.intakeSub.runStarWheelAndCPM(0);
79-
// }
80-
81-
// System.out.println(RobotContainer.manipulator.getRawAxis(2));
82-
83-
// if(axis2 > 0.1 && RobotContainer.mb.get()){
84-
// RobotContainer.intakeSub.runIntake(-0.5);
85-
// }else if(axis2 > 0.1){
86-
// // RobotContainer.intakeSub.intakeOut();
87-
// RobotContainer.intakeSub.runIntake(IntakeSub.intakeSpeed);
88-
// if (IntakeSub.starWheelOff) {
89-
// RobotContainer.intakeSub.runStarWheelAndCPM(0);
90-
// } else {
91-
// RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed);
92-
// }
93-
// }else if(RobotContainer.mx.get()){
94-
// RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed);
95-
// }else{
96-
// // RobotContainer.intakeSub.intakeIn();
97-
// RobotContainer.intakeSub.runIntake(0);
98-
// RobotContainer.intakeSub.runStarWheelAndCPM(0);
99-
// }
100-
101-
// if(RobotContainer.ma.get() && RobotContainer.mb.get()){
102-
// RobotContainer.intakeSub.runIntake(-0.5);
103-
// }else if(axis2 > 0.1){
104-
// RobotContainer.intakeSub.intakeOut();
105-
// RobotContainer.intakeSub.runIntake(IntakeSub.intakeSpeed);
106-
// RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed);
107-
// }else{
108-
// RobotContainer.intakeSub.intakeIn();
109-
// RobotContainer.intakeSub.runIntake(0);
110-
// RobotContainer.intakeSub.runStarWheelAndCPM(0);
111-
// }
112-
113-
if(RobotContainer.my.get()){
114-
RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed);
115-
}else{
116-
RobotContainer.intakeSub.runStarWheelAndCPM(0);
117-
}
118-
119-
//testing intake
120-
121-
if(RobotContainer.ty.get()){
122-
RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed);
123-
}else{
124-
RobotContainer.intakeSub.runStarWheelAndCPM(0);
125-
}
126-
127-
if(RobotContainer.ta.get()){
128-
RobotContainer.intakeSub.runIntake(IntakeSub.intakeSpeed);
129-
}else{
130-
RobotContainer.intakeSub.runIntake(0);
131-
}
132-
133-
if(RobotContainer.tb.get()){
134-
RobotContainer.intakeSub. runConveyorBelt(IntakeSub.conveyorMotorSpeed);
135-
}else{
136-
RobotContainer.intakeSub.runConveyorBelt(0);
137-
}
138-
139-
// if(RobotContainer.mx.get()){
64+
// if(axis2 > 0.5 && (RobotContainer.flywheelSub.getFlywheelSpeed() > 15000)){
14065
// RobotContainer.intakeSub.runConveyorBelt(IntakeSub.conveyorMotorSpeed);
14166
// }else{
14267
// RobotContainer.intakeSub.runConveyorBelt(0);
14368
// }
144-
145-
// if(!RobotContainer.ma.get()){
146-
// RobotContainer.intakeSub.intakeIn();
147-
// RobotContainer.intakeSub.runIntake(0);
148-
// RobotContainer.intakeSub.runStarWheelAndCPM(0);
149-
// }
15069

151-
if (RobotContainer.touchOne.get() && !RobotContainer.touchThree.get() && !RobotContainer.mb.get()) {
70+
if (RobotContainer.touchOne.get() && !RobotContainer.touchThree.get() && axis2 < 0.5) {
15271
IntakeSub.cyclingBall = true;
15372
}
15473

15574
if (IntakeSub.cyclingBall) {
15675
if (IntakeSub.targetTS == 2) {
15776
if (!RobotContainer.touchTwo.get()) {
15877
RobotContainer.intakeSub.runConveyorBelt(IntakeSub.conveyorMotorSpeed);
78+
if(!IntakeSub.starWheelOff) RobotContainer.intakeSub.runStarWheelAndCPM(IntakeSub.starWheelAndCPMSpeed);
15979
} else {
16080
RobotContainer.intakeSub.runConveyorBelt(0);
81+
RobotContainer.intakeSub.runStarWheelAndCPM(0);;
16182
IntakeSub.targetTS = 3;
16283
IntakeSub.cyclingBall = false;
16384
}

‎src/main/java/frc/robot/commands/PIDFlywheel.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ public void initialize() {
4040
@Override
4141
public void execute() {
4242
double output = pidController.calculate(RobotContainer.flywheelSub.getFlywheelSpeed(), setpoint);
43-
RobotContainer.flywheelSub.spinFlywheel(output);
43+
RobotContainer.flywheelSub.spinFlywheels(output);
4444
}
4545

4646
@Override

‎src/main/java/frc/robot/commands/PIDHood.java

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,18 @@
11
package frc.robot.commands;
22

33
import edu.wpi.first.wpilibj.controller.PIDController;
4+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
45
import edu.wpi.first.wpilibj2.command.CommandBase;
56
import frc.robot.Constants;
67
import frc.robot.RobotContainer;
78

89
public class PIDHood extends CommandBase {
910

1011
PIDController pidController;
11-
private final double kP = 0;
12+
private final double kP = 0.0015;
1213
private final double kI = 0;
1314
private final double kD = 0;
14-
private final double tolerance = 100;
15+
private final double tolerance = 7;
1516
private double setpoint;
1617

1718
public PIDHood(double setpoint) {
@@ -23,7 +24,9 @@ public void hoodPosition(){
2324
if(RobotContainer.manipulator.getPOV() == 90){
2425
setpoint = Constants.hoodFromTrench;
2526
}else if(RobotContainer.manipulator.getPOV() == 180){
26-
setpoint = Constants.hoodClose;
27+
setpoint = Constants.hoodAutoLine;
28+
}else if(RobotContainer.manipulator.getPOV() == 0){
29+
setpoint = Constants.hoodTriangle;
2730
}
2831
}
2932

@@ -40,7 +43,11 @@ public void initialize() {
4043
@Override
4144
public void execute() {
4245
hoodPosition();
43-
double output = pidController.calculate(RobotContainer.hoodSub.getHoodAngle(), setpoint);
46+
System.out.println("going with the PID hood");
47+
SmartDashboard.putNumber("Hood error!", pidController.getPositionError());
48+
double output = -pidController.calculate(RobotContainer.hoodSub.getHoodPos(), setpoint);
49+
50+
SmartDashboard.putNumber("Hood PID output", output);
4451
RobotContainer.hoodSub.angleHood(output);
4552
}
4653

‎src/main/java/frc/robot/commands/TurretCommand.java

Lines changed: 0 additions & 43 deletions
This file was deleted.

‎src/main/java/frc/robot/commands/TurretFeeder.java

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
import edu.wpi.first.wpilibj2.command.CommandBase;
44
import frc.robot.RobotContainer;
5+
import frc.robot.subsystems.IntakeSub;
56

67
public class TurretFeeder extends CommandBase {
78

@@ -16,7 +17,7 @@ public void initialize() {
1617
@Override
1718
public void execute() {
1819
if (PIDFlywheel.flywheelAtSetpoint) {
19-
RobotContainer.intakeSub.runConveyorBelt(0.6);
20+
RobotContainer.intakeSub.runConveyorBelt(IntakeSub.conveyorMotorSpeed);
2021
}
2122
}
2223

‎src/main/java/frc/robot/commands/auto/BasicAuto.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@ public BasicAuto() {
1919
new ParallelCommandGroup(
2020
new Forward(2),
2121
new PIDTurret(),
22-
new PIDHood(Constants.hoodClose)),
23-
new RunCommand( () -> RobotContainer.flywheelSub.spinFlywheel(1), RobotContainer.flywheelSub)
22+
new PIDHood(Constants.hoodTriangle)),
23+
new RunCommand( () -> RobotContainer.flywheelSub.spinFlywheels(1), RobotContainer.flywheelSub)
2424
);
2525

2626
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ public void teleDrive(){
6060
if(RobotContainer.dbumperLeft.get()){
6161
RobotContainer.diffDrive.arcadeDrive(RobotContainer.driver.getRawAxis(1) * 0.5, (-RobotContainer.driver.getRawAxis(4) * 0.5));
6262
} else {
63-
RobotContainer.diffDrive.arcadeDrive(RobotContainer.driver.getRawAxis(1), -RobotContainer.driver.getRawAxis(4));
63+
RobotContainer.diffDrive.arcadeDrive(RobotContainer.driver.getRawAxis(1), -RobotContainer.driver.getRawAxis(4) * 0.8);
6464
}
6565

6666
}

0 commit comments

Comments
 (0)