Merge pull request #107 from NicoletFEAR/Milwaukee

Milwaukee

Steamworks/src/org/usfirst/frc/team4786/robot/Robot.java

@@ -1,23 +1,18 @@
 package org.usfirst.frc.team4786.robot;
 
 import org.usfirst.frc.team4786.robot.commands.DoNothing;
-import org.usfirst.frc.team4786.robot.commands.DriveArcSpeed;
 import org.usfirst.frc.team4786.robot.commands.DriveToLeftGearPeg;
 import org.usfirst.frc.team4786.robot.commands.DriveToPosition;
 import org.usfirst.frc.team4786.robot.commands.DriveToRightGearPeg;
 import org.usfirst.frc.team4786.robot.commands.OpenLoopDrive;
 import org.usfirst.frc.team4786.robot.commands.SwitchFrontSide;
-import org.usfirst.frc.team4786.robot.commands.TurnToAngle;
 import org.usfirst.frc.team4786.robot.subsystems.Arduino;
 import org.usfirst.frc.team4786.robot.subsystems.Climber;
 import org.usfirst.frc.team4786.robot.subsystems.DrawBridge;
 import org.usfirst.frc.team4786.robot.subsystems.DriveTrain;
 import org.usfirst.frc.team4786.robot.subsystems.Gear;
-//import org.usfirst.frc.team4786.robot.subsystems.FrameData;
 import org.usfirst.frc.team4786.robot.subsystems.Intake;
 import org.usfirst.frc.team4786.robot.subsystems.SwitchState;
-//import org.usfirst.frc.team4786.robot.subsystems.Test;
-//import org.usfirst.frc.team4786.robot.subsystems.VisionImage;
 import org.usfirst.frc.team4786.robot.subsystems.VisionImage;
 
 import edu.wpi.cscore.CvSink;
