First Push

Added files

.gitignore

@@ -0,0 +1,6 @@
+# Ignored files
+*.class
+*.jar
+*.classpath
+*/target/
+/bin/

.project

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>Robot2017</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.jdt.core.javabuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.jdt.core.javanature</nature>
+		<nature>edu.wpi.first.wpilib.plugins.core.nature.FRCProjectNature</nature>
+	</natures>
+</projectDescription>

.settings/org.eclipse.jdt.core.prefs

@@ -0,0 +1,11 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.8
+org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
+org.eclipse.jdt.core.compiler.compliance=1.8
+org.eclipse.jdt.core.compiler.debug.lineNumber=generate
+org.eclipse.jdt.core.compiler.debug.localVariable=generate
+org.eclipse.jdt.core.compiler.debug.sourceFile=generate
+org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
+org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
+org.eclipse.jdt.core.compiler.source=1.8

build.properties

@@ -0,0 +1,4 @@
+# Project specific information
+package=org.usfirst.frc.team3735.robot
+robot.class=${package}.Robot
+simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world

build.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<project name="FRC Deployment" default="deploy">
+
+  <!--
+  The following properties can be defined to override system level
+  settings. These should not be touched unless you know what you're
+  doing. The primary use is to override the wpilib version when
+  working with older robots that can't compile with the latest
+  libraries.
+  -->
+
+  <!-- By default the system version of WPI is used -->
+  <!-- <property name="version" value=""/> -->
+
+  <!-- By default the system team number is used -->
+  <!-- <property name="team-number" value=""/> -->
+
+  <!-- By default the target is set to 10.TE.AM.2 -->
+  <!-- <property name="target" value=""/> -->
+
+  <!-- Any other property in build.properties can also be overridden. -->
+
+  <property file="${user.home}/wpilib/wpilib.properties"/>
+  <property file="build.properties"/>
+  <property file="${user.home}/wpilib/java/${version}/ant/build.properties"/>
+
+  <import file="${wpilib.ant.dir}/build.xml"/>
+
+</project>

src/org/usfirst/frc/team3735/robot/Robot.java

