Knoxville

.classpath

@@ -9,6 +9,5 @@
 	<classpathentry kind="var" path="USERLIBS_DIR/CTRLib.jar"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/navx_frc.jar"/>
 	<classpathentry kind="lib" path="C:/Users/Nick&apos;s Programming/wpilib/user/java/lib/CTRLib.jar"/>
-	<classpathentry kind="var" path="USERLIBS_DIR/TalonSRXLibJava.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

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

@@ -1,20 +1,21 @@
 
 package org.usfirst.frc.team342.robot;
 
+import org.usfirst.frc.team342.robot.commands.AngleSetToZero;
 import org.usfirst.frc.team342.robot.commands.AutoShootGroup;
 import org.usfirst.frc.team342.robot.commands.DriveFoward;
 import org.usfirst.frc.team342.robot.commands.DriveWithJoystick;
 import org.usfirst.frc.team342.robot.commands.LiftWJoystick;
 import org.usfirst.frc.team342.robot.commands.RotateToDegree;
 import org.usfirst.frc.team342.robot.commands.ShooterRun;
-import org.usfirst.frc.team342.robot.commands.Useless;
 import org.usfirst.frc.team342.robot.subsystems.CameraSystem;
 import org.usfirst.frc.team342.robot.subsystems.ClimbSubsystem;
 import org.usfirst.frc.team342.robot.subsystems.DriveSubsystem;
 import org.usfirst.frc.team342.robot.subsystems.GearSubsystem;
 import org.usfirst.frc.team342.robot.subsystems.LightsSubsystem;
 import org.usfirst.frc.team342.robot.subsystems.ShooterSubsystem;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