@@ -82,7 +77,7 @@ public void robotInit() {
 
 		gearPlacementCamera.setResolution(RobotMap.cameraFOVWidth,RobotMap.cameraFOVHeight);
 		ballPlacementCamera.setResolution(RobotMap.cameraFOVWidth, RobotMap.cameraFOVHeight);
-		cvSink = CameraServer.getInstance().getVideo();
+		//cvSink = CameraServer.getInstance().getVideo();
 
 		frontSide = "Gear";
 
@@ -92,20 +87,18 @@ public void robotInit() {
 
 
 		sendableChooser = new SendableChooser<Command>();
-		sendableChooser.addDefault("Drive to Center Gear Peg", new DriveToPosition(7));
+		sendableChooser.addDefault("Drive to Center Gear Peg", new DriveToPosition(9.198-RobotMap.professorElementalRobotLength));
 		sendableChooser.addObject("Do Nothing!", new DoNothing());
 		sendableChooser.addObject("Drive to Baseline", new DriveToPosition(10));
 		sendableChooser.addObject("Drive to Left Gear Peg", new DriveToLeftGearPeg());
 		sendableChooser.addObject("Drive to Right Gear Peg", new DriveToRightGearPeg());
 		//sendableChooser.addObject("GetToGearTest", new GearFromOffset());
 		SmartDashboard.putData("Autonomous Selector", sendableChooser);
 
-		/*Command initial = new SwitchFrontSide();
-		initial.start();*/
 	}
 	@Override
 	public void disabledInit() {
-
+		
 	}
 
 	@Override
@@ -147,7 +140,7 @@ public void autonomousInit() {
 		SmartDashboard.putString("Alliance", allianceColorVal);
 
 
-	//camera.setExposureManual(RobotMap.exposure);
+	//gearPlacementCamera.setExposureManual(RobotMap.exposure);
     autonomousCommand = (Command) sendableChooser.getSelected();
 		if (autonomousCommand != null)
 			autonomousCommand.start();
@@ -166,9 +159,9 @@ public void autonomousInit() {
 	@Override
 	public void autonomousPeriodic() {
 		Scheduler.getInstance().run();
-		SmartDashboard.putNumber("Left Encoder Positon", driveTrain.getLeftEncoderPosition());
+		/*SmartDashboard.putNumber("Left Encoder Positon", driveTrain.getLeftEncoderPosition());
 		SmartDashboard.putNumber("Right Encoder Positon", driveTrain.getRightEncoderPosition());
-		SmartDashboard.putNumber("Servo Angle", drawBridge.getServoAngle());
+		SmartDashboard.putNumber("Servo Angle", drawBridge.getServoAngle());*/
 	}
 
 	@Override
@@ -185,26 +178,25 @@ public void teleopInit() {
 		if (autonomousCommand != null)
 			autonomousCommand.cancel();
 
-		teleopCommand = new OpenLoopDrive();
-
-		if(teleopCommand != null)
-			teleopCommand.start();
+		//TODO Test this
+		Command switchFront = new SwitchFrontSide();
+		switchFront.start();
 	}
 
 
 	@Override
 	public void teleopPeriodic() {
     switchState.switchChange();
 		Scheduler.getInstance().run();
-		timer = new Timer();
+		//timer = new Timer();
 
-		SmartDashboard.putNumber("Left Encoder Positon", driveTrain.getLeftEncoderPosition());
+		/*SmartDashboard.putNumber("Left Encoder Positon", driveTrain.getLeftEncoderPosition());
 		SmartDashboard.putNumber("Right Encoder Positon", driveTrain.getRightEncoderPosition());
 		SmartDashboard.putNumber("Left Motor Output", driveTrain.motorOutputLeft);
 		SmartDashboard.putNumber("Right Motor Output", driveTrain.motorOutputRight);
 		SmartDashboard.putBoolean("Peg Present", gear.pegLimitSwitchPressed());
 		SmartDashboard.putNumber("New State", SwitchState.newState);  //These two lines are dependent of the SwitchState method
-		SmartDashboard.putNumber("Old State", SwitchState.oldState);  // get rid of them if we don't have this method
+		SmartDashboard.putNumber("Old State", SwitchState.oldState);  // get rid of them if we don't have this method*/
 
 	}
 

Steamworks/src/org/usfirst/frc/team4786/robot/RobotMap.java

@@ -9,12 +9,14 @@
  */
 public class RobotMap {
 
+	public static final double professorElementalRobotLength = 2.708;
+
 	//CANTalon ports, use instead of random numbers
 
-	/*public static final int frontLeftPort = 14;
-	public static final int frontRightPort = 13;*/
+
 	public static final int frontLeftPort = 13;
 	public static final int frontRightPort = 14;
+
 
 	//Our CANTalon game mech ports
 	//Change these mech Talon IDs, do not know final CANTalon ids yet
@@ -29,8 +31,8 @@ public class RobotMap {
 	public static final double MESHBOT_ROBOT_LENGTH = 2.34375; //In feet
 
 	//General PID Constants
-	public static final int ERROR_CONSTANT_LEFT = 40; //In native units
-	public static final int ERROR_CONSTANT_RIGHT = 20;
+	public static final int ERROR_CONSTANT_LEFT = 100; //In native units
+	public static final int ERROR_CONSTANT_RIGHT = 100;
 	public static final int ALLOWABLE_TURN_ERROR = 1; //In degrees
 	public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 1024;
 	public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_RIGHT = 1024;
@@ -52,7 +54,7 @@ public class RobotMap {
 	public static final double INTAKE_COLLECT_SPEED_SCALING = -	1;
 	public static final double INTAKE_SPIT_SPEED_SCALING = 1;
 	//public static final double OPEN_LOOP_CLIMBING_SPEED = 0.5;
-	public static final double OPEN_LOOP_CLIMBING_SPEED_SCALING = -1;
+	public static final double OPEN_LOOP_CLIMBING_SPEED_SCALING = 1;
 	public static final double OPEN_BRIDGE_ANGLE = 90;
 	public static final double CLOSED_BRIDGE_ANGLE = -90;
 	//Left GearBox PID Constants
@@ -64,14 +66,14 @@ public class RobotMap {
 	 * If needed, add a D value to smooth it out
 	 */
 	//public static final double LeftP = 0.1;
-	public static final double LeftP = 0.004;
-	public static final double LeftI = 0.00015;
+	public static final double LeftP = 0.0001;
+	public static final double LeftI = 0.000010;
 	public static final double LeftD = 0.0;
 	public static final double LeftF = 0.0;
 	//Right GearBox PID Constants
 	//public static final double RightP = 0.1;
-	public static final double RightP = 0.004;
-	public static final double RightI = 0.00015;
+	public static final double RightP = 0.0001;
+	public static final double RightI = 0.000010;
 	public static final double RightD = 0.0;
 	public static final double RightF = 0.0;
 	//Left GearBox Velocity PID Constants

Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToLeftGearPeg.java

@@ -1,5 +1,7 @@
 package org.usfirst.frc.team4786.robot.commands;
 
+import org.usfirst.frc.team4786.robot.RobotMap;
+
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /**
@@ -14,11 +16,11 @@ public DriveToLeftGearPeg() {
     	addSequential(new DriveToPosition(1));*/
 
     	//initial drive straight in feet
-    	addSequential(new DriveToPosition(2));
+    	addSequential(new DriveToPosition(9.417- RobotMap.professorElementalRobotLength));
     	//turning degrees via .22 power ratio on inside of turn for .75 seconds
-    	addSequential(new DriveArcSpeed(1, 1 * 0.22, 0.75));
+    	addSequential(new TurnToAngle(-60));
     	//final drive straight to target
-    	addSequential(new DriveToPosition(3));
+    	addSequential(new DriveToPosition(1.5));
 
         // To run multiple commands at the same time,
         // use addParallel()

Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToRightGearPeg.java

@@ -1,5 +1,7 @@
 package org.usfirst.frc.team4786.robot.commands;
 
+import org.usfirst.frc.team4786.robot.RobotMap;
+
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /**
@@ -14,12 +16,11 @@ public DriveToRightGearPeg() {
 
 
     	//initial drive straight in feet
-    	addSequential(new DriveToPosition(2));
+    	addSequential(new DriveToPosition(9.417- RobotMap.professorElementalRobotLength));
     	//turning degrees via .22 power ratio on inside of turn for .75 seconds
-    	addSequential(new DriveArcSpeed(1 * 0.22, 1 , 0.75));
+    	addSequential(new TurnToAngle(60));
     	//final drive straight to target
-    	addSequential(new DriveToPosition(3));
-
+    	addSequential(new DriveToPosition(1.5));
         // Add Commands here:
         // e.g. addSequential(new Command1());
         //      addSequential(new Command2());

Steamworks/src/org/usfirst/frc/team4786/robot/commands/SwitchFrontSide.java

@@ -14,6 +14,7 @@ public SwitchFrontSide() {
         // Use requires() here to declare subsystem dependencies
         // eg. requires(chassis);
     	requires(Robot.driveTrain);
+    	this.setInterruptible(false);
     }
 
     // Called just before this Command runs the first time

Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/Climber.java

@@ -1,6 +1,5 @@
 package org.usfirst.frc.team4786.robot.subsystems;
 
-import org.usfirst.frc.team4786.robot.Robot;
 import org.usfirst.frc.team4786.robot.RobotMap;
 import org.usfirst.frc.team4786.robot.commands.OpenClimb;
 import com.ctre.CANTalon;
@@ -36,7 +35,8 @@ public void startOpenClimbing(double speed) {
 
     public boolean isFinishedClimbing(){
     	//The climbing limit switches are ported into DIO 3
-    	return (!Robot.oi.getClimberLimitSwitch().get());
+    	//return (!Robot.oi.getClimberLimitSwitch().get());
+    	return false;
     }
 
     public void stopClimbing() {

Steamworks/src/org/usfirst/frc/team4786/robot/subsystems/DriveTrain.java

@@ -46,6 +46,7 @@ public DriveTrain(){
 		frontRight.enable();
 
 		frontLeft.setInverted(true);
+		frontRight.setInverted(false);
 
 
 		//Beginning of the world of PID!!!!!!!!
@@ -66,7 +67,6 @@ public DriveTrain(){
 		try {
 			Thread.sleep(10);
 		} catch (InterruptedException e) {
-			// TODO Auto-generated catch block
 			e.printStackTrace();
 		}
 		frontLeft.configEncoderCodesPerRev(RobotMap.DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT);
@@ -147,7 +147,6 @@ public void driveToPositionInit(double distanceToDrive){
 		try {
 			Thread.sleep(10);
 		} catch (InterruptedException e) {
-			// TODO Auto-generated catch block
 			e.printStackTrace();
 		}
 		frontLeft.setEncPosition(0);
@@ -163,7 +162,6 @@ public void driveToPositionInit(double distanceToDrive){
 		try {
 			Thread.sleep(10);
 		} catch (InterruptedException e) {
-			// TODO Auto-generated catch block
 			e.printStackTrace();
 		}
 		//Make sure we inverse this right side,
@@ -187,7 +185,6 @@ public void driveArcInit(double horizontalDist, double theta){
 		try {
 			Thread.sleep(10);
 		} catch (InterruptedException e) {
-			// TODO Auto-generated catch block
 			e.printStackTrace();
 		}
 		frontLeft.setEncPosition(0);
@@ -226,7 +223,6 @@ public void driveArcSpeedInit(double leftSpeed, double rightSpeed){
 		try {
 			Thread.sleep(10);
 		} catch (InterruptedException e) {
-			// TODO Auto-generated catch block
 			e.printStackTrace();
 		}
 		frontLeft.setEncPosition(0);
@@ -345,6 +341,7 @@ private double convertToRotations(double distanceInFeet){
 	//Take a speed in feet per second, converts to
 	//native units per 100 milliseconds
 	//Possibly a non-needed method
+	@SuppressWarnings("unused")
 	private double convertToVelocity(double feetPerSecond){
 		double rotationsPerSecond = convertToRotations(feetPerSecond);
 		double temp = rotationsPerSecond / (RobotMap.DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT * 4);