diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 30 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/HwRobot.java | 6 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Teleop.java | 7 |
3 files changed, 33 insertions, 10 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index da7b8f8..5ce3aa5 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -31,18 +31,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Autonomous { private static interface Command { + /** Mutate the @{code Control} (possible because it is + * passed by reference), and return whether or not + * this command has finished yet. + * + * @param c The structure to control the robot + * @return Whether the command has finished. + */ public boolean execute(Control c); } private static class DriveDistance implements Command { private static final double pctTaper = 1.0/3; private static final double pctTolerance = 0.005; - private static final double distFinished = 0.1; private final HwRobot robot; private final double pctThrottle; private final double lSpeed, lDistTarget; private final double rSpeed, rDistTarget; + private double lMult = 1 , rMult = 1; public DriveDistance(HwRobot robot, double pctThrottle, double lSpeed, double lDistTarget, double rSpeed, double rDistTarget) { @@ -67,12 +74,26 @@ public class Autonomous { c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct); if (Math.abs(1-lPct) < pctTaper) { - c.lDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper)); + c.lDrive *= Math.abs((1-lPct)/pctTaper); + if ( Math.abs(robot.lRate.pidGet()) < 0.05) { + lMult += 0.05; + } + c.lDrive *= lMult; + if (c.lDrive > lSpeed) { + c.lDrive = lSpeed; + } } c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct); if (Math.abs(1-rPct) < pctTaper) { - c.rDrive *= Math.sqrt(Math.abs((1-rPct)/pctTaper)); + c.rDrive *= Math.abs((1-rPct)/pctTaper); + if ( Math.abs(robot.rRate.pidGet()) < 0.05) { + rMult += 0.05; + } + c.rDrive *= rMult; + if (c.rDrive > rSpeed) { + c.rDrive = rSpeed; + } } if (Math.abs(lPct - rPct) > pctTolerance) { @@ -82,7 +103,8 @@ public class Autonomous { c.rDrive *= pctThrottle; } } - return Math.abs(lDistCurr - lDistTarget) < distFinished && Math.abs(rDistCurr - rDistTarget) < distFinished; + + return lPct >= 1 && rPct >= 1; } } diff --git a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java index 091b68b..e3b2bd5 100644 --- a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java @@ -34,6 +34,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.Talon; @@ -42,6 +43,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter; import org.usfirst.frc.team4272.robotlib.PIDServo; +import org.usfirst.frc.team4272.robotlib.RollingAvg; public class HwRobot { /* Relay == a Spike */ @@ -71,6 +73,7 @@ public class HwRobot { camTilt = new PIDServo(9); public final Encoder lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false); public final Encoder rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true); + public final PIDSource lRate, rRate; public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */ public final DriverStation ds = DriverStation.getInstance(); @@ -88,6 +91,9 @@ public class HwRobot { lDriveE.setPIDSourceType(PIDSourceType.kRate); rDriveE.setPIDSourceType(PIDSourceType.kRate); //PDP.initTable(NetworkTable.getTable("PDP")); + + lRate = new RollingAvg(5, lDriveE); + rRate = new RollingAvg(5, rDriveE); } public void run(Control c) { diff --git a/src/org/usfirst/frc/team4272/robot2017/Teleop.java b/src/org/usfirst/frc/team4272/robot2017/Teleop.java index 254c530..103bd66 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2017/Teleop.java @@ -31,17 +31,14 @@ package org.usfirst.frc.team4272.robot2017; import edu.wpi.first.wpilibj.GenericHID.Hand; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc.team4272.robotlib.PushButton; -import org.usfirst.frc.team4272.robotlib.RollingAvg; public class Teleop { private final HwRobot robot; - private final PIDSource lRate, rRate; private final Timer timer = new Timer(); private final Preferences prefs = Preferences.getInstance(); @@ -51,8 +48,6 @@ public class Teleop { public Teleop(HwRobot robot) { this.robot = robot; - this.lRate = new RollingAvg(5, robot.lDriveE); - this.rRate = new RollingAvg(5, robot.rDriveE); timer.start(); } @@ -83,7 +78,7 @@ public class Teleop { } else if (oi.rStick.getTrigger()) { control.highGear = true; } else { - double speed = Math.abs((lRate.pidGet() + rRate.pidGet()) / 2); + double speed = Math.abs((robot.lRate.pidGet() + robot.rRate.pidGet()) / 2); if (!control.highGear) { if (speed > shiftUp) control.highGear = true; |