Merge pull request #64 from 2202Programming/dev

Post Season 2017

src/auto/commands/BangBangTurnCommand.java

@@ -0,0 +1,52 @@
+package auto.commands;
+
+import com.kauailabs.navx.frc.AHRS;
+
+import PID.PIDController;
+import auto.ICommand;
+import auto.IStopCondition;
+import drive.DriveControl;
+import drive.IDrive;
+import input.SensorController;
+import robot.Global;
+
+public class BangBangTurnCommand implements ICommand {
+
+	private double power, degreesToTurn;
+	private IDrive drive;
+	private AHRS navx;
+
+	public BangBangTurnCommand(double degreesToTurn, double power) {
+		this.degreesToTurn=degreesToTurn;
+		this.power=power;
+
+	}
+
+	public void init() {
+		//set the objects and set external control
+		drive=(IDrive)Global.controlObjects.get("DRIVE");
+		drive.setDriveControl(DriveControl.EXTERNAL_CONTROL);
+		navx=(AHRS)SensorController.getInstance().getSensor("NAVX");
+		//reset navx so we turn degrees from when this command is called
+		navx.reset();
+	}
+
+	public boolean run() {
+		double angle=navx.getAngle();
+		if (angle>degreesToTurn) {//turn in the right direction
+			drive.setLeftMotors(-power);
+			drive.setRightMotors(power);			
+		}
+		else {
+			drive.setLeftMotors(power);
+			drive.setRightMotors(-power);
+		}
+		//If we have turned more positive or more negative than our angle, we are done turning. Otherwise, keep going.
+		return Math.abs(angle)<Math.abs(degreesToTurn);
+	}
+
+	public void stop() {
+		drive.setDriveControl(DriveControl.DRIVE_CONTROLLED);
+	}
+
+}

src/auto/commands/ContinuousPegVisionCommand.java

@@ -2,33 +2,42 @@
 
 import java.util.ArrayList;
 
-import auto.CommandList;
-import auto.CommandListRunner;
 import auto.ICommand;
-import auto.stopConditions.DistanceStopCondition;
 import auto.stopConditions.TimerStopCondition;
 import comms.NetworkTables;
 import comms.SmartWriter;
 import comms.TableNamesEnum;
-import drive.DriveControl;
-import drive.IDrive;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Ultrasonic;
 import input.SensorController;
 import robot.Global;
 import robotDefinitions.BabbageControl;
 