@@ -0,0 +1,260 @@
+package org.usfirst.frc.team3735.robot;
+
+import org.usfirst.frc.team3735.robot.assists.NavxAssist;
+import org.usfirst.frc.team3735.robot.assists.NavxVisionAssist;
+import org.usfirst.frc.team3735.robot.commands.autonomous.*;
+import org.usfirst.frc.team3735.robot.commands.autonomous.newstuff.AutonBottomGear;
+import org.usfirst.frc.team3735.robot.commands.autonomous.newstuff.AutonTopGear;
+import org.usfirst.frc.team3735.robot.commands.autonomous.newstuff.AutonTopGearHopper;
+import org.usfirst.frc.team3735.robot.commands.drive.RecordAverageSpeed;
+import org.usfirst.frc.team3735.robot.commands.drive.SendSDSpeed;
+import org.usfirst.frc.team3735.robot.commands.drive.arcing.DriveMoveInCircleProfile;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.DriveExp;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.DriveMoveDistanceProfile;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.MoveSine;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.ResetPosition;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.ZeroYaw;
+import org.usfirst.frc.team3735.robot.commands.scaler.ScalerUp;
+import org.usfirst.frc.team3735.robot.commands.sequences.DriveAcquireGear;
+import org.usfirst.frc.team3735.robot.commands.sequences.DrivePlaceGear;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.ois.DemoOI;
+import org.usfirst.frc.team3735.robot.ois.GTAOI;
+import org.usfirst.frc.team3735.robot.ois.NormieOI;
+import org.usfirst.frc.team3735.robot.ois.drivee;
+import org.usfirst.frc.team3735.robot.settings.Dms;
+import org.usfirst.frc.team3735.robot.subsystems.Drive;
+import org.usfirst.frc.team3735.robot.subsystems.GearIntake;
+import org.usfirst.frc.team3735.robot.subsystems.CubeIntake;
+import org.usfirst.frc.team3735.robot.subsystems.Navigation;
+import org.usfirst.frc.team3735.robot.subsystems.Scaler;
+import org.usfirst.frc.team3735.robot.subsystems.Ultrasonic;
+import org.usfirst.frc.team3735.robot.subsystems.Vision;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+import org.usfirst.frc.team3735.robot.util.bases.VortxIterative;
+import org.usfirst.frc.team3735.robot.util.oi.DriveOI;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+import org.usfirst.frc.team3735.robot.util.profiling.Position;
+import org.usfirst.frc.team3735.robot.util.settings.BooleanSetting;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the manifest file in the resource
+ * directory.
+ */
+public class Robot extends VortxIterative {
+
+	SendableChooser<Command> autonomousChooser;
+	Command autonomousCommand;
+
+	public static Drive drive;
+	public static GearIntake gearIntake;
+	public static Scaler scaler;
+	public static Navigation navigation;
+	public static Ultrasonic ultra;
+	public static Vision vision;
+	drivee d = new drivee(); 
+	public static DriveOI oi;
+	public static CubeIntake cubeIntake; 
+	//take this L
+	private double dt;
+	private double prevTime = 0;
+
+
+	public static SendableChooser<Side> sideChooser;
+	public static Side side = Side.Left;
+
+
+
+	@Override
+	public void robotInit() {
+		gearIntake = new GearIntake();
+		scaler = new Scaler();
+		drive = new Drive();
+		navigation = new Navigation();
+		ultra = new Ultrasonic();
+		vision = new Vision();
+		cubeIntake = new CubeIntake();
+
+
+		oi = new GTAOI(); //MUST be instantiated after the subsystems
+
+		autonomousChooser = new SendableChooser<Command>();
+			autonomousChooser.addDefault("Do Nothing", new AutonDoNothing());
+			autonomousChooser.addObject("Base Line", new AutonBaseline());
+
+			autonomousChooser.addObject("Left Gear Hopper", new  AutonLeftGearHopper());
+			autonomousChooser.addObject("Left Gear", new  AutonLeftGear());
+			autonomousChooser.addObject("Left Gear Baseline", new  AutonLeftGearBaseline());
+
+			autonomousChooser.addObject("Middle Gear", new  AutonMiddleGear());
+
+			autonomousChooser.addObject("Right Gear", new  AutonRightGear());
+			autonomousChooser.addObject("Right Gear Baseline", new  AutonRightGearBaseline());
+			autonomousChooser.addObject("Right Gear Hopper", new  AutonRightGearHopper());
+
+			autonomousChooser.addObject("Testing", new  AutonTest());
+
+			autonomousChooser.addObject("Top Gear", new AutonTopGear());
+			autonomousChooser.addObject("Top Gear Hopper", new AutonTopGearHopper());
+
+			autonomousChooser.addObject("Bottom Gear", new AutonBottomGear());
+
+		SmartDashboard.putData("AUTONOMOUS SELECTION", autonomousChooser);
+
+		sideChooser = new SendableChooser<Side>();
+			sideChooser.addDefault("Red", Side.Left);
+			sideChooser.addObject("Blue", Side.Right);
+		SmartDashboard.putData("Side Selection", sideChooser);	
+
+//		SmartDashboard.putData("Reset Position", new ResetPosition());
+//		SmartDashboard.putData("Gear Dropoff", new GearIntakeDropOff());
+//		SmartDashboard.putData("Scaler Start", new ScalerUp(1));
+//		SmartDashboard.putData("Acquire Gear", new  DriveAcquireGear());
+//		SmartDashboard.putData("Place Gear", new  DrivePlaceGear());
+//		SmartDashboard.putData("Zero Yaw", new ZeroYaw());
+//		
+////		SmartDashboard.putData(new RecordVoltageData());
+////		SmartDashboard.putData(new SendSDVoltage());
+//		SmartDashboard.putData(new DriveMoveDistanceProfile(100.0, 30, 30, 0).addA(new NavxAssist()));
+//		SmartDashboard.putData(new DriveMoveInCircleProfile(90, 60, true, 30, 30, 30));
+//		SmartDashboard.putData("Drive Test", new DriveExp(.5,0).addA(new NavxVisionAssist(Pipes.Peg)));
+//		SmartDashboard.putData("Sine Profile 100", new MoveSine(100));
+		side = Side.Left;
+		prevTime = Timer.getFPGATimestamp();
+	}
+	@Override
+	public void robotPeriodic() {		
+		Setting.fetchAround();
+		BooleanSetting.fetchAround();
+
+        vision.debugLog();
+        //navigation.integrate();
+        navigation.displayPosition();
+        drive.debugLog();
+        log();       
+	}
+	@Override
+	public void robotContinuous() {
+//		dt = Timer.getFPGATimestamp() - prevTime;
+//		prevTime += dt;
+//		SmartDashboard.putNumber("dt", dt);
+		navigation.integrate();
+	}
+
+
+
+	@Override
+	public void autonomousInit() {
+
+		navigation.resetPosition();
+		retrieveSide();
+		Location.changeSide(side, Dms.Field.LENGTH);
+        autonomousCommand = autonomousChooser.getSelected();
+        if (autonomousCommand != null) autonomousCommand.start();
+
+	}
+	@Override
+	public void autonomousPeriodic() {
+		 Scheduler.getInstance().run();
+	}
+	@Override
+	public void autonomousContinuous() {
+
+	}
+
+
+
+	@Override
+    public void teleopInit() {
+
+
+    }
+	@Override
+	public void teleopPeriodic() {
+        Scheduler.getInstance().run();
+//        d.Arm();
+//        d.Pivot();
+
+	}
+	@Override
+	public void teleopContinuous() {
+
+	}
+
+
+
+	@Override
+	public void testPeriodic() {
+		LiveWindow.run();
+
+	}
+	@Override
+	public void disabledInit() {
+		if (autonomousCommand != null)
+			autonomousCommand.cancel();
+	}
+	@Override
+	public void disabledPeriodic() {
+		Scheduler.getInstance().run();
+
+	}
+	@Override
+	public void disabledContinuous() {
+
+	}
+
+
+
+	public void log(){
+		scaler.log();
+		drive.log();
+		//shooter.log();
+		//ballIntake.log();
+		gearIntake.log();
+		navigation.log();
+		//ultra.log();
+		vision.log();
+	}
+
+	public void debugLog(){
+		scaler.debugLog();
+		drive.debugLog();
+		//shooter.debugLog();
+		//ballIntake.debugLog();
+		gearIntake.debugLog();
+		navigation.debugLog();
+		//ultra.debugLog();
+		vision.debugLog();
+	}
+
+
+
+
+
+
+	public static void retrieveSide(){
+		if(sideChooser.getSelected() != null){
+			side = sideChooser.getSelected();
+		}else{
+			System.out.println("Error : sideChooser was found null when retrieving side.");
+		};
+	}
+
+
+}
+

src/org/usfirst/frc/team3735/robot/assists/NavxAssist.java

@@ -0,0 +1,32 @@
+package org.usfirst.frc.team3735.robot.assists;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.cmds.ComAssist;
+
+public class NavxAssist extends ComAssist{
+
+	Double angle;
+
+	public NavxAssist(Double yaw){
+		angle = yaw;
+	}
+
+	public NavxAssist(){
+
+	}
+
+	@Override
+	public void initialize() {
+		if(angle == null){
+			Robot.navigation.getController().setSetpoint(Robot.navigation.getYaw());
+		}else{
+			Robot.navigation.getController().setSetpoint(angle.doubleValue());
+		}
+	}
+
+	@Override
+	public void execute() {
+		Robot.drive.setNavxAssist(Robot.navigation.getController().getError());
+	}
+
+}