Skip to content

Commit

Permalink
Merge pull request #66 from blair-robot-project/regular_drive_worlds
Browse files Browse the repository at this point in the history 
Regular drive worlds
  • Loading branch information
@rytse
rytse authored May 15, 2017
2 parents 22a4162 + ca556ec commit 40e6d48
Show file tree
Hide file tree
Showing 90 changed files with 4,848 additions and 4,712 deletions.
8 changes: 8 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,3 +23,11 @@ out/
robot2017.iml
robot2017.ipr
robot2017.iws

# R workspace files
*.RData
*.Rhistory

#Other
.metadata/
.DS_Store
1 change: 1 addition & 0 deletions .travis.yml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
sudo: required
language: java
jdk: oraclejdk8

script:
- ./gradlew build
Expand Down
264 changes: 246 additions & 18 deletions src/main/java/org/usfirst/frc/team449/robot/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,32 +1,47 @@
package org.usfirst.frc.team449.robot;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import maps.org.usfirst.frc.team449.robot.Robot2017Map;
import maps.org.usfirst.frc.team449.robot.components.MotionProfileMap;
import org.usfirst.frc.team449.robot.components.MappedDigitalInput;
import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX;
import org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDrive;
import org.usfirst.frc.team449.robot.drive.talonCluster.commands.DefaultArcadeDrive;
import org.usfirst.frc.team449.robot.drive.talonCluster.commands.ExecuteProfile;
import org.usfirst.frc.team449.robot.drive.talonCluster.commands.PIDTest;
import org.usfirst.frc.team449.robot.drive.talonCluster.commands.SwitchToLowGear;
import org.usfirst.frc.team449.robot.drive.talonCluster.commands.*;
import org.usfirst.frc.team449.robot.drive.talonCluster.util.MPLoader;
import org.usfirst.frc.team449.robot.drive.talonCluster.util.MotionProfileData;
import org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearSubsystem;
import org.usfirst.frc.team449.robot.mechanism.activegear.commands.FirePiston;
import org.usfirst.frc.team449.robot.mechanism.climber.ClimberSubsystem;
import org.usfirst.frc.team449.robot.mechanism.feeder.FeederSubsystem;
import org.usfirst.frc.team449.robot.mechanism.intake.Intake2017.Intake2017;
import org.usfirst.frc.team449.robot.mechanism.intake.Intake2017.commands.updown.IntakeUp;
import org.usfirst.frc.team449.robot.mechanism.intake.IntakeSubsystem;
import org.usfirst.frc.team449.robot.mechanism.pneumatics.PneumaticsSubsystem;
import org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.SingleFlywheelShooter;
import org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.commands.AccelerateFlywheel;
import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.FireShooter;
import org.usfirst.frc.team449.robot.oi.OI2017ArcadeGamepad;
import org.usfirst.frc.team449.robot.vision.CameraSubsystem;

import java.io.IOException;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

