Skip to content

Commit

Permalink
v1.0.1 reorganised code structure
Browse files Browse the repository at this point in the history
yuhao1118 committed Jul 30, 2019
1 parent d92b7c2 commit 7d695c2
Showing 22 changed files with 676 additions and 279 deletions.
1 change: 1 addition & 0 deletions .idea/gradle.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

453 changes: 297 additions & 156 deletions .idea/workspace.xml

Large diffs are not rendered by default.

4 changes: 4 additions & 0 deletions build.gradle
Original file line number Diff line number Diff line change
@@ -53,6 +53,10 @@ dependencies {
nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
testCompile 'junit:junit:4.12'

simulation "edu.wpi.first.halsim:halsim_ds_socket:${wpi.wpilibVersion}:${wpi.platforms.desktop}@zip"
simulation "edu.wpi.first.halsim:halsim_ds_socket:${wpi.wpilibVersion}:${wpi.platforms.desktop}debug@zip"

}

// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
35 changes: 14 additions & 21 deletions src/main/java/frc/robot/OI.java
Original file line number Diff line number Diff line change
@@ -1,20 +1,17 @@
package frc.robot;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import frc.robot.commands.HatchPanelCommand;
import frc.robot.commands.IntakeBallCommand;
import frc.robot.commands.IntakeBallReleaseCommand;
import frc.robot.commands.IntakeSpinCP;
import frc.robot.commands.buttonCommands.*;
import frc.robot.commands.intakeCommands.*;

