Auto works from center and to left scale using encoder! (state compet…

…ition)
Team4256 · Jun 9, 2018 · 4ce7686 · 4ce7686
1 parent bbddceb
commit 4ce7686
Show file tree

Hide file tree

Showing 9 changed files with 38 additions and 16 deletions.
diff --git a/src/com/cyborgcats/reusable/Autonomous/O_Encoder.java b/src/com/cyborgcats/reusable/Autonomous/O_Encoder.java
@@ -7,7 +7,7 @@
 public class O_Encoder extends Odometer {
 	private R_SwerveModule module;
 	private R_Gyro gyro;
-	private double tareX = 0.0, tareY = 0.0;
+//	private double tareX = 0.0, tareY = 0.0;
 	private ConsumableDouble x = new ConsumableDouble();
 	private ConsumableDouble y = new ConsumableDouble();
 
@@ -24,13 +24,13 @@ public O_Encoder(final R_SwerveModule module, final R_Gyro gyro) {
 	public void init() {}//unused
 
 	@Override
-	public void setOrigin(final double x, final double y) {tareX = x;	tareY = y;}
+	public void setCurrent(final double x, final double y) {this.x.set(x);	this.y.set(y);;}
 
 	@Override
-	public double getX(final boolean markAsRead) {return x.get(markAsRead) - tareX;}
+	public double getX(final boolean markAsRead) {return x.get(markAsRead)/* - tareX*/;}
 
 	@Override
-	public double getY(final boolean markAsRead) {return y.get(markAsRead) - tareY;}
+	public double getY(final boolean markAsRead) {return y.get(markAsRead)/* - tareY*/;}
 
 	@Override
 	public boolean newX() {return x.isNew();}

diff --git a/src/com/cyborgcats/reusable/Autonomous/O_ZED.java b/src/com/cyborgcats/reusable/Autonomous/O_ZED.java
@@ -25,7 +25,7 @@ public void init() {
 		position.addEntryListener("Y", (position, key, entry, value, flags) -> {this.onUpdatedY(value.getDouble());}, EntryListenerFlags.kUpdate);
 	}
 	@Override
-	public void setOrigin(final double x, final double y) {tareX = x;	tareY = y;}
+	public void setCurrent(final double x, final double y) {tareX = getX(false) - x;	tareY = getY(false) - y;}
 	@Override
 	public double getX(final boolean markAsRead) {return x.get(markAsRead) - tareX;}
 	@Override

diff --git a/src/com/cyborgcats/reusable/Autonomous/Odometer.java b/src/com/cyborgcats/reusable/Autonomous/Odometer.java
@@ -2,7 +2,7 @@
 
 public abstract class Odometer {
 	public abstract void init();
-	public abstract void setOrigin(final double x, final double y);
+	public abstract void setCurrent(final double x, final double y);
 
 	public abstract double getX(final boolean markAsRead);
 	public abstract double getY(final boolean markAsRead);

diff --git a/src/com/cyborgcats/reusable/Autonomous/V_Events.java b/src/com/cyborgcats/reusable/Autonomous/V_Events.java
@@ -5,6 +5,8 @@
 
 import com.cyborgcats.reusable.R_Gyro;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 public class V_Events {
 	private final Command[] commands;//list of actions
 	private final double[] triggers;//list of values where independentVariable triggers an action
@@ -30,6 +32,7 @@ public void check(final double independentVariable) {
 			step++;
 			if (step + 2 > triggers.length) doneRunning = true;
 		}
+		SmartDashboard.putNumber("autoStep", step);
 	}
 
 	public double execute(final R_Clamp clamp, final R_Combined elevator, final R_Gyro gyro) {

diff --git a/src/org/usfirst/frc/team4256/robot/Autonomous/A_ThreeScale.java b/src/org/usfirst/frc/team4256/robot/Autonomous/A_ThreeScale.java
@@ -10,6 +10,8 @@
 import com.cyborgcats.reusable.Autonomous.V_Events;
 import com.cyborgcats.reusable.Autonomous.V_Leash;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 import com.cyborgcats.reusable.Autonomous.Odometer;
 
 public class A_ThreeScale implements Autonomous{
@@ -49,6 +51,7 @@ public A_ThreeScale(final int startingPosition, final String gameData, final Odo
 
 	public void run(final R_Drivetrain swerve, final R_Clamp clamp, final R_Combined elevator) {
 		events.check(leash.getIndependentVariable());
+		SmartDashboard.putNumber("indVar", leash.getIndependentVariable());
   		final double spin = events.execute(clamp, elevator, swerve.gyro);
 
 		//run path processing only if odometer values are new
@@ -148,7 +151,7 @@ private V_Events useEvents_center() {
 			{1, Parameters.ElevatorPresets.SWITCH.height(), 0, 5}
 		};
 
-		events = new V_Events(V_Events.getFromArray(instructions), new double[] {0.1, 0.2, 1.0});
+		events = new V_Events(V_Events.getFromArray(instructions), new double[] {0.1, 0.2, 0.92});
 		return events;
 	}
 	//------------------------------------------------------------------------------------------

diff --git a/src/org/usfirst/frc/team4256/robot/Autonomous/Autonomous.java b/src/org/usfirst/frc/team4256/robot/Autonomous/Autonomous.java
@@ -10,7 +10,7 @@ public interface Autonomous {
 	//starting positions are measured from field origin to ZED's left camera
 	public static final double leftStartX = -127.54 - 9.0, centerStartX = 11.21 - 9.0, rightStartX = 106.94 - 9.0;
 	public static final double switchX = 51.0, cubeX = 65.0, scaleX = 74.11;
-	public static final double initY = 28.75 + 5.1, switchY = 120.0, cubeY = 210.0, scaleY = 276.0;
+	public static final double initY = 28.75, switchY = 120.0, cubeY = 210.0, scaleY = 276.0;
 
 	public enum StartingPosition {LEFT, CENTER, RIGHT};
 	public enum FieldPieceConfig {LEFT, RIGHT};

diff --git a/src/org/usfirst/frc/team4256/robot/R_Drivetrain.java b/src/org/usfirst/frc/team4256/robot/R_Drivetrain.java
@@ -36,8 +36,8 @@ public R_Drivetrain(final R_Gyro gyro, final R_SwerveModule moduleA, final R_Swe
 	**/
 	public void init() {
 		moduleA.init(/*reversed traction*/true);
-		moduleB.init(/*reversed traction*/true);
-		moduleC.init(/*reversed traction*/false);
+		moduleB.init(/*reversed traction*/false);
+		moduleC.init(/*reversed traction*/true);
 		moduleD.init(/*reversed traction*/false);
 	}
 
@@ -79,7 +79,10 @@ public void holonomic(final double direction, double speed, final double spin) {
 		if (!bad && isThere(6.0)) {
 			final double[] speeds_final = computeSpeeds(comps_desired);
 			for (int i = 0; i < 4; i++) modules[i].set(speeds_final[i]);//control traction if good and there
-		}else stop();//otherwise, stop traction
+		}else {
+			stop();//otherwise, stop traction
+			moduleD.tractionDeltaPathLength = 0.0;
+		}
 
 		if (spin < 0.07) moduleD.checkTractionEncoder();
 
@@ -106,7 +109,10 @@ public void holonomic_encoderIgnorant(final double direction, double speed, fina
 		if (!bad && isThere(10.0)) {
 			final double[] speeds_final = computeSpeeds(comps_desired);
 			for (int i = 0; i < 4; i++) modules[i].set(speeds_final[i]);//control traction if good and there
-		}else stop();//otherwise, stop traction
+		}else {
+			stop();//otherwise, stop traction
+			moduleD.tractionDeltaPathLength = 0.0;
+		}
 
 		if (spin < 0.07) moduleD.checkTractionEncoder();
 	}

diff --git a/src/org/usfirst/frc/team4256/robot/R_SwerveModule.java b/src/org/usfirst/frc/team4256/robot/R_SwerveModule.java
@@ -16,7 +16,7 @@ public class R_SwerveModule {
 	private final R_Talon traction;
 	private final boolean hasTractionSensor;
 	private double decapitated = 1.0;
-	private double tractionDeltaPathLength = 0.0;
+	public double tractionDeltaPathLength = 0.0;
 	private double tractionPreviousPathLength = 0.0;
 	private DigitalInput magnet;
 	private boolean aligned = true;
@@ -41,7 +41,7 @@ public void init(final boolean reversedTraction) {
 		rotation.init();
 
 		rotation.setNeutralMode(R_Talon.coast);
-		rotation.config_kP(0, 15.0, R_Talon.kTimeoutMS);
+		rotation.config_kP(0, 12.0, R_Talon.kTimeoutMS);
 		rotation.config_kI(0, 0.0, R_Talon.kTimeoutMS);
 		rotation.config_kD(0, 2.0, R_Talon.kTimeoutMS);
 
@@ -126,6 +126,13 @@ public void checkTractionEncoder() {
 			tractionPreviousPathLength = currentPathLength;
 		}
 	}
+	public void resetTractionEncoder() {
+		if (hasTractionSensor) {
+			tractionDeltaPathLength = 0.0;
+			tractionPreviousPathLength = 0.0;
+			traction.setSelectedSensorPosition(0, 0, 0);
+		}
+	}
 	/**
 	 * A shortcut to call completeLoopUpdate on all the Talons in the module.
 	**/

diff --git a/src/org/usfirst/frc/team4256/robot/Robot.java b/src/org/usfirst/frc/team4256/robot/Robot.java
@@ -95,7 +95,7 @@ public void robotInit() {
 
 		setupLogging(ds);
 
-		moduleA.setTareAngle(-78.0);moduleB.setTareAngle(-28.0);moduleC.setTareAngle(45.0);moduleD.setTareAngle(-77.0);
+		moduleA.setTareAngle(-45.0);moduleB.setTareAngle(0.0);moduleC.setTareAngle(6.0);moduleD.setTareAngle(48.0);
 		moduleA.setParentLogger(logger);moduleB.setParentLogger(logger);moduleC.setParentLogger(logger);moduleD.setParentLogger(logger);
 
 		elevatorOne.setZero(0.5);
@@ -132,7 +132,9 @@ public void autonomousInit() {
 
 		//{Robot Input}
 		zed.getEntry("Enable Odometry").setBoolean(false);//TODO if we want to switch back to ZED make sure to remove this line
-		odometer.setOrigin(odometer.getX(false) - autonomous.initX()/12.0, odometer.getY(false) - Autonomous.initY/12.0);
+		moduleD.resetTractionEncoder();
+		odometer.update();
+		odometer.setCurrent(autonomous.initX()/12.0, Autonomous.initY/12.0);
 
 		//{Robot Output}
 		swerve.autoMode(true);
@@ -277,6 +279,7 @@ public void disabledPeriodic() {
 		pollGameData();
 		driver.setRumble(RumbleType.kLeftRumble, 0.0); driver.setRumble(RumbleType.kRightRumble, 0.0);
 		gunner.setRumble(RumbleType.kLeftRumble, 0.0); gunner.setRumble(RumbleType.kRightRumble, 0.0);
+		moduleD.tractionDeltaPathLength = 0.0;
 	}