Skip to content

Commit 2ebbae5

Browse files
kevinzwangkevinzwang
committed
Merge branch 'dev' of https://github.com/DeepBlueRobotics/RobotCode2019 into manual-climb
2 parents 1bfc8ef + 1843961 commit 2ebbae5

14 files changed

Lines changed: 286 additions & 34 deletions

‎Robot2019/src/main/java/frc/robot/DrivetrainCharAnalysis.java‎

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,11 @@
77
import java.io.IOException;
88
import java.io.Reader;
99
import java.util.ArrayList;
10+
import java.util.Arrays;
1011
import java.util.List;
1112

1213
import org.apache.commons.math3.stat.regression.OLSMultipleLinearRegression;
14+
import org.apache.commons.math3.linear.SingularMatrixException;
1315
import org.apache.commons.lang3.ArrayUtils;
1416
import org.apache.commons.csv.CSVRecord;
1517
import org.apache.commons.csv.CSVFormat;
@@ -64,9 +66,14 @@ public static void ordinaryLeastSquares(String file1, String file2, String outfi
6466
ys[i] = voltages[i];
6567
}
6668

67-
algorithm = new OLSMultipleLinearRegression();
68-
algorithm.newSampleData(ys, xs);
69-
params = algorithm.estimateRegressionParameters();
69+
try {
70+
algorithm = new OLSMultipleLinearRegression();
71+
algorithm.newSampleData(ys, xs);
72+
params = algorithm.estimateRegressionParameters();
73+
} catch (SingularMatrixException e) {
74+
System.out.println(Arrays.deepToString(xs));
75+
System.out.println(Arrays.toString(ys));
76+
}
7077
// System.out.println(params.length);
7178
rightKv = params[1];
7279
rightKa = params[2];

‎Robot2019/src/main/java/frc/robot/OI.java‎

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -12,20 +12,23 @@
1212
import edu.wpi.first.wpilibj.Joystick;
1313
import edu.wpi.first.wpilibj.buttons.JoystickButton;
1414
import frc.robot.commands.ActuateClimberRails;
15-
import frc.robot.commands.NormalDrive;
1615
import frc.robot.commands.Climb;
1716
import frc.robot.commands.EjectCargo;
1817
import frc.robot.commands.IntakeOnlyCargo;
1918
import frc.robot.commands.ManualClimb;
19+
import frc.robot.commands.NormalDrive;
20+
import frc.robot.commands.ResetWobble;
2021
import frc.robot.commands.SetArcadeOrTank;
2122
import frc.robot.commands.SlowDrive;
2223
import frc.robot.commands.ToggleCamera;
2324
import frc.robot.commands.ToggleHatch;
25+
import frc.robot.commands.ToggleLight;
2426
import frc.robot.commands.WobbleDrive;
2527
import frc.robot.subsystems.Cargo;
2628
import frc.robot.subsystems.Climber;
2729
import frc.robot.subsystems.Drivetrain;
2830
import frc.robot.subsystems.HatchPanel;
31+
import frc.robot.subsystems.Lights;
2932

