Skip to content

Commit f3fb032

Browse files
ashwinc12ashwinc12
authored
Merge pull request #102 from FRCteam4909/Elevator-Fix-P2
Working code that could climb 2/19
2 parents 8bdf570 + 7497427 commit f3fb032

30 files changed

+893
-237
lines changed

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

Lines changed: 51 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -3,15 +3,20 @@
33
import edu.wpi.first.wpilibj.Compressor;
44
import edu.wpi.first.wpilibj.PowerDistributionPanel;
55
import edu.wpi.first.wpilibj.TimedRobot;
6+
import edu.wpi.first.wpilibj.buttons.POVButton;
67
import edu.wpi.first.wpilibj.command.Scheduler;
8+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
79
import frc.team4909.robot.operator.controllers.BionicF310;
10+
import frc.team4909.robot.operator.controllers.FlightStick;
811
import frc.team4909.robot.sensors.LidarLitePWM;
912
import frc.team4909.robot.sensors.Stream;
13+
import frc.team4909.robot.setpoints.HatchLow;
14+
import frc.team4909.robot.setpoints.HatchMiddle;
1015
import frc.team4909.robot.subsystems.climber.ClimberSubsystem;
1116
import frc.team4909.robot.subsystems.climber.commands.DriveStiltsBack;
1217
import frc.team4909.robot.subsystems.climber.commands.DriveStiltsForward;
13-
import frc.team4909.robot.subsystems.climber.commands.ExtendStilts;
14-
import frc.team4909.robot.subsystems.climber.commands.RetractStilts;
18+
import frc.team4909.robot.subsystems.climber.commands.StiltsDownOnly;
19+
import frc.team4909.robot.subsystems.climber.commands.StiltsUpOnly;
1520
import frc.team4909.robot.subsystems.drivetrain.DriveTrainSubsystem;
1621
import frc.team4909.robot.subsystems.drivetrain.Linefollow;
1722
import frc.team4909.robot.subsystems.drivetrain.commands.InvertDriveDirection;
@@ -23,6 +28,9 @@
2328
import frc.team4909.robot.subsystems.intake.commands.CargoIntakeOut;
2429
import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeOpen;
2530

31+
import frc.team4909.robot.subsystems.climber.commands.ZeroStilts;
32+
import frc.team4909.robot.subsystems.elevator.commands.ZeroElevator;
33+
2634
// Controls:
2735
//
2836
// Driver Gamepad (Port 0):
@@ -41,6 +49,10 @@
4149
// RT: Cargo Intake In
4250
// RB: Cargo Intake Out
4351
// LT: Hatch Panel Intake In
52+
//
53+
// Flight Stick (Port 2):
54+
// Y: Stilt/Elevator Climb Speed
55+
// Joystick: Stilt Up/Down
4456

