Merge pull request #40 from hammerhead226/michingtonak/states-changes

Minchingtonak/states changes

Author: minchingtonak

.classpath

@@ -13,6 +13,6 @@
 	<classpathentry kind="var" path="USERLIBS_DIR/navx_frc.jar"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/opencsv-4.1.jar"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/HammerheadLib-v1.0.jar"/>
-	<classpathentry kind="var" path="USERLIBS_DIR/SharkMacro-v1.1.2.jar"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/SharkMacro-v1.1.4.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

src/org/usfirst/frc/team226/robot/Constants.java

@@ -3,7 +3,7 @@
 public class Constants {
 
 	// Arm PID Tolerance
-	public static final int ARM_ERROR_TOLERANCE = 50;
+	public static final int ARM_ERROR_TOLERANCE = 10;
 
 	// Arm Soft Limits
 	public static final boolean ARM_FORWARD_LIMIT_ENABLED = false;
@@ -34,7 +34,7 @@ public class Constants {
 	public static final boolean CLIMBER_CURRENT_LIMIT_ENABLED = false;
 
 	// Invert Motors
-	public static final boolean INTAKE_INVERT_L = true;
+	public static final boolean INTAKE_INVERT_L = false;
 	public static final boolean INTAKE_INVERT_R = false;
 	public static final boolean ARM_INVERT_R = true;
 	public static final boolean ARM_INVERT_L = true;
@@ -57,7 +57,7 @@ public class Constants {
 	public static final double ARM_MAX_SPEED = 0.75;
 	public static final int CLIMBER_SENSOR_TIMEOUT = 10;
 	public static final double CLIMBER_SPEED = 0;
-	public static final double DT_VOLTAGE_RAMP_RATE = 0.3;
+	public static final double DT_VOLTAGE_RAMP_RATE = 0.1;
 
 	public static final double INTAKE_PULL_IN_SPEED = 0.5;
 

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

@@ -7,8 +7,15 @@
 
 package org.usfirst.frc.team226.robot;
 
-import org.usfirst.frc.team226.robot.auton.ExecuteDoubleMacro;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.hammerhead226.sharkmacro.Parser;
+import org.hammerhead226.sharkmacro.motionprofiles.ProfileParser;
+import org.usfirst.frc.team226.robot.auton.ExecuteChoiceMacro;
 import org.usfirst.frc.team226.robot.auton.ExecuteMacro;
+import org.usfirst.frc.team226.robot.auton.grp_ExecuteMacroList;
 import org.usfirst.frc.team226.robot.commands.PS_ShiftDriveTrainHighGear;
 import org.usfirst.frc.team226.robot.subsystems.Arm;
 import org.usfirst.frc.team226.robot.subsystems.Climber;
@@ -50,11 +57,29 @@ public void robotInit() {
 		intake = new Intake();
 		oi = new OI();
 		chooser.addDefault("Baseline Cross", new ExecuteMacro("baseline"));
-		chooser.addObject("Left Switch", new ExecuteDoubleMacro("left", "baseline"));
-		chooser.addObject("Center Switch", new ExecuteDoubleMacro("centerswitch_left", "centerswitch_right"));
-		chooser.addObject("Right Switch", new ExecuteDoubleMacro("baseline", "rightswitch_right"));
+		chooser.addObject("Left Switch", new ExecuteChoiceMacro("leftswitch_left", "baseline"));
+		chooser.addObject("Center Switch", new ExecuteChoiceMacro("centerswitch_left", "centerswitch_right"));
+		chooser.addObject("Right Switch", new ExecuteChoiceMacro("baseline", "rightswitch_right"));
+		
+		ArrayList<String> left = new ArrayList<String>();
+		left.add("centerswitch_left");
+		left.add("centerswitch_left_pickup_fast");
+		left.add("centerswitch_left_twocube");
+		ArrayList<String> right = new ArrayList<String>();
+		right.add("centerswitch_right");
+		right.add("nothing");
+		right.add("nothing");
+		chooser.addObject("Center switch 2 cube?", new grp_ExecuteMacroList(left, right));
+		
 		SmartDashboard.putData("Auto mode", chooser);
+
 		SmartDashboard.putData(new PS_ShiftDriveTrainHighGear());
+
+		ProfileParser.cache("centerswitch_left");
+		ProfileParser.cache("centerswitch_left_pickup_fast");
+		ProfileParser.cache("centerswitch_left_twocube");
+		ProfileParser.cache("centerswitch_right");
+		ProfileParser.cache("baseline");
 	}
 
 	@Override
@@ -78,13 +103,6 @@ public void autonomousInit() {
 		SmartDashboard.putString("Field State", DriverStation.getInstance().getGameSpecificMessage());
 		m_autonomousCommand = chooser.getSelected();
 
-		/*
-		 * String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
-		 * switch(autoSelected) { case "My Auto": autonomousCommand = new
-		 * MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new
-		 * ExampleCommand(); break; }
-		 */
-
 		// schedule the autonomous command (example)
 		if (m_autonomousCommand != null) {
 			m_autonomousCommand.start();

src/org/usfirst/frc/team226/robot/RobotMap.java

@@ -30,7 +30,7 @@ public class RobotMap {
 
 	public static final int CLIMBER_LEFT = 11;
 	public static final int CLIMBER_RIGHT = 15;
-	public static final int CLIMBER_SERVO = 1;
+	public static final int CLIMBER_SERVO = 0;
 
 	public static final int PCM_A = 11;
 	public static final int PCM_B = 12;

src/org/usfirst/frc/team226/robot/auton/ExecuteDoubleMacro.java renamed to src/org/usfirst/frc/team226/robot/auton/ExecuteChoiceMacro.java

@@ -15,7 +15,7 @@
 /**
  *
  */
-public class ExecuteDoubleMacro extends Command {
+public class ExecuteChoiceMacro extends Command {
 
 	private String leftName;
 	private String rightName;
@@ -24,7 +24,7 @@ public class ExecuteDoubleMacro extends Command {
 
 	private TalonSRX[] talons = Robot.driveTrain.getMotionProfileTalons();
 
-    public ExecuteDoubleMacro(String leftName, String rightName) {
+    public ExecuteChoiceMacro(String leftName, String rightName) {
         // Use requires() here to declare subsystem dependencies
         // eg. requires(chassis);
     	requires(Robot.driveTrain);
@@ -61,7 +61,7 @@ protected void execute() {
 
     // Make this return true when this Command no longer needs to run execute()
     protected boolean isFinished() {
-        return false;
+        return profileToExecute.isFinished() && actionListToExecute.isFinished();
     }
 
     // Called once after isFinished returns true

src/org/usfirst/frc/team226/robot/auton/grp_ExecuteMacroList.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc.team226.robot.auton;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class grp_ExecuteMacroList extends CommandGroup {
+
+	String voidMacro = "null";
+
+	public grp_ExecuteMacroList(List<String> leftNames, List<String> rightNames) {
+//		while(true) {
+//			if(leftNames.size() < rightNames.size()) {
+//				leftNames.add(voidMacro);
+//			}
+//			
+//			if(rightNames.size() < leftNames.size()) {
+//				rightNames.add(voidMacro);
+//			}
+//			
+//			if(leftNames.size() == rightNames.size()) {
+//				break;
+//			}
+//		}
+		
+		/*for (int i = 0; i < leftNames.size() || i < rightNames.size(); i++) {
+			addSequential(new ExecuteChoiceMacro(leftNames.get(i), rightNames.get(i)));
+			System.out.println(leftNames.get(i) + "   " + rightNames.get(i));
+		}*/
+		
+		for (int i = 0; i < leftNames.size(); i++) {
+			addSequential(new ExecuteChoiceMacro(leftNames.get(i), rightNames.get(i)));
+		}
+	}
+}

src/org/usfirst/frc/team226/robot/commands/C_UnlockClimber.java

@@ -2,39 +2,22 @@
 
 import org.usfirst.frc.team226.robot.Robot;
 
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.InstantCommand;
 
 /**
  *
  */
-public class C_UnlockClimber extends Command {
+public class C_UnlockClimber extends InstantCommand {
 
     public C_UnlockClimber() {
+    	super();
         // Use requires() here to declare subsystem dependencies
         // eg. requires(chassis);
     	requires(Robot.climber);
     }
 
     // Called just before this Command runs the first time
     protected void initialize() {
-    }
-
-    // Called repeatedly when this Command is scheduled to run
-    protected void execute() {
     	Robot.climber.activateClimber();
     }
-
-    // Make this return true when this Command no longer needs to run execute()
-    protected boolean isFinished() {
-        return false;
-    }
-
-    // Called once after isFinished returns true
-    protected void end() {
-    }
-
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
-    protected void interrupted() {
-    }
 }

src/org/usfirst/frc/team226/robot/subsystems/Arm.java

@@ -31,7 +31,7 @@ public Arm() {
 		right.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, Constants.ARM_PIDSLOT_IDX,
 				Constants.ARM_TIMEOUT);
 		right.setSensorPhase(Constants.ARM_SENSOR_PHASE);
-		
+
 		right.configContinuousCurrentLimit(Constants.ARM_CURRENT_LIMIT, Constants.ARM_TIMEOUT);
 		left.configContinuousCurrentLimit(Constants.ARM_CURRENT_LIMIT, Constants.ARM_TIMEOUT);
 
@@ -56,7 +56,7 @@ public Arm() {
 		right.configPeakOutputReverse(-Constants.ARM_MAX_SPEED, Constants.ARM_TIMEOUT);
 
 		left.follow(right);
-		
+
 		hardZeroEncoder();
 
 	}
@@ -85,7 +85,6 @@ public int getArmPos() {
 	}
 
 	public void hardZeroEncoder() {
-//		right.getSensorCollection().setPulseWidthPosition(0, Constants.ARM_TIMEOUT);
 		right.setSelectedSensorPosition(ArmSetpoint.STRAIGHT_UP.position, 0, Constants.ARM_TIMEOUT);
 		setArmSetpoint(ArmSetpoint.STRAIGHT_UP);
 	}
@@ -100,7 +99,7 @@ public void driveArm(double speed) {
 
 	public void controlArm(double speed) {
 		if (speed != 0) {
-			right.set(ControlMode.PercentOutput, /*softLimit(speed)*/speed);
+			right.set(ControlMode.PercentOutput, softLimit(speed));
 			setpointPosition = getArmPos();
 		} else {
 			right.set(ControlMode.Position, setpointPosition);
@@ -112,10 +111,6 @@ private double softLimit(double input) {
 			if (input > 0) {
 				input = 0;
 			}
-		} else if (getArmPos() <= ArmSetpoint.BACK_GROUND.position - Constants.ARM_ERROR_TOLERANCE) {
-			if (input < 0) {
-				input = 0;
-			}
 		}
 		return input;
 	}