@@ -40,16 +41,19 @@ public class Robot extends IterativeRobot {
 	private static LightsSubsystem lights;
 	private static ShooterSubsystem shooter;
 
+	private static DriverStation DS;
+
 	private static Command driveWithJoystick;
 	private static Command LiftJoystick;
 	private static Command ShooterFire;
-	private static Command Drivefoward;
+	private static Command DriveForward;
 	private static Command useless;
 	private static Command autoShoot;
 	private static Command autoRotate;
+
 
 
-	SendableChooser<Command> chooser = new SendableChooser<>();
+	SendableChooser<Integer> chooser = new SendableChooser<>();
 
 	/**
 	 * This function is run when the robot is first started up and should be
@@ -58,6 +62,8 @@ public class Robot extends IterativeRobot {
 	public Robot(){
 		cameraSystem = CameraSystem.getInstance();
 
+		DS = DriverStation.getInstance();
+
 		climbSubsystem = ClimbSubsystem.getInstance();
 		drive = DriveSubsystem.getInstance();
 		gearSubsystem = GearSubsystem.getInstance();
@@ -73,16 +79,14 @@ public Robot(){
 	@Override
 	public void robotInit() {
 		// chooser.addObject("My Auto", new MyAutoCommand());
-		useless = new Useless();
-		autoShoot = new AutoShootGroup();
-		Drivefoward = new DriveFoward(2.0 );
-		autoRotate = new RotateToDegree(180.0);
+
 		drive.resetGyro();
-		chooser.addDefault("NoAutonomus", useless);
-		chooser.addObject("It's Alive!", Drivefoward);
-		chooser.addObject("Shoot!", autoShoot);
-		chooser.addObject("Rotate to 0 deg!", autoRotate);
+		chooser.addDefault("NoAutonomus", 1);
+		chooser.addObject("Drive Forward", 2);
+		chooser.addObject("Shoot!", 3);
+		chooser.addObject("Rotate to 180 Degrees", 4);
 		SmartDashboard.putData("Auto mode", chooser);
+		SmartDashboard.putString("Alliance", DS.getAlliance().toString());
 
 	}
 
@@ -114,7 +118,29 @@ public void disabledPeriodic() {
 	 */
 	@Override
 	public void autonomousInit() {
-		Command autonomousCommand = chooser.getSelected();
+		Command autonomousCommand;
+		useless = new AngleSetToZero();
+		autoShoot = new AutoShootGroup();
+		DriveForward = new DriveFoward(2.0);
+		autoRotate = new RotateToDegree(180.0); 
+		switch(chooser.getSelected()){
+		case 1: 
+			autonomousCommand = useless;
+			break;
+		case 2:
+			autonomousCommand = DriveForward;
+			break;
+		case 3:
+			autonomousCommand = autoShoot;
+			break;
+		case 4:
+			autonomousCommand = autoRotate;
+			break;
+		default:
+			autonomousCommand = DriveForward;
+			break;
+		}
+
 		autonomousCommand.start();
 	}
 

src/org/usfirst/frc/team342/robot/commands/AngleSetToZero.java

@@ -0,0 +1,39 @@
+package org.usfirst.frc.team342.robot.commands;
+
+import org.usfirst.frc.team342.robot.subsystems.DriveSubsystem;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class AngleSetToZero extends Command {
+
+	private DriveSubsystem drive;
+
+	public AngleSetToZero(){
+		drive = DriveSubsystem.getInstance();
+	}
+
+	@Override
+	protected void initialize() {
+	}
+
+	@Override
+	protected void execute() {
+		drive.ResetAngle();
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return false;
+	}
+
+	@Override
+	protected void end() {
+		drive.stopAll();
+	}
+
+	@Override
+	protected void interrupted() {
+
+	}
+
+}

src/org/usfirst/frc/team342/robot/commands/AutoShootGroup.java

@@ -1,12 +1,43 @@
 package org.usfirst.frc.team342.robot.commands;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class AutoShootGroup extends CommandGroup {
 
+	private static final double Time = 6.0;
+	private static final double BlueBKSpeedSetting = 4000;
+	private static final double BlueFRSpeedSetting = 2300;
+	private static final double RedBKSpeedSetting = 4000;
+	private static final double RedFRSpeedSetting = 2300;
+
 	public AutoShootGroup(){
-		addParallel(new ConveyerRun());
-		addParallel(new ShooterRun(1.0));
+		boolean Blue = true;
+		DriverStation DS = DriverStation.getInstance();
+		SmartDashboard.putString("Alliance Color", DS.getAlliance().toString());
+		SmartDashboard.putString("StartedAutoShoot","Started");
+		if(DS.getAlliance().toString().equals("Red")){
+			Blue = false;
+			SmartDashboard.putString("StartedAutoShoot","Red");
+		}
+		if(Blue){
+			SmartDashboard.putString("StartedAutoShoot","Blue");
+				addSequential(new TimedManualShoot(BlueFRSpeedSetting, BlueBKSpeedSetting, Time));
+				addSequential(new DriveFoward(2.0));
+			}else{
+				addSequential(new DriveFoward(1.0));
+				addSequential(new RotateToDegree(135.0));
+				addSequential(new DriveBackward(1.0));
+				addSequential(new TimedManualShoot(RedFRSpeedSetting, RedBKSpeedSetting, Time));
+				addSequential(new DriveFoward(2.0));
+
+				//addSequential(new TimedManualShoot(RedFRSpeedSetting, RedBKSpeedSetting, Time));
+				///addSequential(new DriveFoward(2.0));
+			}
+
+
 	}
 
 

src/org/usfirst/frc/team342/robot/commands/DriveBackward.java

@@ -0,0 +1,48 @@
+package org.usfirst.frc.team342.robot.commands;
+
+import org.usfirst.frc.team342.robot.subsystems.DriveSubsystem;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class DriveBackward extends Command {
+
+	private DriveSubsystem Drive;
+	private long Endtime;
+	private double Seconds;
+
+	public DriveBackward(double seconds) {
+		Seconds = seconds;
+		Drive = DriveSubsystem.getInstance();
+	}
+
+	@Override
+	protected boolean isFinished() {
+		// TODO Auto-generated method stub
+
+		return (System.currentTimeMillis() > Endtime);
+
+	}
+
+	protected void execute(){
+		Drive.DWJmanup(0.5, 0.5, 0.0, true);
+		//SmartDashboard.putString("AutoStatus","StartTime " + System.currentTimeMillis() + "Endtime " + Endtime);
+
+	}
+
+	protected void end(){
+		Drive.DWJmanup(0, 0, 0, true);
+	}
+
+	protected void interupted() {
+		end();
+	}
+
+	protected void initialize(){
+		int MillisTime = (int) (Seconds * 1000.0);
+		Endtime = (System.currentTimeMillis() + MillisTime);
+		SmartDashboard.putString("AutoStatus","StartTime " + System.currentTimeMillis() + "Endtime " + Endtime);
+
+	}
+
+}

src/org/usfirst/frc/team342/robot/commands/DriveWithJoystick.java

@@ -11,7 +11,7 @@ public class DriveWithJoystick extends Command {
 
 	private DriveSubsystem driveSystem;
 	private Joystick joy;
-	private final double DEADZONE = 0.15;
+	private final double DEADZONE = 0.3;
 	private double angle;
 	private double magnitude;
 	private double rightStick;

src/org/usfirst/frc/team342/robot/commands/RotateToDegree.java

@@ -11,22 +11,30 @@ public class RotateToDegree extends Command {
 	private double goal;
 	private double goalhigh;
 	private double goallow;
-	private double direction;
+	private boolean direction;
 	private double speed;
+	private double highmodifier;
+	private double lowmodifier;
 
 	public RotateToDegree(double degrees) {
 		drive = DriveSubsystem.getInstance();
 		goal = degrees;
 		goalhigh = goal + 20;
 		goallow = goal - 20;
-		direction = 1.0;
+		direction = true;
 		speed = 0.5;
 
 		if (goalhigh > 360.0) {
-			goalhigh = goalhigh - 360;
+			goalhigh -= 360;
+			goallow -= 360;
+			highmodifier = 0.0;
+			lowmodifier = 360.0;
 		}
 		if (goallow < 0.0) {
 			goallow += 360;
+			goalhigh += 360;
+			highmodifier = 360.0;
+			lowmodifier = 0.0;
 		}
 	}
 
@@ -38,30 +46,29 @@ protected void initialize() {
 	@Override
 	protected void execute() {
 		double angle = drive.getGyro();
-		if(angle < 0.0){
-			angle += 360;
-		}
-
-		if ((angle < goalhigh) && (angle > goallow)) {
+
+		if (((angle + highmodifier) < goalhigh) && ((angle + lowmodifier) > goallow)) {
 			speed = 0.2;
 		}
 
 		if ((goal > 180) && (angle < (goal - 180))) {
 			angle += 360;
 		}
-		if ((angle - goal < 180.0) && !((angle - goal) < 0.0)) {
-			direction = -1.0;
-		}
+
+		//if (((angle - goal) < 180.0) && !((angle - goal) < 0.0)) {
+			//direction = false;
+		//}
+
 
-		SmartDashboard.putNumber("Gyro", drive.getGyro());
-		SmartDashboard.putNumber("angle", angle);
-		SmartDashboard.putNumber("Speed", speed);
-		SmartDashboard.putNumber("Direction", direction);
-		SmartDashboard.putNumber("GoalHigh", goalhigh);
-		SmartDashboard.putNumber("GoalLow", goallow);
+		//if (!direction) {
+			//speed = speed * -1;
+		//}
+
+		//SmartDashboard.putNumber("angle", angle);
+		//SmartDashboard.putNumber("speed", speed);
 
-		speed = speed * -1;
-		drive.spinning(speed * direction);
+
+		drive.spinning(speed);
 	}
 
 	@Override
@@ -71,12 +78,13 @@ protected boolean isFinished() {
 
 	@Override
 	protected void end() {
-
+		drive.stopAll();
+		speed = 0.5;
 	}
 
 	@Override
 	protected void interrupted() {
-
+		drive.stopAll();
 	}
 
 }

src/org/usfirst/frc/team342/robot/commands/SpinUpShooter.java

@@ -1,15 +1,21 @@
 package org.usfirst.frc.team342.robot.commands;
 
+import org.usfirst.frc.team342.robot.subsystems.DriveSubsystem;
 import org.usfirst.frc.team342.robot.subsystems.ShooterSubsystem;
 
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.command.Command;
 
+
 public class SpinUpShooter extends Command {
 
 	private ShooterSubsystem shooterSystem;
-
+	private DriveSubsystem driveSystem;
+	private Joystick joy;
+	private double angle;
 	private double triggerAxis;
-
+	private double magnitude;
+	private double rightstick;
 	private boolean shooterAtSpeed;
 
 	public SpinUpShooter(){

src/org/usfirst/frc/team342/robot/commands/TimedManualShoot.java

@@ -30,6 +30,7 @@ protected void initialize(){
 		Endtime = System.currentTimeMillis() + (Delay * 1000);
 
 		SmartDashboard.putString("TS", "It's Working!");
+		shooter.setDriveParametersAuto();
 	}
 
 	protected void execute(){