diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/Autonomous.java')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Autonomous.java | 25 |
1 files changed, 19 insertions, 6 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java index 0a7452c..756c33d 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java @@ -1,19 +1,32 @@ package org.usfirst.frc.team4272.robot2016; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class Autonomous { + private final Timer time = new Timer(); private final HwRobot robot; public Autonomous(HwRobot robot) { this.robot = robot; + time.reset(); + time.start(); } public Control run(Control c) { - //if (!robot.armL.get()) { - // c.arm = -0.2; - //} else { - // c.arm = 0; - // robot.armE.reset(); - //} + if (robot.armL.get()) { + c.arm = -0.5; + } else { + c.arm = 0; + robot.armE.reset(); + } + + if (time.get() < 4) { + c.lDrive1 = c.rDrive1 = -0.4; + } else { + c.lDrive1 = c.rDrive1 = 0; + } + SmartDashboard.putNumber("autoTime", time.get()); return c; } |