diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Autonomous.java | 12 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/HwRobot.java | 19 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Teleop.java | 22 |
3 files changed, 33 insertions, 20 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java index 0a7452c..1d61448 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java @@ -8,12 +8,12 @@ public class Autonomous { } 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.2; + } else { + c.arm = 0; + robot.armE.reset(); + } return c; } diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java index ebc4efc..7d63f86 100644 --- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java @@ -29,27 +29,25 @@ public class HwRobot { // naming convention: {side_letter}{Thing}{E|M (encoder/motor)} - //public final Encoder lDriveE = new Encoder(/*DIO*/2,/*DIO*/3); - //public final Encoder rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true); private final PIDOutput lDriveM1 = new Talon(/*PWM*/1), lDriveM2 = new Talon(/*PWM*/0); private final PIDOutput rDriveM1 = new Talon(/*PWM*/2), rDriveM2 = new Talon(/*PWM*/4); private final PIDOutput armM = new Talon(/*PWM*/8); - public final Encoder armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2); private final PIDOutput fBallM = new PIDOutputSplitter(new Talon(/*PWM*/9), new Talon(/*PWM*/3)); private final PIDOutput bBallM = new PIDOutputSplitter(new Talon(/*PWM*/6), new Talon(/*PWM*/7)); + + public final Encoder armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2, true); public final DigitalInput armL = new DigitalInput(/*DIO*/3); public final DigitalInput ballL = new DigitalInput(/*DIO*/4); + public final Encoder rDriveE = new Encoder(/*DIO*/5,/*DIO*/6); + public final Encoder lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true); public HwRobot() { - //lDriveE.setPIDSourceType(PIDSourceType.kRate); - //rDriveE.setPIDSourceType(PIDSourceType.kRate); armE.setPIDSourceType(PIDSourceType.kDisplacement); + lDriveE.setPIDSourceType(PIDSourceType.kRate); + rDriveE.setPIDSourceType(PIDSourceType.kRate); } public void run(Control c) { - if (armE.pidGet() < -295) - c.arm = 0; - lDriveM1.pidWrite( c.lDrive1); lDriveM2.pidWrite( c.lDrive2); rDriveM1.pidWrite(-c.rDrive1); @@ -61,6 +59,11 @@ public class HwRobot { SmartDashboard.putNumber("fBall", c.fBall); SmartDashboard.putNumber("bBall", c.bBall); SmartDashboard.putNumber("armM", c.arm); + SmartDashboard.putNumber("armE", armE.pidGet()); + SmartDashboard.putNumber("lDriveE", lDriveE.pidGet()); + SmartDashboard.putNumber("rDriveE", rDriveE.pidGet()); + SmartDashboard.putBoolean("ballL", ballL.get()); + SmartDashboard.putBoolean("armL", armL.get()); } } diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java index cafbca1..19dfc6e 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java @@ -6,9 +6,11 @@ import edu.wpi.first.wpilibj.Joystick; public class Teleop { private final HwRobot robot; + private final double armSoftStop = 95;//295; public Teleop(HwRobot robot) { this.robot = robot; + robot.armE.reset(); } private static double jsScale(Joystick j) { @@ -36,14 +38,22 @@ public class Teleop { double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger); double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B)); - boolean ballL = false; //robot.ballL.get(); - control.fBall = bound(-oneIf(!ballL), fBall, 1); - control.bBall = bound(-oneIf(!ballL), bBall < 0 ? 0.75*bBall : bBall, 1); + boolean ballL = robot.ballL.get(); + control.fBall = bound(-oneIf(ballL), fBall, 1); + control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.75*bBall : bBall, 1); /* I'm eyeballing 10 degrees ≈ 50 clicks */ - //if (robot.armE.pidGet() < 50 && robot.armL.get()) { - // robot.armE.reset(); - //} + if (robot.armE.pidGet() < 50 && !robot.armL.get()) { + robot.armE.reset(); + } + + if (oi.xbox.getButton(Button.LBumper)) + control.arm = 1-(robot.armE.pidGet()/armSoftStop); + else if (oi.xbox.getButton(Button.RBumper)) + control.arm = robot.armE.pidGet()/armSoftStop; + + //if (robot.armE.pidGet() > armSoftStop && control.arm > 0) + // control.arm = 0; /* Take pictures */ /* |