Strafe auton

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

@@ -28,8 +28,7 @@ public class Constants {
 	public static final Solenoid IntakeSolenoid = new Solenoid(0);
 	public static final Solenoid IntakeSolenoidOpenClose = new Solenoid(1);
 
-	public static final Solenoid LeftLiftReleaseSolenoid = new Solenoid(2);
-	public static final Solenoid RightLiftReleaseSolenoid = new Solenoid(3);
+	public static final Solenoid LiftReleaseSolenoid = new Solenoid(2);
 
 	public static final boolean EnableDriveBrake = false;
 	public static final double DeadZoneLimit = 0.18;
@@ -80,6 +79,7 @@ public class Constants {
 	public static final boolean RightEncoderReversed = false;
 	public static final double EncoderPulsesPerRev = 360; //in degrees
 	public static final double WheelDiameter = 6.0; //in inches
+	public static final double DistancePerRev = (WheelDiameter*PI);
 	public static final double EncoderDistancePerPulse = 
 									(WheelDiameter*PI)/(EncoderPulsesPerRev);
 
@@ -121,12 +121,13 @@ public class Constants {
 	public static final double TriggerActiveThreshold = 0.5;
 
 	//Vision Constants
-	public static final double CAMERA_WIDTH = 640;
+	public static final double CAMERA_WIDTH = 320;
 	public static final double FIELD_OF_VIEW = 60; //Logitech C270
 	public static final double DEG_PER_PIXEL = FIELD_OF_VIEW/CAMERA_WIDTH;
 
 	//Elevator Positions
 	public static final double ElevatorPosCtrlThreshold = 1000;
+	public static final double PickupPosition           = 500;
 	public static final double LowCarryPosition  		= 5000;//estimated 3"
 	public static final double SwitchPosition    	    = 20000;//estimated 12"
 	public static final double LowScalePosition  		= 20000;//estimated 48"
@@ -137,15 +138,34 @@ public class Constants {
 	public static final double TargetDistanceThreshold = 6.0;
 
 	//Center Auto Constants
-	public static final double IntakeCubeDistance      = 1.5;
+	public static final double IntakeCubeDistance      = 1.25;
 	public static final double CenterTurnToSwitchSpeed = 0.5;
 	public static final double CenterTurnToSwitchAngle = 15.0;
 
+	public static final double CenterStrafeXDistanceLeft   = 66;
+	public static final double CenterStrafeXDistanceRight  = 28;
+	public static final double CenterStrafeXSpeed      = 0.75;
+
+	public static final double Center2ndStrafeXDistance = 28;
+	public static final double Center2ndStrafeXSpeed = 0.75;
+	public static final double Center2ndStrafeYDistance = 40;
+	public static final double Center2ndStrafeYSpeed = 0.25;
+
+	public static final double CenterStrafeYDistance   = 24;
+	public static final double CenterStrafeYSpeed      = 0.25;
+
+	public static final double CenterSwitchFwdDistance = 70;
 	public static final double CenterSwitchFwdSpeed    = 0.5;
-	public static final double CenterSwitchFwdDistance = 110;
 
 	public static final double CenterSwitchRwdSpeed    = -0.5;
-	public static final double CenterSwitchRwdDistance = -100;
+	public static final double CenterSwitchRwdDistance = -45;
+	public static final double CenterCrossSwitchSpeed  = 0.75;
+	public static final double CenterCrossSwitchDist   = 46;
+	public static final double CenterClearCubeZoneSpeed  = 0.5;
+	public static final double CenterClearCubeZoneDist   = 24;
+
+	public static final double CenterApproachCubeSpeed   = 0.3;
+	public static final double CenterApproachCubeDist    = 18;
 	//public static final double SideTurnToSwitchAngle = 90.0;
 
 	//Left & Right Auto Constants
@@ -163,12 +183,20 @@ public class Constants {
 
 	public static final double LeftRightCreepSwitchDist = 18;
 	public static final double LeftRightCreepScaleDist = 18;
-	public static final double LeftRightCrossCreepDistance = 18;
+	public static final double CrossScaleCreepDistance = 40;
 
-	public static final double LeftRightCrossScaleDistance = 200.0;
-	public static final double LeftRightCrossScaleSpeed = 0.75;
+	public static final double LeftRightCrossScaleYDistance = 200.0;
+	public static final double LeftRightCrossScaleYSpeed = 0.75;
+	public static final double LeftRightCrossScaleXDistance = 248.0;
+	public static final double LeftRightCloseScaleXDistance = 36.0;
+	public static final double LeftRightCrossScaleXSpeed = 0.75;
 
 	public static final double LeftRightCreepSpeed    = 0.3;
+
+	public static final double AlignCubeMaxDistance   = 18;
+	public static final double AlignCubeSpeed   = 0.3;
+
+	public static final double CubePixelThreshold = 50;
 
 
 }

src/org/usfirst/frc/team470/robot/DashboardState.java

@@ -14,8 +14,9 @@ public void updateSmartDashboard() {
 		SmartDashboard.putNumber("Right Wheel Distance", sensors.getRightWheelDistance());
 		SmartDashboard.putNumber("Average Wheel Distance", sensors.getMainAvgDistance());
 
-		//SmartDashboard.putNumber("BottomDistance", Drivetrain.getBottomSlideDistance());
-		//SmartDashboard.putNumber("TopDistance", Drivetrain.getTopSlideDistance());
+		SmartDashboard.putNumber("BottomDistance", Drivetrain.getBottomMotorDistance());
+		SmartDashboard.putNumber("TopDistance", Drivetrain.getTopMotorDistance());
+		SmartDashboard.putNumber("Average Strafe Motor Distance", Drivetrain.getStrafeAverageDistance());
 
 		SmartDashboard.putNumber("Elevator Position", Elevator.getElevatorPosition());
 		SmartDashboard.putNumber("Elevator Target Position", Elevator.commandedPosition);

src/org/usfirst/frc/team470/robot/Drivetrain.java

@@ -8,6 +8,7 @@
 
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Drivetrain {
 
@@ -52,6 +53,7 @@ public class Drivetrain {
 	public static boolean isMovingYDistance = false;
 	public static boolean isMovingXDistance = false;
 	public static boolean isTurning = false;
+	public static boolean isMovingToVisionTarget = false;
 
 	//Sensors
 	static SensorData sensors = new SensorData();
@@ -73,10 +75,19 @@ public Drivetrain() {
 		//Calibrate Gyro
 		gyro.calibrate();
 
+		//Configure strafe encoders
+		topSlideMotor.setSensorPhase(true);
+		bottomSlideMotor.setSensorPhase(true);
+
+		bottomSlideMotor.setSelectedSensorPosition(0, 0, 0);
+		topSlideMotor.setSelectedSensorPosition(0, 0, 0);
 	}
 
 	//Main drivetrain movement code
 	public void updateDrivetrainTeleop() {
+
+		System.out.println("Cube Distance" + PiMath.getCubeDistance());
+		System.out.println("Cube Angle" + PiMath.angleToCube());
 
 		isDriveStraight = isStraightButtonPressed();
 
@@ -129,27 +140,44 @@ public void updateDrivetrainTeleop() {
 	public void updateDrivetrainAuton(){
 		double y_distance_error;
 		double x_distance_error;
+		double pixel_error;
 
 		if((isMovingYDistance)||
 		   (isMovingXDistance)){
 			//Move distance without tracking vision target
 			y_distance_error = yTargetDistance - sensors.getMainAvgDistance();
-			//x_distance_error = xTargetDistance - getTopSlideDistance();
+			x_distance_error = xTargetDistance - getStrafeAverageDistance();
 
 			if(Math.abs(y_distance_error) <= Constants.TargetDistanceThreshold){
 				//Robot has reached target
 				targetYThrottle = 0.0;
 				isMovingYDistance = false;
 			}
 
-			/*if(Math.abs(x_distance_error) <= Constants.TargetDistanceThreshold){
+			if(Math.abs(x_distance_error) <= Constants.TargetDistanceThreshold){
 				//Robot has reached target
 				targetXThrottle = 0.0;
 				isMovingXDistance = false;
-			}*/
+			}
 
 			targetTurn = -1*(gyro.getAngle()*Constants.GyroGain);
 		}
+		else if(isMovingToVisionTarget){
+			pixel_error = SmartDashboard.getNumber("PixelsToCube", 0);
+			x_distance_error = xTargetDistance - getStrafeAverageDistance();
+
+			if((Math.abs(pixel_error) <= Constants.CubePixelThreshold)||
+			   (Math.abs(x_distance_error) <= Constants.TargetDistanceThreshold)){
+				targetXThrottle = 0.0;
+				isMovingToVisionTarget = false;
+			}
+			else
+			{
+				targetXThrottle = pixel_error * -0.005;
+			}
+
+			targetTurn = -1*(gyro.getAngle()*Constants.GyroGain);
+		}
 		else if(isTurning)
 		{			
 			if(Math.abs(gyro.getAngle()) >= targetAngle)
@@ -175,6 +203,10 @@ else if(isTurning)
 
 		leftMasterMotor.set(ControlMode.PercentOutput, (Constants.LeftDriveReversed ? -1:1) * leftMotorCommand * driveGain);
 		rightMasterMotor.set(ControlMode.PercentOutput, (Constants.RightDriveReversed ? -1:1) * rightMotorCommand * driveGain);
+
+		//update slides
+		bottomSlideMotor.set(ControlMode.PercentOutput, bottomSlideMotorCommand);
+		topSlideMotor.set(ControlMode.PercentOutput, -topSlideMotorCommand);
 	}
 
 	//compute turn gain
@@ -341,11 +373,14 @@ public static double getTopSlideDistance() {
 
 	}*/
 
-	public static void setMoveDistance(double yDistance, double yThrottle){
+	public static void setMoveDistance(double yDistance, double yThrottle, double xDistance, double xThrottle){
 
 		sensors.resetEncoder();
+		topSlideMotor.setSelectedSensorPosition(0, 0, 0);
+		bottomSlideMotor.setSelectedSensorPosition(0, 0, 0);
 
 		yTargetDistance = yDistance;
+		xTargetDistance = xDistance;
 
 		enableDrivetrainDynamicBraking(true);
 
@@ -359,6 +394,17 @@ public static void setMoveDistance(double yDistance, double yThrottle){
     		isMovingYDistance = false;
     		targetYThrottle = 0.0;
     	}
+
+		if(Math.abs(xDistance) > Constants.TargetDistanceThreshold)
+		{
+    		isMovingXDistance = true;
+    		targetXThrottle = xThrottle;
+		}
+    	else
+    	{
+    		isMovingXDistance = false;
+    		targetXThrottle = 0.0;
+    	}
 	}
 
 	public static void setTurnToTarget(double turn, double angle){
@@ -367,19 +413,70 @@ public static void setTurnToTarget(double turn, double angle){
 		targetTurn = turn;//Turn power
 	}
 
+	public static void setAlignCube(double distance, double power){
+		sensors.resetEncoder();
+		topSlideMotor.setSelectedSensorPosition(0, 0, 0);
+		bottomSlideMotor.setSelectedSensorPosition(0, 0, 0);
+
+		targetTurn = 0.0;
+
+		boolean is_target_found = SmartDashboard.getBoolean("ContoursFound", false);
+		double pixels = SmartDashboard.getNumber("PixelsToCube", 0);
+
+		if((is_target_found) &&
+		   (Math.abs(pixels) > Constants.CubePixelThreshold))
+		{
+			isMovingToVisionTarget = true;
+
+			if(pixels > 0)
+			{
+				targetXThrottle = -power;
+				xTargetDistance = -distance;
+			}
+			else
+			{
+				targetXThrottle = power;
+				xTargetDistance = distance;
+			}	
+		}
+		else
+		{
+			isMovingToVisionTarget = false;
+			targetXThrottle = 0.0;
+		}
+	}
+
 	public static void enableDrivetrainDynamicBraking(boolean enable){
 		if(enable){
 			leftMasterMotor.setNeutralMode(NeutralMode.Brake);
 			leftSlaveMotor.setNeutralMode(NeutralMode.Brake);
 			rightMasterMotor.setNeutralMode(NeutralMode.Brake);
 			rightSlaveMotor.setNeutralMode(NeutralMode.Brake);
+			topSlideMotor.setNeutralMode(NeutralMode.Brake);
+			bottomSlideMotor.setNeutralMode(NeutralMode.Brake);
 		}
 		else{
 			leftMasterMotor.setNeutralMode(NeutralMode.Coast);
 			leftSlaveMotor.setNeutralMode(NeutralMode.Coast);
 			rightMasterMotor.setNeutralMode(NeutralMode.Coast);
 			rightSlaveMotor.setNeutralMode(NeutralMode.Coast);
-		}
+			topSlideMotor.setNeutralMode(NeutralMode.Coast);
+			bottomSlideMotor.setNeutralMode(NeutralMode.Coast);
+		}	
+	}
+
+	public static double getStrafeAverageDistance(){
+		double topdistance = getTopMotorDistance();
+		double bottomdistance = getBottomMotorDistance();
 
+		return((topdistance+bottomdistance)/2);
+	}
+
+	public static double getTopMotorDistance(){
+		return -1*((topSlideMotor.getSelectedSensorPosition(0)/4096)*Constants.DistancePerRev);
+	}
+
+	public static double getBottomMotorDistance(){
+		return (bottomSlideMotor.getSelectedSensorPosition(0)/4096)*Constants.DistancePerRev;
 	}
 }

src/org/usfirst/frc/team470/robot/Elevator.java

@@ -56,7 +56,12 @@ public void updateElevatorDisabled(){
 	}
 
 	public void updateElevatorAuton(){
-
+		//Monitor for upper or lower limit pressed
+		if((isLowerLimitPressed())||
+		   (isUpperLimitPressed()))
+		{
+			elevatorMotor.set(ControlMode.PercentOutput, 0.0);
+		}
 	}
 
 	public void updateElevatorTeleop(){
@@ -76,8 +81,8 @@ public void updateElevatorTeleop(){
 			isPositionControl = false;
 			elevatorMotor.set(ControlMode.PercentOutput, getElevatorInput());
 		}
-		else if ((!isElevatorZeroed)||
-				 (elevatorPosition < Constants.ElevatorPosCtrlThreshold)){ 
+		else if ((!isElevatorZeroed)/*||
+				 (elevatorPosition < Constants.ElevatorPosCtrlThreshold)*/){ 
 			/*Joystick released but relative position is unknown or close to lower limit 
 			  so can't move or hold relative position*/
 			isManualControl = true;
@@ -172,4 +177,8 @@ public void zeroElevatorPosition(){
 		isElevatorZeroed = true;
 		elevatorMotor.setSelectedSensorPosition(0, 0, 0);
 	}
+
+	public void setElevatorOutput(double output){
+		elevatorMotor.set(ControlMode.PercentOutput, output);
+	}
 }