summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java30
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/HwRobot.java6
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Teleop.java7
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;