+/**
+ * 
+ * @author 13dha
+ *
+ */
 public class ContinuousPegVisionCommand implements ICommand {
 	private NetworkTables table;
 	private double degreesToTurn;
 	private double distanceToMove;
-	private boolean doneWithVision=false;
-	private double distance;
 	private Ultrasonic distanceSensor;//7 is not in use
 	private DriveAtAngle driveAtAngleCommand;
 	private ArrayList<Encoder> encoders;
 	private double lastAngle=0;
+	private long msToRunFor=99999999;
+	private long maxEndTime=Long.MAX_VALUE;
 
+
+	/**
+	 * Constructs a ContinuousPegVision command object
+	 * 
+	 * @param percentToFinish
+	 * We don't actually use this at all anymore. We used to use it 
+	 * based on our distance results from when we were measuring 
+	 * distance from vision. We use an ultrasonic sensor now, though
+	 */
 	public ContinuousPegVisionCommand(double percentToFinish) {
 		table=new NetworkTables(TableNamesEnum.VisionTable);
 		driveAtAngleCommand=new DriveAtAngle(new TimerStopCondition(10000), 0.3, 0);
@@ -37,17 +46,32 @@ public ContinuousPegVisionCommand(double percentToFinish) {
 		distanceSensor.setAutomaticMode(true);
 
 	}
+
+	/**
+	 * Constructs a ContinuousPegVision command that stops after a certain number of milliseconds
+	 * @param percentToFinish
+	 * We don't actually use this at all anymore. We used to use it 
+	 * based on our distance results from when we were measuring 
+	 * distance from vision. We use an ultrasonic sensor now, though
+	 * @param msToRunFor
+	 * The number of milliseconds before this command ends. We use this in autonomous in case we get stuck on the wall.
+	 */
+	public ContinuousPegVisionCommand(double percentToFinish, long msToRunFor) {
+		this(percentToFinish);
+		this.msToRunFor=msToRunFor;
+	}
 
+	//comments in ICommand
 	public void init() {
 		SmartWriter.putD("Peg Vision activated", System.currentTimeMillis());
-		doneWithVision=false;
 		driveAtAngleCommand.init();
-		driveAtAngleCommand.setAngle(0);
+		driveAtAngleCommand.setAngle(0);//drive forward until we know what angle we should drive at
 		table.setBoolean("processVision", true);
 		encoders=new ArrayList<>();
 		encoders.add((Encoder)SensorController.getInstance().getSensor("ENCODER0"));
 		encoders.get(0).reset();
 		lastAngle=0;
+		maxEndTime=System.currentTimeMillis()+msToRunFor;
 	}
 
 	public boolean run() {
@@ -57,6 +81,13 @@ public boolean run() {
 			return true;
 		}
 
+		if (System.currentTimeMillis()>=maxEndTime) {
+			stop();
+			return true;
+		}
+
+
+		//Go slower if we are closer to where we want to be. If we are less than 10 inches from the target, the distance sensor will no longer work, so stop.
 		if (distanceSensor.getRangeInches()<24) {
 			driveAtAngleCommand.setSpeed(0.2);
 			if (distanceSensor.getRangeInches()<10) {
@@ -78,7 +109,7 @@ public boolean run() {
 			}
 		}
 
-		driveAtAngleCommand.run();
+		driveAtAngleCommand.run();//run the command to drive at an angle
 		SmartWriter.putD("Distance Sensor", distanceSensor.getRangeInches());
 
 		if (table.getBoolean("processVision")) {
@@ -93,19 +124,19 @@ public boolean run() {
 			SmartWriter.putD("degreesToTurn final", degreesToTurn);
 			SmartWriter.putD("distanceToMove final", distanceToMove);
 
-			double distanceTraveled=encoders.get(0).getDistance();
-			double distanceX=Math.cos(Math.toRadians(degreesToTurn))*distanceTraveled;
-			double distanceY=Math.sin(Math.toRadians(degreesToTurn))*distanceTraveled;
-			double increasedAngle=Math.toDegrees(Math.atan2(distanceY, distanceToMove-distanceX));
+			//double distanceTraveled=encoders.get(0).getDistance();
+			//double distanceX=Math.cos(Math.toRadians(degreesToTurn))*distanceTraveled;
+			//double distanceY=Math.sin(Math.toRadians(degreesToTurn))*distanceTraveled;
+			//double increasedAngle=Math.toDegrees(Math.atan2(distanceY, distanceToMove-distanceX));
 			//degreesToTurn+=increasedAngle;
-			distanceToMove=Math.hypot(distanceToMove-distanceX, distanceY);
+			//distanceToMove=Math.hypot(distanceToMove-distanceX, distanceY);
 
 			double tempLastAngle=lastAngle;
 			lastAngle=driveAtAngleCommand.getAngle();
-			driveAtAngleCommand.setAngle(tempLastAngle+degreesToTurn);
+			driveAtAngleCommand.setAngle(tempLastAngle+degreesToTurn);//turn at the angle vision was when we took the picture, plus what vision said to turn
 
 			table.setBoolean("processVision", true);
-			encoders.get(0).reset();
+			encoders.get(0).reset();//we don't use this anymore, it was from when we used encoders and vision to measure distance
 			return false;
 		}
 

src/auto/commands/DriveAtAngle.java

@@ -33,7 +33,7 @@ public DriveAtAngle(IStopCondition stop, double speed, double angle) {
 		usePID = true;
 		// these will most likely be small as the value needs to be under 1.0/
 		// -1.0
-		controller = new PIDController(0.01, 0.00005, .0, true, false);
+		controller = new PIDController(0.01, 0.00005, .01, true, false);
 		stopCondition = stop;
 		this.angle = angle;
 		slowSpeed = speed;

src/auto/commands/ShootCommand.java

@@ -0,0 +1,26 @@
+package auto.commands;
+
+import auto.ICommand;
+import babbage.Shooter;
+import robot.Global;
+
+public class ShootCommand implements ICommand {
+	private Shooter shooter;
+	@Override
+	public void init() {
+		shooter = (Shooter) Global.controlObjects.get("SHOOTER");
+		shooter.setAutoShoot(true);
+	}
+
+	@Override
+	public boolean run() {
+
+		return false;
+	}
+
+	@Override
+	public void stop() {
+		shooter.setAutoShoot(false);
+	}
+
+}

src/auto/stopConditions/AndStopCondition.java

@@ -0,0 +1,22 @@
+package auto.stopConditions;
+
+import auto.IStopCondition;
+
+public class AndStopCondition implements IStopCondition {
+	IStopCondition first;
+	IStopCondition second;
+
+	public AndStopCondition(IStopCondition firstCondition, IStopCondition secondCondition) {
+		first = firstCondition;
+		second = secondCondition;
+	}
+
+	public void init() {
+		first.init();
+		second.init();
+	}
+
+	public boolean stopNow() {
+		return first.stopNow() && second.stopNow();
+	}
+}

src/auto/stopConditions/OrStopCondition.java

@@ -0,0 +1,22 @@
+package auto.stopConditions;
+
+import auto.IStopCondition;
+
+public class OrStopCondition implements IStopCondition {
+	IStopCondition first;
+	IStopCondition second;
+
+	public OrStopCondition(IStopCondition firstCondition, IStopCondition secondCondition) {
+		first = firstCondition;
+		second = secondCondition;
+	}
+
+	public void init() {
+		first.init();
+		second.init();
+	}
+
+	public boolean stopNow() {
+		return first.stopNow() || second.stopNow();
+	}
+}

src/babbage/BabbageAutoLists.java

@@ -6,12 +6,13 @@
 import auto.CommandList;
 import auto.commands.ContinuousPegVisionCommand;
 import auto.commands.DriveCommand;
+import auto.commands.ShootCommand;
 import auto.commands.TurnCommand;
+import auto.commands.WaitCommand;
 import auto.stopConditions.AngleStopCondition;
 import auto.stopConditions.DistanceStopCondition;
 import auto.stopConditions.TimerStopCondition;
 import edu.wpi.first.wpilibj.Encoder;
-import auto.commands.WaitCommand;
 import input.SensorController;
 
 //TODO all distances, speeds and angles are not final
@@ -26,6 +27,26 @@ public class BabbageAutoLists {
 	private static List<Encoder> encoders = new ArrayList<Encoder>();
 
 
+	public static CommandList shootingRed() {
+		encoders.add((Encoder) sensors.getSensor("ENCODER0"));
+		CommandList cl=new CommandList();
+		cl.addCommand(new DriveCommand(new DistanceStopCondition(encoders, 64), 0.6));
+		cl.addCommand(new TurnCommand(-90, 10, 0.3));
+		cl.addCommand(new DriveCommand(new TimerStopCondition(1000), -0.6));
+		cl.addCommand(new ShootCommand());
+		return cl;
+	}
+
+	public static CommandList shootingBlue() {
+		encoders.add((Encoder) sensors.getSensor("ENCODER0"));
+		CommandList cl=new CommandList();
+		cl.addCommand(new DriveCommand(new DistanceStopCondition(encoders, 64), 0.6));
+		cl.addCommand(new TurnCommand(-90, 10, 0.3));
+		cl.addCommand(new DriveCommand(new TimerStopCondition(1000), 0.6));
+		cl.addCommand(new ShootCommand());
+		return cl;
+	}
+
 	/** Tells the robot to move forward. Then, it runs
 	 * the PegVision
 	 * 
@@ -35,8 +56,9 @@ public static CommandList centerRed(){
 		encoders.add((Encoder) sensors.getSensor("ENCODER0"));
 		CommandList CL = new CommandList();
 		CL.addCommand(new WaitCommand(new TimerStopCondition(500)));
-		CL.addCommand(new ContinuousPegVisionCommand(1));
+		CL.addCommand(new ContinuousPegVisionCommand(1, 4000));
 		CL.addCommand(new DriveCommand(new TimerStopCondition(400), 1));
+		//CL.addCommand(new ShootCommand());
 		return CL;
 	}
 
@@ -49,10 +71,11 @@ public static CommandList boilerRed(){
 
 		encoders.add((Encoder) sensors.getSensor("ENCODER0"));
 
+		//TODO add timer built into this 
 		CommandList CL = new CommandList();
 		CL.addCommand(new DriveCommand(new DistanceStopCondition(encoders, distanceFromWall), speed));
 		CL.addCommand(new TurnCommand(new AngleStopCondition(-turnAngle,turnTolerance,toleranceTime)));
-		CL.addCommand(new ContinuousPegVisionCommand(1));
+		CL.addCommand(new ContinuousPegVisionCommand(1, 4000));
 		CL.addCommand(new DriveCommand(new TimerStopCondition(400), 1));
 		return CL;
 	}
@@ -69,7 +92,7 @@ public static CommandList notBoilerRed(){
 		CommandList CL = new CommandList();
 		CL.addCommand(new DriveCommand(new DistanceStopCondition(encoders, distanceFromWall), speed));
 		CL.addCommand(new TurnCommand(new AngleStopCondition(turnAngle,turnTolerance,toleranceTime)));
-		CL.addCommand(new ContinuousPegVisionCommand(1));
+		CL.addCommand(new ContinuousPegVisionCommand(1, 4000));
 		CL.addCommand(new DriveCommand(new TimerStopCondition(400), 1));
 		return CL;
 	}
@@ -83,7 +106,7 @@ public static CommandList centerBlue(){
 		encoders.add((Encoder) sensors.getSensor("ENCODER0"));
 		CommandList CL = new CommandList();
 		CL.addCommand(new WaitCommand(new TimerStopCondition(500)));
-		CL.addCommand(new ContinuousPegVisionCommand(1));
+		CL.addCommand(new ContinuousPegVisionCommand(1, 4000));
 		CL.addCommand(new DriveCommand(new TimerStopCondition(400), 1));
 		return CL;
 	}
@@ -100,7 +123,7 @@ public static CommandList boilerBlue(){
 		CommandList CL = new CommandList();
 		CL.addCommand(new DriveCommand(new DistanceStopCondition(encoders, distanceFromWall), speed));
 		CL.addCommand(new TurnCommand(new AngleStopCondition(turnAngle,turnTolerance,toleranceTime)));
-		CL.addCommand(new ContinuousPegVisionCommand(1));
+		CL.addCommand(new ContinuousPegVisionCommand(1, 4000));
 		CL.addCommand(new DriveCommand(new TimerStopCondition(400), 1));
 		return CL;
 	}
@@ -118,7 +141,7 @@ public static CommandList notBoilerBlue(){
 		CommandList CL = new CommandList();
 		CL.addCommand(new DriveCommand(new DistanceStopCondition(encoders, distanceFromWall), speed));
 		CL.addCommand(new TurnCommand(new AngleStopCondition(-turnAngle,turnTolerance,toleranceTime)));
-		CL.addCommand(new ContinuousPegVisionCommand(1));
+		CL.addCommand(new ContinuousPegVisionCommand(1, 4000));
 		CL.addCommand(new DriveCommand(new TimerStopCondition(400), 1));
 		return CL;
 	}

src/babbage/Climber.java

@@ -26,7 +26,7 @@ public void teleopInit() {
 	public void teleopPeriodic() {
 		turbo = controllers.climberHold();
 		if (turbo) {
-			climber.setSpeed(0.9);
+			climber.setSpeed(1);
 		}
 		else {
 			if (controllers.climberOn()) {