3033
/**
3134
* This class is the glue that binds the controls on the physical operator
@@ -43,9 +46,10 @@ public class OI {
4346
JoystickButton climbBtn;
4447
JoystickButton toggleCameraBtn;
4548
JoystickButton wobbleDriveBtn;
49+
JoystickButton cycleLightBtn;
4650

47-
OI(Drivetrain dt, HatchPanel hp, Cargo cargo, Climber climber, UsbCamera driveCamera, UsbCamera hatchCamera,
48-
VideoSink cameraServer) {
51+
OI(Drivetrain dt, HatchPanel hp, Cargo cargo, Climber climber, Lights lights, UsbCamera driveCamera,
52+
UsbCamera hatchCamera, VideoSink cameraServer) {
4953
leftJoy = new Joystick(0);
5054
rightJoy = new Joystick(1);
5155
manipulator = new Joystick(2);
@@ -55,7 +59,8 @@ public class OI {
5559
rightSlowBtn = new JoystickButton(rightJoy, 1);
5660
rightSlowBtn.whileHeld(new SlowDrive(SlowDrive.Side.RIGHT));
5761
wobbleDriveBtn = new JoystickButton(rightJoy, 4); // TODO: confirm button with drivers
58-
wobbleDriveBtn.whileHeld(new WobbleDrive(dt));
62+
wobbleDriveBtn.whenPressed(new WobbleDrive(dt));
63+
wobbleDriveBtn.whenReleased(new ResetWobble(dt));
5964

6065
arcadeOrTankBtn = new JoystickButton(leftJoy, 4);
6166
arcadeOrTankBtn.whenPressed(new SetArcadeOrTank());
@@ -78,6 +83,9 @@ public class OI {
7883

7984
toggleCameraBtn = new JoystickButton(leftJoy, 2);
8085
toggleCameraBtn.whenPressed(new ToggleCamera(driveCamera, hatchCamera, cameraServer));
86+
87+
cycleLightBtn = new JoystickButton(manipulator, Manip.START);
88+
cycleLightBtn.whenPressed(new ToggleLight(lights));
8189
}
8290

8391
private class Manip {

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

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,13 +18,15 @@
1818
import frc.robot.subsystems.Climber;
1919
import frc.robot.subsystems.Drivetrain;
2020
import frc.robot.subsystems.HatchPanel;
21+
import frc.robot.subsystems.Lights;
2122

2223
public class Robot extends TimedRobot {
2324
private static Drivetrain dt;
2425
private static HatchPanel hp;
2526
private static OI oi;
2627
private static Cargo cargo;
2728
private static Climber climber;
29+
private static Lights lights;
2830
private static String fname1, fname2, fname3, fname4;
2931

3032
@Override
@@ -34,8 +36,8 @@ public void robotInit() {
3436
hp = new HatchPanel(RobotMap.hatchPistons);
3537
cargo = new Cargo(RobotMap.cargoRoller, RobotMap.pdp, RobotMap.cargoPDPPort);
3638
climber = new Climber(RobotMap.climberMotor, RobotMap.climberEncoder, RobotMap.ahrs, RobotMap.climberPistons);
37-
38-
oi = new OI(dt, hp, cargo, climber, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer);
39+
lights = new Lights(RobotMap.lights);
40+
oi = new OI(dt, hp, cargo, climber, lights, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer);
3941

4042
fname1 = "/home/lvuser/drive_char_linear_for.csv";
4143
fname2 = "/home/lvuser/drive_char_stepwise_for.csv";

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

Lines changed: 8 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@ public class RobotMap {
4141
static VictorSP cargoRoller;
4242
static Encoder leftEnc, rightEnc;
4343
static String driveMode;
44+
static VictorSP lights;
4445
static AHRS ahrs;
4546
static PowerDistributionPanel pdp;
4647
static UsbCamera driveCamera, hatchCamera;
@@ -74,6 +75,9 @@ public class RobotMap {
7475
leftEnc = new Encoder(new DigitalInput(0), new DigitalInput(1));
7576
rightEnc = new Encoder(new DigitalInput(2), new DigitalInput(3));
7677

78+
// Initialize lights
79+
lights = new VictorSP(3); // TODO: Set this to correct port
80+
7781
ahrs = new AHRS(SPI.Port.kMXP);
7882
pdp = new PowerDistributionPanel();
7983

@@ -120,11 +124,6 @@ private static WPI_TalonSRX createConfiguredTalon(int port) {
120124
catchError(tsrx.configNeutralDeadband(0.001, 10));
121125
tsrx.setNeutralMode(NeutralMode.Brake);
122126

123-
ecDeadband = tsrx.configNeutralDeadband(0.001, 10);
124-
if (!ecDeadband.equals(ErrorCode.OK)) {
125-
throw new RuntimeException(ecDeadband.toString());
126-
}
127-
128127
return tsrx;
129128
}
130129

@@ -140,11 +139,6 @@ private static WPI_VictorSPX createConfiguredVictor(int port) {
140139
catchError(vspx.configNeutralDeadband(0.001, 10));
141140
vspx.setNeutralMode(NeutralMode.Brake);
142141

143-
ecDeadband = vspx.configNeutralDeadband(0.001, 10);
144-
if (!ecDeadband.equals(ErrorCode.OK)) {
145-
throw new RuntimeException(ecDeadband.toString());
146-
}
147-
148142
return vspx;
149143
}
150144

@@ -156,9 +150,12 @@ private static UsbCamera configureCamera(int port) {
156150

157151
/**
158152
* Checks an error code and prints it to standard out if it is not ok
153+
*
159154
* @param ec The error code to check
160155
*/
161156
private static void catchError(ErrorCode ec) {
162-
if(ec != ErrorCode.OK) System.out.println("Error configuring in RobotMap.java at line: " + new Throwable().getStackTrace()[1].getLineNumber());
157+
if (ec != ErrorCode.OK)
158+
System.out
159+
.println("Error configuring in RobotMap.java at line: " + new Throwable().getStackTrace()[1].getLineNumber());
163160
}
164161
}

