summaryrefslogtreecommitdiff
path: root/src/org/usfirst/frc/team4272
diff options
context:
space:
mode:
Diffstat (limited to 'src/org/usfirst/frc/team4272')
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java31
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/HwRobot.java3
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);
}