Skip to content

Commit 0cce415

Browse files
twistybittwistybit
committed
working v4.1
1 parent e6b779a commit 0cce415

File tree

8 files changed

+85
-31
lines changed

8 files changed

+85
-31
lines changed

‎src/main/java/frc/team5115/Auto/AutoCommands/DriveDistance.java‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,10 @@
55
import frc.team5115.Configuration.Constants;
66
import frc.team5115.Subsystems.Drivetrain;
77
import frc.team5115.Subsystems.Locationator;
8+
import io.github.oblarg.oblog.Loggable;
9+
import io.github.oblarg.oblog.annotations.Log;
810

9-
public class DriveDistance extends CommandBase {
11+
public class DriveDistance extends CommandBase implements Loggable {
1012

1113
Loc2D targetLocation;
1214
Drivetrain drivetrain;
@@ -22,7 +24,9 @@ public DriveDistance(Loc2D targetLocation, Drivetrain drivetrain, Locationator l
2224
this.locationator = locationator;
2325
}
2426

25-
@Override
27+
@Log
28+
public double getDistance(){return locationator.getCurrentLocation().distanceFrom(targetLocation);}
29+
2630
public void initialize() {
2731
System.out.println("Starting Drive Distance Command");
2832
}

‎src/main/java/frc/team5115/Auto/AutoSeries.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ public AutoSeries(Drivetrain drivetrain, Locationator locationator, Shooter shoo
2424

2525
//These commands do a basic auto series.
2626
addCommands(
27-
new DriveDistance(overLineLocation, drivetrain, locationator),
27+
new DriveDistance(overLineLocation, drivetrain, locationator).withTimeout(2),
2828
new InstantCommand(shooter::shoot)
2929
//new ShootHighGoal(drivetrain, locationator, shooter, limelight)
3030
//new PickupBallAuto(drivetrain,locationator,limelight)

‎src/main/java/frc/team5115/Commands/Feeder/FeedtheDemon.java‎

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -22,15 +22,13 @@ public void execute() {
2222
feeder.updateBallCount();
2323
//get sensor value
2424
if (feeder.getBallCount() < FULL_CAPACITY) {
25-
// if (feeder.getBallPresentInFeeder()) {
26-
// new FunctionalCommand(
27-
// feeder::debug,
28-
// () -> feeder.moveCells(),
29-
// () -> feeder.stopCells(),
30-
// () -> !feeder.getBallPresentInFeeder()
31-
// feeder
32-
// ).deadlineWith(new WaitCommand(1)).schedule();
33-
// }
25+
if (feeder.getBallPresentInFeeder()) {
26+
new StartEndCommand(
27+
() -> feeder.moveCells(),
28+
() -> feeder.stopCells(),
29+
feeder
30+
).withTimeout(.15).schedule();
31+
}
3432
}
3533
else {
3634
feeder.stopCells();

‎src/main/java/frc/team5115/Configuration/Constants.java‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,10 @@
22

33
import io.github.oblarg.oblog.Loggable;
44
import io.github.oblarg.oblog.annotations.Config;
5+
import io.github.oblarg.oblog.annotations.Log;
56

67
public class Constants implements Loggable {
7-
8+
@Log
89
public static final String VERSION = "v3.3";
910
//Best disatnce for semi-auto high goal is 148 inches
1011

@@ -51,7 +52,7 @@ public class Constants implements Loggable {
5152
public static final byte SHOOTER_BUTTON_ID = 6;
5253
public static final byte AUTO_TURN_BUTTON_ID = 8;
5354
public static final byte TEST_BUTTON_ID = 9;
54-
55+
@Log
5556
public static final int WINCH_DOWN_BUTTON_ANGLE = 180;
5657
public static final int WINCH_UP_BUTTON_ANGLE = 0;
5758

@@ -62,12 +63,12 @@ public class Constants implements Loggable {
6263
public static final double FEEDER_STORE_SPEED = -0.4;
6364
public static final double FEEDER_FLUSH_SPEED = -0.8;
6465
public static final double INTAKE_INHALE_SPEED = -0.3;
65-
public static final double FEEDER_SPEED = -0.8;
66+
public static final double FEEDER_SPEED = -1;
6667

6768
public static final int POINTING_UP = 250;
68-
public static final int FULL_CAPACITY = 5;
69-
public static final int FEEDER_PROXIMITY_BOUND = 35;
70-
public static final int SHOOTER_PROXIMITY_BOUND = 200;
69+
public static final int FULL_CAPACITY = 10; //used to be 5
70+
public static final int FEEDER_PROXIMITY_BOUND = 30;
71+
public static final int SHOOTER_PROXIMITY_BOUND = 50;
7172
public static final int DRIVING_CAM_MAX_ANGLE = 95;
7273
public static final int DRIVING_CAM_MIN_ANGLE = 55;
7374
public static final double DRIVING_CAM_ANGLE_GAIN = .4;

‎src/main/java/frc/team5115/Robot/RobotContainer.java‎

Lines changed: 15 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,14 @@
11
package frc.team5115.Robot;
22

33
import edu.wpi.first.wpilibj.Joystick;
4+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
45
import edu.wpi.first.wpilibj2.command.*;
56
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
67
import edu.wpi.first.wpilibj2.command.button.POVButton;
78
import frc.team5115.Auto.AutoCommands.ShootHighGoal;
89
import frc.team5115.Auto.AutoSeries;
910
import frc.team5115.Auto.AutoCommands.AssistedShootHighGoal;
11+
import frc.team5115.Auto.Loc2D;
1012
import frc.team5115.Configuration.Heartbeat;
1113
import frc.team5115.Subsystems.*;
1214

@@ -30,19 +32,22 @@ public class RobotContainer {
3032

3133
private Heartbeat heartbeat;
3234

35+
3336
public RobotContainer() {
3437
// Configure the button bindings
3538
//sets the navx to work.
3639
locationator = new Locationator(this, startingConfiguration, startingAngle);
3740
drivetrain = new Drivetrain(this);
3841
autoSeries = new AutoSeries(drivetrain, locationator, shooter, limelight);
3942
heartbeat = new Heartbeat();
43+
4044
configureButtonBindings();
4145
}
4246

4347
private void configureButtonBindings() {
4448
new JoystickButton(joy, TEST_BUTTON_ID)
45-
.whenHeld(new InstantCommand(feeder::debug).alongWith(new InstantCommand(climber::debug)));
49+
.whenHeld(new InstantCommand(feeder::moveCells).alongWith(new InstantCommand(climber::debug)))
50+
.whenReleased(new InstantCommand(feeder::stopCells));
4651
new JoystickButton(joy, AUTO_TURN_AND_MOVE_BUTTON_ID)
4752
.whenHeld(new ShootHighGoal(drivetrain, locationator, shooter, limelight));
4853

@@ -101,7 +106,7 @@ private void configureButtonBindings() {
101106
new JoystickButton(joy, SHOOTER_BUTTON_ID)
102107
.whenHeld(
103108
new SequentialCommandGroup(
104-
new RunCommand(shooter::shoot).withTimeout(2),
109+
new RunCommand(shooter::shoot).withTimeout(1),
105110
new InstantCommand(shooter::shoot).alongWith(new InstantCommand(feeder::moveCells))
106111
))
107112
.whenReleased(new InstantCommand(shooter::stopShoot)
@@ -124,7 +129,7 @@ private void configureButtonBindings() {
124129
.alongWith(new InstantCommand(feeder::stopCells)));
125130

126131
new JoystickButton(joy, INTAKE_BUTTON_ID)
127-
.whenHeld(new InstantCommand(intake::driverIntake).alongWith(new InstantCommand(feeder::moveCells)))
132+
.whenHeld(new InstantCommand(intake::inhale))
128133
.whenReleased(new InstantCommand(intake::stopIntake));
129134

130135
drivetrain.setDefaultCommand(new driveDefaultCommand(drivetrain, joy).perpetually());
@@ -146,6 +151,7 @@ public driveDefaultCommand(Drivetrain drivetrain, Joystick joystick) {
146151
public void execute() {
147152
drivetrain.drive(joystick.getRawAxis(XBOX_X_AXIS_ID), joystick.getRawAxis(XBOX_Y_AXIS_ID), drivetrain.throttle(XBOX_THROTTLE_1_ID, XBOX_THROTTLE_2_ID));
148153
drivetrain.setAngle();
154+
drivetrain.printThrottle();
149155
}
150156
}
151157

@@ -164,12 +170,17 @@ public void startTeleop() {
164170
System.out.println("Starting teleop: " + VERSION);
165171
feeder.reset();
166172
heartbeat.start();
173+
drivetrain.resetCameraAngle();
167174
}
168175

169176
public void periodic() {
170177
//feeder.debug();
171-
climber.debug();
172178
heartbeat.check();
179+
feeder.debug();
180+
}
181+
182+
public void p() {
183+
drivetrain.update();
173184
}
174185

175186
public double timerDifference() {

‎src/main/java/frc/team5115/Subsystems/Drivetrain.java‎

Lines changed: 42 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,30 +4,41 @@
44
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
55
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
66
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
7+
import edu.wpi.first.networktables.NetworkTable;
8+
import edu.wpi.first.networktables.NetworkTableEntry;
9+
import edu.wpi.first.networktables.NetworkTableInstance;
710
import edu.wpi.first.wpilibj.DriverStation;
811
import edu.wpi.first.wpilibj.Joystick;
912
import edu.wpi.first.wpilibj.Servo;
13+
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
14+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1015
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1116
import frc.team5115.Auto.DriveBase;
17+
import frc.team5115.Auto.Loc2D;
1218
import frc.team5115.Robot.Robot;
1319
import frc.team5115.Robot.RobotContainer;
1420
import io.github.oblarg.oblog.Loggable;
21+
import io.github.oblarg.oblog.annotations.Log;
1522

1623
import static frc.team5115.Configuration.Constants.*;
1724

1825
public class Drivetrain extends SubsystemBase implements DriveBase, Loggable {
1926
private Locationator locationator;
2027

21-
private DriverStation driverStation = DriverStation.getInstance();
22-
2328
//instances of the speed controllers
2429
private VictorSPX frontLeft;
2530
private VictorSPX frontRight;
2631
private TalonSRX backLeft;
2732
private TalonSRX backRight;
2833

34+
35+
NetworkTable networkTable = NetworkTableInstance.getDefault().getTable("SmartDashboard");
36+
NetworkTableEntry t;
37+
NetworkTableEntry l;
38+
2939
Servo servo;
3040

41+
3142
private double targetAngle; //during regular operation, the drive train keeps control of the drive. This is the angle that it targets.
3243

3344
private double rightSpd;
@@ -37,10 +48,13 @@ public class Drivetrain extends SubsystemBase implements DriveBase, Loggable {
3748

3849
double throttle = .7;
3950

51+
Loc2D targetLocation;
52+
4053
private RobotContainer robotContainer;
4154

4255
public Drivetrain(RobotContainer x) {
4356
//this.locationator = x.locationator;
57+
4458
robotContainer = x;
4559

4660
servo = new Servo(1);
@@ -51,6 +65,7 @@ public Drivetrain(RobotContainer x) {
5165
backLeft = new TalonSRX(BACK_LEFT_MOTOR_ID);
5266
backRight = new TalonSRX(BACK_RIGHT_MOTOR_ID);
5367

68+
5469
//backRight.setInverted(true);
5570

5671
//back motors are master
@@ -60,6 +75,17 @@ public Drivetrain(RobotContainer x) {
6075
backLeft.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
6176
backRight.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
6277
lastAngle = locationator.getAngle();
78+
79+
targetLocation = new Loc2D(
80+
locationator.getCurrentLocation().getX(), //goes strait forward.
81+
90);
82+
83+
84+
t = networkTable.getEntry("Throttle");
85+
l = networkTable.getEntry("Target Location");
86+
87+
t.setDouble(throttle);
88+
l.setDouble(locationator.getCurrentLocation().distanceFrom(targetLocation));
6389
}
6490

6591
@Override
@@ -86,6 +112,7 @@ public void drive(double x, double y, double throttle) { //Change the drive outp
86112
backRight.set(ControlMode.PercentOutput, rightSpd);
87113

88114
//System.out.println("servo.get() = " + servo.get());
115+
89116
}
90117

91118
public double throttle(double increase, double decrease) {
@@ -240,6 +267,19 @@ public void debug() {
240267
System.out.println("getSpeed() = " + 15*getSpeed());
241268
}
242269

270+
public void printThrottle() {
271+
System.out.println("throttle = " + throttle);
272+
}
273+
274+
public void resetCameraAngle() {
275+
servo.setAngle(DRIVING_CAM_MIN_ANGLE);
276+
}
277+
278+
public void update() {
279+
t.setValue(throttle);
280+
l.setDouble(locationator.getCurrentLocation().distanceFrom(targetLocation));
281+
}
282+
243283
}
244284

245285

‎src/main/java/frc/team5115/Subsystems/Feeder.java‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -25,9 +25,9 @@ public Feeder() {
2525
feeder = new VictorSPX(FEEDER_MOTOR_ID);
2626

2727
feederColorSensor.configureProximitySensor(ColorSensorV3.ProximitySensorResolution.kProxRes8bit, ColorSensorV3.ProximitySensorMeasurementRate.kProxRate50ms);
28-
shooterColorSensor.configureProximitySensor(ColorSensorV3.ProximitySensorResolution.kProxRes8bit, ColorSensorV3.ProximitySensorMeasurementRate.kProxRate6ms);
28+
shooterColorSensor.configureProximitySensor(ColorSensorV3.ProximitySensorResolution.kProxRes8bit, ColorSensorV3.ProximitySensorMeasurementRate.kProxRate50ms);
2929

30-
//setDefaultCommand(new FeedtheDemon(this).perpetually());
30+
setDefaultCommand(new FeedtheDemon(this).perpetually());
3131
//ballCount = 0;
3232
}
3333

@@ -74,10 +74,10 @@ public int getBallCount() {
7474

7575

7676
public void debug() {
77-
System.out.println("feederColorSensor.getProximity() = " + feederColorSensor.getProximity());
78-
System.out.println("shooterColorSensor.getProximity() = " + shooterColorSensor.getProximity());
79-
System.out.println("getBallPresentInFeeder() = " + getBallPresentInFeeder());
80-
System.out.println("getBallPresentInShooter() = " + getBallPresentInShooter());
77+
// System.out.println("feederColorSensor.getProximity() = " + feederColorSensor.getProximity());
78+
// System.out.println("shooterColorSensor.getProximity() = " + shooterColorSensor.getProximity());
79+
// System.out.println("getBallPresentInFeeder() = " + getBallPresentInFeeder());
80+
// System.out.println("getBallPresentInShooter() = " + getBallPresentInShooter());
8181
System.out.println("ballCount = " + ballCount);
8282
}
8383

‎src/main/java/frc/team5115/Subsystems/Intake.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88

99
public class Intake extends SubsystemBase {
1010
VictorSPX intake_m;
11-
double intakeSpeed = -0.3;
11+
double intakeSpeed = -0.5;
1212

1313
public Intake() {
1414
intake_m = new VictorSPX(INTAKE_MOTOR_ID);

0 commit comments

Comments
 (0)