‎Robot2019/src/main/java/frc/robot/commands/Climb.java‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -19,11 +19,12 @@ public class Climb extends Command {
1919
private State state;
2020

2121
private final double backDrive = -0.5; // TODO: Set this to reasonable/tested value;
22-
private final double climbUp = 0.5; // TODO: Set this to reasonable/tested value;
23-
private final double climbDown = -0.5; // TODO: Set this to reasonable/tested value;
24-
private final double retract = -0.5; // TODO: Set this to reasonable/tested value;
22+
private final double climbUp = 1;
23+
private final double climbDown = -1;
24+
private final double retract = -1;
2525
private final double overrideThreshold = 0.1; // TODO: Set this to reasonable/tested value;
26-
private final double retractGoal = 0; // TODO: Set this to reasonable/tested value;
26+
private final double retractGoal = 15; // This only needs to be off the ground. Climb is 19 inches.
27+
private final double offGroundHeight = 10;
2728

2829
public Climb(Climber climber, Drivetrain dt, Joystick joy) {
2930
// Use requires() here to declare subsystem dependencies
@@ -102,7 +103,7 @@ protected void interrupted() {
102103
* overriding the climb
103104
*/
104105
private boolean robotOnPlatform() {
105-
return dt.isStalled() || Math.abs(dtJoy.getY()) > overrideThreshold;
106+
return (dt.isStalled() && climber.getEncDistance() > offGroundHeight) || Math.abs(dtJoy.getY()) > overrideThreshold;
106107
}
107108

108109
private enum State {

‎Robot2019/src/main/java/frc/robot/commands/KeepClimber.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,8 @@
1313
public class KeepClimber extends Command {
1414
private Climber climber;
1515

16-
private final double retractSpeed = -1; // TODO: Confirm number
16+
private final double retractSpeed = -1;
17+
1718
public KeepClimber(Climber climber) {
1819
// Use requires() here to declare subsystem dependencies
1920
// eg. requires(chassis);
@@ -29,7 +30,7 @@ protected void initialize() {
2930
// Called repeatedly when this Command is scheduled to run
3031
@Override
3132
protected void execute() {
32-
if(climber.slipping()) {
33+
if (climber.slipping()) {
3334
climber.runClimber(retractSpeed);
3435
} else {
3536
climber.stopClimber();

‎Robot2019/src/main/java/frc/robot/commands/ResetWobble.java‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package frc.robot.commands;
9+
10+
import edu.wpi.first.wpilibj.command.InstantCommand;
11+
import frc.robot.subsystems.Drivetrain;
12+
13+
/**
14+
* Add your docs here.
15+
*/
16+
public class ResetWobble extends InstantCommand {
17+
/**
18+
* Add your docs here.
19+
*/
20+
Drivetrain dt;
21+
public ResetWobble(Drivetrain dt) {
22+
super();
23+
// Use requires() here to declare subsystem dependencies
24+
// eg. requires(chassis);
25+
this.dt = dt;
26+
}
27+
28+
// Called once when the command executes
29+
@Override
30+
protected void initialize() {
31+
dt.setWobbleDone(true);
32+
}
33+
34+
}

‎Robot2019/src/main/java/frc/robot/commands/SetArcadeOrTank.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ public SetArcadeOrTank() {
88

99
}
1010

11+
@Override
1112
protected void initialize() {
1213
if (SmartDashboard.getBoolean("Arcade Drive", true)) {
1314
SmartDashboard.putBoolean("Arcade Drive", false);

‎Robot2019/src/main/java/frc/robot/commands/SetLight.java‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package frc.robot.commands;
9+
10+
import edu.wpi.first.wpilibj.command.Command;
11+
import frc.robot.subsystems.Lights;
12+
13+
public class SetLight extends Command {
14+
private Lights lights;
15+
16+
public SetLight(Lights lights) {
17+
// Use requires() here to declare subsystem dependencies
18+
// eg. requires(chassis);
19+
this.lights = lights;
20+
requires(lights);
21+
}
22+
23+
// Called just before this Command runs the first time
24+
@Override
25+
protected void initialize() {
26+
}
27+
28+
// Called repeatedly when this Command is scheduled to run
29+
@Override
30+
protected void execute() {
31+
lights.setLights();
32+
}
33+
34+
// Make this return true when this Command no longer needs to run execute()
35+
@Override
36+
protected boolean isFinished() {
37+
return false;
38+
}
39+
40+
// Called once after isFinished returns true
41+
@Override
42+
protected void end() {
43+
}
44+
45+
// Called when another command which requires one or more of the same
46+
// subsystems is scheduled to run
47+
@Override
48+
protected void interrupted() {
49+
end();
50+
}
51+
}

‎Robot2019/src/main/java/frc/robot/commands/ToggleLight.java‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package frc.robot.commands;
9+
10+
import edu.wpi.first.wpilibj.command.InstantCommand;
11+
import frc.robot.subsystems.Lights;
12+
13+
/**
14+
* Add your docs here.
15+
*/
16+
public class ToggleLight extends InstantCommand {
17+
/**
18+
* Add your docs here.
19+
*/
20+
private Lights lights;
21+
22+
public ToggleLight(Lights lights) {
23+
super();
24+
// Use requires() here to declare subsystem dependencies
25+
// eg. requires(chassis);
26+
this.lights = lights;
27+
}
28+
29+
// Called once when the command executes
30+
@Override
31+
protected void initialize() {
32+
lights.toggleLights();
33+
}
34+
35+
}

0 commit comments

Comments
 (0)