/**
* The main class of the robot, constructs all the subsystems and initializes default commands.
*/
public class Robot extends IterativeRobot {

public static double WHEEL_DIAMETER;

private static final double MP_UPDATE_RATE = 0.005;

private Notifier MPNotifier;


/**
* The shooter subsystem (flywheel only)
*/
Expand Down Expand Up @@ -67,11 +82,32 @@ public class Robot extends IterativeRobot {
*/
public static FeederSubsystem feederSubsystem;

public static ActiveGearSubsystem gearSubsystem;

/**
* The object constructed directly from map.cfg.
* */
private static Robot2017Map.Robot2017 cfg;

public static boolean commandFinished = false;

private int completedCommands = 0;

private long startedGearPush = 0;

private long timeToPushGear;

private Map<String, MotionProfileData> rightProfiles, leftProfiles;

private List<RotPerSecCANTalonSRX> talons;

private boolean redAlliance, dropGear;
private String allianceString;

private String position;

private I2C robotInfo;

/**
* The method that runs when the robot is turned on. Initializes all subsystems from the map.
*/
Expand All @@ -80,8 +116,7 @@ public void robotInit() {

try {
//Try to construct map from the cfg file
//cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig("/home/lvuser/449_resources/balbasaur_map.cfg",
//cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig("/home/lvuser/449_resources/final_map.cfg",
// cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig("/home/lvuser/449_resources/balbasaur_map.cfg",
cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig("/home/lvuser/449_resources/fancy_map.cfg",
Robot2017Map.Robot2017.newBuilder());
} catch (IOException e) {
Expand All @@ -90,6 +125,10 @@ public void robotInit() {
e.printStackTrace();
}

if (cfg.hasRioduinoPort()) {
robotInfo = new I2C(I2C.Port.kOnboard, cfg.getRioduinoPort());
}

//Construct the OI (has to be done first because other subsystems take the OI as an argument.)
oiSubsystem = new OI2017ArcadeGamepad(cfg.getArcadeOi());
System.out.println("Constructed OI");
Expand Down Expand Up @@ -130,15 +169,74 @@ public void robotInit() {
feederSubsystem = new FeederSubsystem(cfg.getFeeder());
}

if(cfg.hasGear()){
gearSubsystem = new ActiveGearSubsystem(cfg.getGear());
}

if(cfg.hasBlueRed()){
redAlliance = new MappedDigitalInput(cfg.getBlueRed()).getStatus().get(0);
if(redAlliance){
allianceString = "red";
} else {
allianceString = "blue";
}
dropGear = new MappedDigitalInput(cfg.getDropGear()).getStatus().get(0);
List<Boolean> tmp = new MappedDigitalInput(cfg.getLocation()).getStatus();
if (!tmp.get(0) && !tmp.get(1)){
position = "center";
} else if (tmp.get(0)){
position = "left";
} else {
position = "right";
}
}

System.out.println("redAlliance: "+redAlliance);
System.out.println("dropGear: "+dropGear);
System.out.println("positon: "+position);

//Map the buttons (has to be done last because all the subsystems need to have been instantiated.)
oiSubsystem.mapButtons();
System.out.println("Mapped buttons");

//Activate the compressor if its module number is in the map.
if (cfg.hasModule()) {
System.out.println("Setting up a compressor at module number "+cfg.getModule());
Compressor compressor = new Compressor(cfg.getModule());
compressor.setClosedLoopControl(true);
compressor.start();
System.out.println(compressor.enabled());
}

if(cfg.getDoMP()) {
WHEEL_DIAMETER = cfg.getWheelDiameterInches() / 12.;
timeToPushGear = (long) (cfg.getTimeToPushGear() * 1000);

leftProfiles = new HashMap<>();
rightProfiles = new HashMap<>();

for (MotionProfileMap.MotionProfile profile : cfg.getLeftMotionProfileList()) {
leftProfiles.put(profile.getName(), new MotionProfileData("/home/lvuser/449_resources/" + profile.getFilename(), profile.getInverted()));
}

for (MotionProfileMap.MotionProfile profile : cfg.getRightMotionProfileList()) {
rightProfiles.put(profile.getName(), new MotionProfileData("/home/lvuser/449_resources/" + profile.getFilename(), profile.getInverted()));
}

if(cfg.getTestMP()){
MPLoader.loadTopLevel(leftProfiles.get("test"), driveSubsystem.leftMaster, WHEEL_DIAMETER);
MPLoader.loadTopLevel(rightProfiles.get("test"), driveSubsystem.rightMaster, WHEEL_DIAMETER);
} else {
MPLoader.loadTopLevel(leftProfiles.get(allianceString+"_"+position), driveSubsystem.leftMaster, WHEEL_DIAMETER);
MPLoader.loadTopLevel(rightProfiles.get(allianceString+"_"+position), driveSubsystem.rightMaster, WHEEL_DIAMETER);
}

talons = new ArrayList<>();
talons.add(driveSubsystem.leftMaster);
talons.add(driveSubsystem.rightMaster);
MPNotifier = MPLoader.startLoadBottomLevel(talons, MP_UPDATE_RATE);
commandFinished = false;
completedCommands = 0;
}
}

Expand All @@ -148,21 +246,54 @@ public void robotInit() {
@Override
public void teleopInit() {
//Stop the drive for safety reasons
if (MPNotifier != null) {
MPNotifier.stop();
}
driveSubsystem.setVBusThrottle(0, 0);
driveSubsystem.overrideNavX = !cfg.getArcadeOi().getOverrideNavXWhileHeld();

driveSubsystem.leftMaster.canTalon.enable();
driveSubsystem.rightMaster.canTalon.enable();

driveSubsystem.setDefaultCommandManual(new DefaultArcadeDrive(driveSubsystem.straightPID, driveSubsystem, oiSubsystem));

// Scheduler.getInstance().add(new PIDTest(driveSubsystem));
//Switch to low gear if we have gears
if (driveSubsystem.shifter != null) {
Scheduler.getInstance().add(new SwitchToLowGear(driveSubsystem));
if (cfg.getStartLowGear()){
Scheduler.getInstance().add(new SwitchToLowGear(driveSubsystem));
} else {
Scheduler.getInstance().add(new SwitchToHighGear(driveSubsystem));
}
}

if (intakeSubsystem != null) {
Scheduler.getInstance().add(new IntakeUp(intakeSubsystem));
}

// Scheduler.getInstance().add(new DefaultArcadeDrive(driveSubsystem.straightPID, driveSubsystem, oiSubsystem));
if(gearSubsystem != null){
Scheduler.getInstance().add(new FirePiston(gearSubsystem, DoubleSolenoid.Value.kForward));
}

if (robotInfo != null) {
if (redAlliance) {
String WriteString = "red_teleop";
char[] CharArray = WriteString.toCharArray();
byte[] WriteData = new byte[CharArray.length];
for (int i = 0; i < CharArray.length; i++) {
WriteData[i] = (byte) CharArray[i];
}
robotInfo.transaction(WriteData, WriteData.length, null, 0);
} else {
String WriteString = "blue_teleop";
char[] CharArray = WriteString.toCharArray();
byte[] WriteData = new byte[CharArray.length];
for (int i = 0; i < CharArray.length; i++) {
WriteData[i] = (byte) CharArray[i];
}
robotInfo.transaction(WriteData, WriteData.length, null, 0);
}
}
}

/**
Expand All @@ -171,6 +302,15 @@ public void teleopInit() {
@Override
public void teleopPeriodic() {
//Run all commands. This is a WPILib thing you don't really have to worry about.
if (singleFlywheelShooterSubsystem != null) {
SmartDashboard.putBoolean("Shooter Running", singleFlywheelShooterSubsystem.spinning);
SmartDashboard.putNumber("Shooter Speed", singleFlywheelShooterSubsystem.talon.getSpeed());
SmartDashboard.putNumber("Shooter Error", singleFlywheelShooterSubsystem.talon.getError());
SmartDashboard.putNumber("Shooter Current", singleFlywheelShooterSubsystem.talon.canTalon.getOutputCurrent());
}
if(gearSubsystem != null) {
SmartDashboard.putBoolean("Gear Open", gearSubsystem.contracted);
}
Scheduler.getInstance().run();
}

Expand All @@ -180,14 +320,52 @@ public void teleopPeriodic() {
@Override
public void autonomousInit() {
//Set throttle to 0 for safety reasons
//Switch to low gear if we have gears
if (driveSubsystem.shifter != null) {
Scheduler.getInstance().add(new SwitchToLowGear(driveSubsystem));
if (cfg.getStartLowGear()){
Scheduler.getInstance().add(new SwitchToLowGear(driveSubsystem));
} else {
Scheduler.getInstance().add(new SwitchToHighGear(driveSubsystem));
}
}
driveSubsystem.leftMaster.canTalon.enable();
driveSubsystem.rightMaster.canTalon.enable();

if(gearSubsystem != null){
Scheduler.getInstance().add(new FirePiston(gearSubsystem, DoubleSolenoid.Value.kForward));
}

commandFinished = false;

driveSubsystem.setVBusThrottle(0, 0);
Scheduler.getInstance().add(new PIDTest(driveSubsystem));
// Scheduler.getInstance().add(new ExecuteProfile(driveSubsystem));

if (cfg.getDoMP()) {
if (singleFlywheelShooterSubsystem != null && !cfg.getTestMP()){
Scheduler.getInstance().add(new AccelerateFlywheel(singleFlywheelShooterSubsystem, 20));
}
Scheduler.getInstance().add(new ExecuteProfile(talons, 15, driveSubsystem));

if (robotInfo != null) {
if (redAlliance) {
String WriteString = "red_auto";
char[] CharArray = WriteString.toCharArray();
byte[] WriteData = new byte[CharArray.length];
for (int i = 0; i < CharArray.length; i++) {
WriteData[i] = (byte) CharArray[i];
}
robotInfo.transaction(WriteData, WriteData.length, null, 0);
} else {
String WriteString = "blue_auto";
char[] CharArray = WriteString.toCharArray();
byte[] WriteData = new byte[CharArray.length];
for (int i = 0; i < CharArray.length; i++) {
WriteData[i] = (byte) CharArray[i];
}
robotInfo.transaction(WriteData, WriteData.length, null, 0);
}

}
}else {
Scheduler.getInstance().add(new PIDTest(driveSubsystem, cfg.getDriveBackTime()));
}
}

/**
Expand All @@ -197,6 +375,56 @@ public void autonomousInit() {
public void autonomousPeriodic() {
//Run all commands. This is a WPILib thing you don't really have to worry about.
Scheduler.getInstance().run();
SmartDashboard.putNumber("Heading", driveSubsystem.getGyroOutput());
if (cfg.getDoMP() && !cfg.getTestMP()) {
if (System.currentTimeMillis() - startedGearPush > timeToPushGear && completedCommands == 1) {
commandFinished = true;
}
if (commandFinished) {
System.out.println("A command finished!");
completedCommands++;
commandFinished = false;
if (completedCommands == 1) {
if (gearSubsystem != null && dropGear) {
Scheduler.getInstance().add(new FirePiston(gearSubsystem, DoubleSolenoid.Value.kReverse));
}
startedGearPush = System.currentTimeMillis();
} else if (completedCommands == 2) {
if (position.equals("center")) {
Scheduler.getInstance().add(new DriveAtSpeed(driveSubsystem, -0.3, cfg.getDriveBackTime()));
} else if (position.equals("right") && redAlliance) {
loadProfile("red_shoot");
Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem));
} else if (position.equals("left") && !redAlliance) {
loadProfile("blue_shoot");
Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem));
} else if (redAlliance){
loadProfile("red_backup");
Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem));
} else {
loadProfile("blue_backup");
Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem));
}
} else if (completedCommands == 3) {
if (((position.equals("right") && redAlliance) || (position.equals("left") && !redAlliance)) && singleFlywheelShooterSubsystem != null) {
Scheduler.getInstance().add(new FireShooter(singleFlywheelShooterSubsystem, intakeSubsystem, feederSubsystem));
} else if (!((position.equals("right") && redAlliance) || (position.equals("left") && !redAlliance))) {
loadProfile("forward");
Scheduler.getInstance().add(new ExecuteProfile(talons, 10, driveSubsystem));
}
}
}
}
}

@Override
public void disabledInit(){
driveSubsystem.setVBusThrottle(0, 0);
}

private void loadProfile(String name){
MPNotifier.stop();
MPLoader.loadTopLevel(leftProfiles.get(name), driveSubsystem.leftMaster, WHEEL_DIAMETER);
MPLoader.loadTopLevel(rightProfiles.get(name), driveSubsystem.rightMaster, WHEEL_DIAMETER);
MPNotifier.startPeriodic(MP_UPDATE_RATE);
}
}
}
Loading

0 comments on commit 40e6d48

Please sign in to comment.