diff options
Diffstat (limited to 'src/org/usfirst')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 31 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/HwRobot.java | 3 |
2 files changed, 24 insertions, 10 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index d012a98..038dd5a 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -27,6 +27,8 @@ */ package org.usfirst.frc.team4272.robot2017; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class Autonomous { private static interface Command { public boolean execute(Control c); @@ -65,23 +67,29 @@ public class Autonomous { double lPct = lDistCurr/lDistTarget; double rPct = rDistCurr/rDistTarget; - c.lDrive = Math.copySign(lSpeed, 1-lPct); + 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.rDrive = Math.copySign(rSpeed, 1-rPct); + c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct); if (Math.abs(1-rPct) < pctTaper) { - c.rDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper)); + c.rDrive *= Math.sqrt(Math.abs((1-rPct)/pctTaper)); } if (Math.abs(lPct - rPct) > pctTolerance) { if (lPct > rPct) { - c.lDrive *= pctThrottle; + c.lDrive *= 0.6; + //c.rDrive *= 2; } else { - c.rDrive *= pctThrottle; + //c.lDrive *= 2; + c.rDrive *= 0.6; } } + SmartDashboard.putNumber("lPct", lPct); + SmartDashboard.putNumber("rPct", rPct); + SmartDashboard.putNumber("lTarget", lDistTarget); + SmartDashboard.putNumber("rTarget", rDistTarget); return Math.abs(lDistCurr - lDistTarget) < distFinished && Math.abs(rDistCurr - rDistTarget) < distFinished; } } @@ -92,8 +100,8 @@ public class Autonomous { } private static Command turnRadians(HwRobot robot, double speed, double rad) { return new DriveDistance(robot, - speed, Math.copySign(rad*axleWidth, rad), - speed, Math.copySign(rad*axleWidth, -rad)); + speed, Math.copySign(rad*axleWidth/2, rad), + speed, Math.copySign(rad*axleWidth/2, -rad)); } private static Command turnDegrees(HwRobot robot, double speed, double deg) { return turnRadians(robot, speed, deg * Math.PI/180); @@ -115,20 +123,23 @@ public class Autonomous { // Left peg commands = new Command[]{ - driveDistance(robot, 0.3, 3.5), - turnDegrees(robot, 0.3, 60), - driveDistance(robot, 0.3, 8.3), + //driveDistance(robot, 0.3, 3.5), + //turnDegrees(robot, 0.3, 60), + //driveDistance(robot, 0.3, 8.3), + turnDegrees(robot, 0.5, 90), }; } public Control run(Control c) { c.gedOut = false; + c.highGear = false; if (step < commands.length) { if (commands[step].execute(c)) step++; } + SmartDashboard.putNumber("step", step); return c; } } diff --git a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java index 091b68b..7608376 100644 --- a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java @@ -119,6 +119,9 @@ public class HwRobot { SmartDashboard.putBoolean("highGear", c.highGear); SmartDashboard.putBoolean("gedOut", c.gedOut); + SmartDashboard.putNumber("lDrive", c.lDrive); + SmartDashboard.putNumber("rDrive", c.rDrive); + //PDP.updateTable(); //SmartDashboard.putData("PDP", PDP); } |