summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java31
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Robot.java48
2 files changed, 65 insertions, 14 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 021a00a..60dcd24 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -65,7 +65,7 @@ public class Autonomous {
|| Modes: Define the high-level autonomous modes ||
\*====================================================================*/
- public static void robotInit() {
+ public static void networkInit() {
Command init = c->{
c.lDrive = c.rDrive = 0;
c.highGear = c.gedOut = false;
@@ -79,6 +79,12 @@ public class Autonomous {
driveDistance(robot, 0.4, 10),
stop,
});
+
+ modeChooser.addObject("Drive 7 seconds", robot->new Command[]{
+ init,
+ timeout(7.0, c->{c.lDrive = c.rDrive = 0.4; return false;}),
+ stop,
+ });
modeChooser.addObject("Center Peg", robot->new Command[]{
init,
timeout(6.0, driveDistance(robot, 0.4, 8.75)),
@@ -89,9 +95,9 @@ public class Autonomous {
});
modeChooser.addObject("Left Peg", robot->new Command[]{
init,
- driveDistance(robot, 0.4, 3.5),
+ driveDistance(robot, 0.5, 8.0),
turnDegrees(robot, 0.4, 60),
- driveDistance(robot, 0.4, 8.3),
+ driveDistance(robot, 0.4, 2.16),
stop,
});
modeChooser.addObject("Right Peg", robot->new Command[]{
@@ -158,18 +164,24 @@ public class Autonomous {
private static class DriveDistance implements Command {
private static final double pctTaper = 1.0/6;
- private static final double pctTolerance = 0.002;
+
+ private double
+ cntT = 0,
+ cntL = 0,
+ cntR = 0;
private final HwRobot robot;
private final double pctThrottle;
+ private final double pctTolerance;
private final double lSpeed, lDistTarget;
private final double rSpeed, rDistTarget;
private double lMin = 0, rMin = 0;
- public DriveDistance(HwRobot robot, double pctThrottle,
+ public DriveDistance(HwRobot robot, double pctThrottle, double pctTolerance,
double lSpeed, double lDistTarget,
double rSpeed, double rDistTarget) {
this.robot = robot;
this.pctThrottle = pctThrottle;
+ this.pctTolerance = pctTolerance;
this.lSpeed = lSpeed;
this.lDistTarget = lDistTarget;
this.rSpeed = rSpeed;
@@ -218,10 +230,13 @@ public class Autonomous {
c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);
/* Make sure the left and right sides are even */
+ cntT++;
if (Math.abs(lPct - rPct) > pctTolerance) {
if (lPct > rPct) {
+ cntL++;
c.lDrive *= pctThrottle;
} else {
+ cntR++;
c.rDrive *= pctThrottle;
}
}
@@ -232,6 +247,8 @@ public class Autonomous {
SmartDashboard.putNumber("rMin", rMin);
SmartDashboard.putNumber("lDrive", c.lDrive);
SmartDashboard.putNumber("rDrive", c.rDrive);
+ SmartDashboard.putNumber("lCnt", cntL/(double)cntT);
+ SmartDashboard.putNumber("rCnt", cntR/(double)cntT);
return lPct >= 1 && rPct >= 1;
}
}
@@ -259,12 +276,12 @@ public class Autonomous {
}
private static Command driveDistance(HwRobot robot, double speed, double dist) {
- return new DriveDistance(robot, 0.2,
+ return new DriveDistance(robot, 0.05, 0.001,
speed, dist,
speed, dist);
}
private static Command turnRadians(HwRobot robot, double speed, double rad) {
- return new DriveDistance(robot, 0.6,
+ return new DriveDistance(robot, 0.6, 0.002,
speed, Math.copySign(rad*robot.axleWidth/2, rad),
speed, Math.copySign(rad*robot.axleWidth/2, -rad));
}
diff --git a/src/org/usfirst/frc/team4272/robot2017/Robot.java b/src/org/usfirst/frc/team4272/robot2017/Robot.java
index b65ce35..851f3f1 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Robot.java
@@ -43,27 +43,43 @@ public class Robot extends IterativeRobot {
private Autonomous auto;
private Teleop teleop;
+ private boolean networkInitialized = false;
+ private void networkInit() {
+ Autonomous.networkInit();
+ }
+
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
public void robotInit() {
- Autonomous.robotInit();
}
public void autonomousInit() {
try {
+ if (!networkInitialized) {
+ networkInit();
+ networkInitialized = true;
+ }
auto = new Autonomous(robot);
- } catch (Exception e) {}
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
}
public void autonomousPeriodic() {
try {
robot.run(auto.run(control));
- } catch (Exception e) {}
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
}
public void teleopInit() {
try {
+ if (!networkInitialized) {
+ networkInit();
+ networkInitialized = true;
+ }
teleop = new Teleop(robot);
} catch (Exception e) {}
}
@@ -71,18 +87,36 @@ public class Robot extends IterativeRobot {
public void teleopPeriodic() {
try {
robot.run(teleop.run(control, oi));
- } catch (Exception e) {}
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
}
public void disabledInit() {
+ try {
+ if (!networkInitialized) {
+ networkInit();
+ networkInitialized = true;
+ }
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
}
public void disabledPeriodic() {
}
- /**
- * This function is called periodically during test mode
- */
+ public void testInit() {
+ try {
+ if (!networkInitialized) {
+ networkInit();
+ networkInitialized = true;
+ }
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
+ }
+
public void testPeriodic() {
}
}