4557
public class Robot extends TimedRobot {
4658

@@ -50,7 +62,8 @@ public class Robot extends TimedRobot {
5062
// Operator Input
5163
public static BionicF310 driverGamepad;
5264
public static BionicF310 manipulatorGamepad;
53-
public boolean StiltsStop;
65+
public static BionicF310 climberGamepad;
66+
public static FlightStick climbStick;
5467

5568
// Subsystems
5669
public static PowerDistributionPanel powerDistributionPanel;
@@ -77,9 +90,9 @@ public class Robot extends TimedRobot {
7790
public void robotInit() {
7891

7992
// Cameras (subsystem)
80-
stream = new Stream();
81-
// CameraServer.getInstance().startAutomaticCapture();
82-
stream.streamCamera();
93+
// stream = new Stream();
94+
// // CameraServer.getInstance().startAutomaticCapture();
95+
// stream.streamCamera();
8396
// grip = new GripPipeline();
8497

8598
// Compressor
@@ -107,6 +120,15 @@ public void robotInit() {
107120
RobotConstants.manipulatorGamepadDeadzone, // Deadzone
108121
RobotConstants.manipulatorGamepadSensitivity // Gamepad sensitivity
109122
);
123+
124+
climberGamepad = new BionicF310(3, // Port
125+
RobotConstants.manipulatorGamepadDeadzone, // Deadzone
126+
RobotConstants.manipulatorGamepadSensitivity // Gamepad sensitivity
127+
);
128+
129+
130+
climbStick = new FlightStick(2);
131+
110132
/* Drivetrain */
111133

112134
/* Intake */
@@ -115,18 +137,23 @@ public void robotInit() {
115137
manipulatorGamepad.buttonHeld(BionicF310.LT, 0.2, new HatchPanelIntakeOpen());
116138

117139
/* Climber */
118-
driverGamepad.buttonHeld(BionicF310.RT, 0.2, new ExtendStilts());
119-
driverGamepad.buttonHeld(BionicF310.LT, 0.2, new RetractStilts());
120140
driverGamepad.buttonHeld(BionicF310.LB, new DriveStiltsBack());
121141
driverGamepad.buttonHeld(BionicF310.RB, new DriveStiltsForward());
142+
climberGamepad.buttonHeld(BionicF310.RB, new StiltsUpOnly());
143+
climberGamepad.buttonHeld(BionicF310.LB, new StiltsDownOnly());
144+
122145

123-
/* Elevator */
124-
manipulatorGamepad.buttonPressed(BionicF310.A, new SetElevatorPosition(-13000, 1));
146+
/* Elevator Setpoints */
147+
manipulatorGamepad.buttonPressed(BionicF310.A, new HatchMiddle());
148+
manipulatorGamepad.buttonPressed(BionicF310.B, new HatchLow());
125149

126150
/* Sensors/Misc. */
127151
driverGamepad.buttonPressed(BionicF310.A, new InvertDriveDirection());
128152
driverGamepad.buttonPressed(BionicF310.B, new Linefollow());
129-
}
153+
154+
SmartDashboard.putData(new ZeroElevator());
155+
SmartDashboard.putData(new ZeroStilts());
156+
}
130157

131158
/**
132159
* '' This function is called every robot packet, no matter the mode. Use this
@@ -158,9 +185,21 @@ public void autonomousPeriodic() {
158185
/**
159186
* This function is called periodically during operator control.
160187
*/
188+
161189
@Override
162-
public void teleopPeriodic() {
190+
public void teleopInit() {
191+
// Reset elevator encoder
192+
193+
}
194+
@Override
195+
public void disabledPeriodic() {
196+
Scheduler.getInstance().run();
197+
elevatorSubsystem.reset();
198+
climberSubsystem.reset();
199+
}
163200

201+
@Override
202+
public void teleopPeriodic() {
164203
}
165204

166205
/**

‎src/main/java/frc/team4909/robot/RobotConstants.java‎

Lines changed: 30 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -12,15 +12,15 @@ public class RobotConstants {
1212

1313
/* Drivetrain */
1414
public static final double speedMultiplier = .7;
15-
public static final double speedTurnMultiplier = .4;
16-
public static final double speedTurnPreciseMultiplier = .2;
15+
public static final double speedTurnMultiplier = .5;
16+
public static final double speedTurnPreciseMultiplier = .4;
1717

1818
/* Intake */
1919
public static final double cargoIntakeInSpeed = 1.0;
2020
public static final double cargoIntakeHoldSpeed = 0.0625;
2121
public static final double cargoIntakeOutSpeed = -1.0;
2222

23-
// public static final double cargoIntakeCurrentLimit = 0;
23+
// public static final double cargoIntakeCurrentLimit = 0;
2424

2525
/* LIDAR */
2626
public static final int calibrationOffset = -9; // calibration constant used for Lidar
@@ -40,16 +40,41 @@ public class RobotConstants {
4040
public static final double elevatorSpeedMultiplier = 0.5; // Multiplier for elevator speed
4141
public static final double elevatorArmSpeedMultiplier = 0.75; // Multiplier for elevator arm speed
4242

43+
public static final double initialp = 0.1;
44+
public static final double initiali = 0;
45+
public static final double initiald = 0;
46+
47+
public static final double newp = 0;
48+
public static final double newi = 0;
49+
public static final double newd = 0;
50+
51+
/* Elevator Setpoints */
52+
public static final int elevatorSetpointHatchLow = 0; // Bottom hatch position
53+
public static final int elevatorSetpointHatchMiddle = -25300; // Middle hatch position
54+
public static final int elevatorSetpointHatchHigh = -51300; // Top hatch position
55+
56+
public static final int elevatorSetpointCargoLow = -10582;
57+
public static final int elevatorSetpointCargoMiddle = -30638;
58+
public static final int elevatorSetpointCargoHigh = -20000;
59+
public static final int elevatorSetpointCargoShip = -25300;
60+
61+
public static final int elevatorArmSetpoint90 = 90;
62+
public static final int elevatorArmSetpoint45 = 45;
4363
/* Linefollow */
4464
public static final double fastVelocity = 0.7;
4565
public static final double mediumVelocity = 0.5; // constant velocity given to both motors
4666
public static final double slowVelocity = 0.2; // value given to motor when trying to turn
4767

4868
/* Climber */
49-
public static final double climberStiltSpeed = 0.3;
50-
public static final double climberDriveSpeed = 0.3;
69+
public static final double climberStiltSpeed = 0.5;
70+
public static final double climberDriveSpeedAuto = 0.2;
71+
public static final double climberDriveSpeedManual = 0.5;
5172
public static final double elevatorClimbSpeed = 0.3;
73+
public static final double climbSpeedMultiplier = 0.5;
74+
public static final double climbVelocityMultiplier = 800;
75+
5276

5377
/* Timeout */
5478
public static final int timeoutMs = 30; // milliseconds
79+
5580
}

‎src/main/java/frc/team4909/robot/RobotMap.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,10 +49,12 @@ public class RobotMap {
4949
// number and the module. For example you with a rangefinder:
5050

5151
/* Climber */
52-
public static final int climberSRXID = 10;
53-
public static final int climberSPXID = 11;
52+
public static final int climberMasterSRXID = 10;
53+
public static final int climberSlaveSPXID = 12; // @todo not on robot yet
54+
public static final int climberDriveSPXID = 11;
5455

5556
/* Sensors */
5657
public static final int lidarPort = 4;
5758

59+
5860
}

‎src/main/java/frc/team4909/robot/operator/controllers/FlightStick.java‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
package frc.team4909.robot.operator.controllers;
2+
3+
import edu.wpi.first.wpilibj.buttons.JoystickButton;
4+
import edu.wpi.first.wpilibj.command.Command;
5+
import frc.team4909.robot.operator.generic.BionicPOV;
6+
7+
public class FlightStick extends edu.wpi.first.wpilibj.Joystick {
8+
9+
public static BionicPOV Top = new BionicPOV(0);
10+
public static BionicPOV Bottom = new BionicPOV(180);
11+
public static BionicPOV TopRight = new BionicPOV(45);
12+
public static BionicPOV Right = new BionicPOV(90);
13+
public static BionicPOV BottomRight = new BionicPOV(135);
14+
public static BionicPOV TopLeft = new BionicPOV(315);
15+
public static BionicPOV Left = new BionicPOV(270);
16+
public static BionicPOV BottomLeft = new BionicPOV(225);
17+
18+
public FlightStick(int port) {
19+
super(port);
20+
}
21+
22+
public double getThresholdAxis(int axis, double deadzone){
23+
if(Math.abs(this.getRawAxis(axis)) > Math.abs(deadzone)){
24+
return this.getRawAxis(axis);
25+
}
26+
else
27+
return 0.0;
28+
}
29+
30+
public void buttonPressed(int button, Command command){
31+
JoystickButton newButton = new JoystickButton(this, button);
32+
33+
newButton.whenPressed(command);
34+
}
35+
36+
public void buttonHeld(int button, Command command){
37+
JoystickButton newButton = new JoystickButton(this, button);
38+
39+
newButton.whileHeld(command);
40+
}
41+
42+
public void buttonToggled(int button, Command command){
43+
JoystickButton newButton = new JoystickButton(this, button);
44+
45+
newButton.toggleWhenPressed(command);
46+
}
47+
}

0 commit comments

Comments
 (0)