public class OI {
public boolean finish = false;
public Joystick xboxLeft = new Joystick(0);
public Joystick xboxRight = new Joystick(1);
public int revertDT = 0;
public int elevatorPosition = 0;

public int elevatorLevel = 0; // elevatorLevel from 0 - 3
public boolean dtRevert = false; // drivetrain revert flag

public Button buttonA1 = new JoystickButton(xboxLeft, 1),
buttonB1 = new JoystickButton(xboxLeft, 2),
@@ -36,12 +33,17 @@ public class OI {

public OI() {
buttonA1.whileHeld(new IntakeBallReleaseCommand(-0.5, -0.5));
buttonB1.whileHeld(new IntakeBallCommand(0.5,0.5));
buttonB1.whileHeld(new IntakeBallCommand(0.5, 0.5));
buttonX1.toggleWhenPressed(new DriveTrainRevertCommand());
buttonLeft1.whenPressed(new ResetElevatorCommand());
buttonRight1.whenPressed(new ResetIntakeCommand());

buttonA2.toggleWhenPressed(new IntakeSpinCP(-5000));
buttonB2.toggleWhenPressed(new IntakeSpinCP(0));
buttonY2.toggleWhenPressed(new IntakeSpinCP(30000));
buttonA2.toggleWhenPressed(new IntakeSpinPID(-200));
buttonB2.toggleWhenPressed(new IntakeSpinPID(0));
buttonY2.toggleWhenPressed(new IntakeSpinPID(300));
buttonX2.toggleWhenPressed(new HatchPanelCommand());
buttonLeft2.whenPressed(new ELUPCountCommand());
buttonRight2.whenPressed(new ELDownCountCommand());
}

public double getLeftAxis(int port) {
@@ -60,13 +62,4 @@ public boolean getRightButton(int port) {
return xboxRight.getRawButton(port);
}

public boolean getDrivetrainRevert() {
if (xboxLeft.getRawButton(1)) {
Timer.delay(0.02);
if (!xboxLeft.getRawButton(1)) {
revertDT++;
}
}
return revertDT % 2 != 0;
}
}
34 changes: 21 additions & 13 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
@@ -1,7 +1,10 @@
package frc.robot;

import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.commands.BackGroundCommand;
import frc.robot.commands.MainCommandGroup;
import frc.robot.constants.PIDConstants;
@@ -19,20 +22,20 @@ public class Robot extends TimedRobot {
// Command instances
public BackGroundCommand backGroundCommand = new BackGroundCommand();
public MainCommandGroup mainCommandGroup = new MainCommandGroup();
public frc.robot.commands.intakeCommands.IntakeSpinPID intakeSpinPID = new frc.robot.commands.intakeCommands.IntakeSpinPID(200);

// Constants objects
public static TimeConstants timeConstants = new TimeConstants();
public static PhysicsConstants physicsConstants = new PhysicsConstants();
public static PortConstants portConstants = new PortConstants();
public static PIDConstants elevatorPID = new PIDConstants("Elevator", 0.5, 0, 0);
public static PIDConstants intakePID = new PIDConstants("Intake", 0, 0, 0);
public static PIDConstants dtLeftPID = new PIDConstants("dtLeft", 0.5, 0, 0);
public static PIDConstants dtRightPID = new PIDConstants("dtRight", 0.5, 0, 0);
public static PIDConstants intakePID = new PIDConstants("Intake", 100, 0, 0);

// Subsystem instances
public static ElevatorSubsystem elevatorSubsytem = new ElevatorSubsystem();
public static IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
public static DrivetrainSubsystem drivetrainSubsystem = new DrivetrainSubsystem();
public UsbCamera camera0, camera1;

@Override
public void robotInit() {
@@ -42,6 +45,10 @@ public void robotInit() {

elevatorSubsytem.resetEncoder();
intakeSubsystem.resetEncoder();

camera0 = CameraServer.getInstance().startAutomaticCapture(0);
camera1 = CameraServer.getInstance().startAutomaticCapture(1);

}

@Override
@@ -50,34 +57,35 @@ public void robotPeriodic() {
}

@Override
public void disabledInit() {
public void autonomousInit() {
mainCommandGroup.start();
}

@Override
public void disabledPeriodic() {
public void autonomousPeriodic() {
Scheduler.getInstance().run();
}

@Override
public void autonomousInit() {
public void teleopInit() {
mainCommandGroup.start();
}

@Override
public void autonomousPeriodic() {
public void teleopPeriodic() {
Scheduler.getInstance().run();
}

@Override
public void teleopInit() {
mainCommandGroup.start();

elevatorSubsytem.resetEncoder();
intakeSubsystem.resetEncoder();
public void disabledInit() {
Robot.drivetrainSubsystem.arcadeDrive(0, 0);
Robot.elevatorSubsytem.setSpeed(0);
Robot.intakeSubsystem.setIntakeSpeed(0);
Robot.intakeSubsystem.setIntakeUDSpeed(0, 0);
}

@Override
public void teleopPeriodic() {
public void disabledPeriodic() {
Scheduler.getInstance().run();
}

16 changes: 6 additions & 10 deletions src/main/java/frc/robot/commands/BackGroundCommand.java
Original file line number Diff line number Diff line change
@@ -8,12 +8,13 @@
package frc.robot.commands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;

public class BackGroundCommand extends Command {

public BackGroundCommand() {

SmartDashboard.putData(this);
}

@Override
@@ -33,16 +34,11 @@ protected void execute() {
Robot.elevatorPID.refresh();
Robot.intakePID.refresh();

Robot.elevatorSubsytem.getSpeed();
Robot.intakeSubsystem.getSpeed();
Robot.elevatorSubsytem.getPosition();
Robot.intakeSubsystem.getPosition();

if (Robot.m_oi.getLeftButton(5))
Robot.elevatorSubsytem.resetEncoder();
// Robot.elevatorSubsytem.getSpeed();
// Robot.intakeSubsystem.getSpeed();
// Robot.elevatorSubsytem.getPosition();
// Robot.intakeSubsystem.getPosition();

if (Robot.m_oi.getLeftButton(6))
Robot.intakeSubsystem.resetEncoder();
}

@Override
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/commands/MainCommandGroup.java
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
package frc.robot.commands;

import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.commands.drivetrainCommands.JoystickCommand;
import frc.robot.commands.elevatorCommands.ElevatorCP;
import frc.robot.commands.elevatorCommands.ElevatorRawCommand;
import frc.robot.commands.intakeCommands.IntakeSpinRawCommand;

public class MainCommandGroup extends CommandGroup {
public MainCommandGroup() {
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
package frc.robot.commands.buttonCommands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;

public class DriveTrainRevertCommand extends Command {

public DriveTrainRevertCommand() {

}

@Override
protected void initialize() {
}

@Override
protected void execute() {
Robot.m_oi.dtRevert = true;
}

@Override
protected boolean isFinished() {
return false;
}

@Override
protected void end() {
Robot.m_oi.dtRevert = false;
}

@Override
protected void interrupted() {
end();
}
}
Original file line number Diff line number Diff line change
@@ -5,42 +5,43 @@
/* the project. */
/*----------------------------------------------------------------------------*/

package frc.robot.commands;
package frc.robot.commands.buttonCommands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;

public class ElevatorRawCommand extends Command {
public ElevatorRawCommand() {
public class ELDownCountCommand extends Command {

public int up = 0;
public int oldLevelValue;

public ELDownCountCommand() {

}

@Override
protected void initialize() {
Robot.elevatorSubsytem.config();
this.oldLevelValue = Robot.m_oi.elevatorLevel;
}

@Override
protected void execute() {
double input = Robot.m_oi.getRightAxis(3) - Robot.m_oi.getRightAxis(2);
if (input < 0.1 && input > -0.1) {
Robot.elevatorSubsytem.setSpeed(0.05);
if (Robot.m_oi.elevatorLevel < 3) {
Robot.m_oi.elevatorLevel++;
}
Robot.elevatorSubsytem.setSpeed(input);
}

@Override
protected boolean isFinished() {
return false;
return Robot.m_oi.elevatorLevel - oldLevelValue == 1 || Robot.m_oi.elevatorLevel == 3;
}

@Override
protected void end() {
Robot.elevatorSubsytem.setSpeed(0);
}

@Override
protected void interrupted() {
end();
}
}
Original file line number Diff line number Diff line change
@@ -5,43 +5,42 @@
/* the project. */
/*----------------------------------------------------------------------------*/

package frc.robot.commands;
package frc.robot.commands.buttonCommands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;

public class IntakeBallReleaseCommand extends Command {
double upSpeed = 0;
double downSpeed = 0;
public class ELUPCountCommand extends Command {
public int downCount = 0;
public int oldLevelValue;

public ELUPCountCommand() {

public IntakeBallReleaseCommand(double upSpeed, double downSpeed) {
this.downSpeed = downSpeed;
this.upSpeed = upSpeed;
}

@Override
protected void initialize() {
Robot.intakeSubsystem.intake_config();
this.oldLevelValue = Robot.m_oi.elevatorLevel;
}

@Override
protected void execute() {
Robot.intakeSubsystem.setIntakeUDSpeed(upSpeed, downSpeed);
if (Robot.m_oi.elevatorLevel > 0) {
Robot.m_oi.elevatorLevel--;
}
}

@Override
protected boolean isFinished() {
return false;
return Robot.m_oi.elevatorLevel - oldLevelValue == -1 || Robot.m_oi.elevatorLevel == 0;
}

@Override
protected void end() {
Robot.intakeSubsystem.setIntakeUDSpeed(-0.05, -0.05);
}

@Override
protected void interrupted() {
end();
}
}
Loading

0 comments on commit 7d695c2

Please sign in to comment.