From 31a99043d50cae624c956a8a8f80146a1c430d16 Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Tue, 4 Apr 2017 16:20:55 -0400 Subject: stuff --- .../usfirst/frc/team4272/robot2017/Autonomous.java | 31 ++++++++++---- src/org/usfirst/frc/team4272/robot2017/Robot.java | 48 ++++++++++++++++++---- 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() { } } -- cgit v1.2.3