diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Autonomous.java | 25 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/HwRobot.java | 33 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Teleop.java | 45 |
3 files changed, 79 insertions, 24 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; } diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java index 4a02076..e61d87f 100644 --- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java @@ -1,14 +1,17 @@ package org.usfirst.frc.team4272.robot2016; -import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter; +import edu.wpi.first.wpilibj.networktables.NetworkTable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PIDOutput; import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.Talon; +import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter; + public class HwRobot { /* Relay == a Spike */ /* PCM = Pneumatics Control Module */ @@ -29,7 +32,6 @@ public class HwRobot { // naming convention: {side_letter}{Thing}{E|M (encoder/motor)} - private final PIDOutput rDriveM1 = new Talon(/*PWM*/2), /* PDP 2, 3 */ rDriveM2 = new Talon(/*PWM*/4), /* PDP 4 */ @@ -41,33 +43,42 @@ public class HwRobot { fBallM = new PIDOutputSplitter(rfBallM, lfBallM), bBallM = new Talon(/*PWM*/6); /* PDP 10, 5 */ - public final Encoder armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2); + 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 lDriveE = new Encoder(/*DIO*/2,/*DIO*/3); - //public final Encoder rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true); + public final Encoder rDriveE = new Encoder(/*DIO*/5,/*DIO*/6); + public final Encoder lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true); + public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */ public HwRobot() { - //lDriveE.setPIDSourceType(PIDSourceType.kRate); - //rDriveE.setPIDSourceType(PIDSourceType.kRate); armE.setPIDSourceType(PIDSourceType.kDisplacement); + lDriveE.setPIDSourceType(PIDSourceType.kRate); + rDriveE.setPIDSourceType(PIDSourceType.kRate); + PDP.initTable(NetworkTable.getTable("PDP")); } public void run(Control c) { - if (armE.pidGet() < -295) - c.arm = 0; - lDriveM1.pidWrite( c.lDrive1); lDriveM2.pidWrite( c.lDrive2); rDriveM1.pidWrite(-c.rDrive1); rDriveM2.pidWrite(-c.rDrive2); - armM.pidWrite(c.arm); + armM.pidWrite(c.arm * 0.5); fBallM.pidWrite(-c.fBall); bBallM.pidWrite(-c.bBall); SmartDashboard.putNumber("fBall", c.fBall); SmartDashboard.putNumber("bBall", c.bBall); SmartDashboard.putNumber("armM", c.arm); + SmartDashboard.putNumber("armE", armE.pidGet()); + SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance()); + SmartDashboard.putNumber("lDriveE rate", lDriveE.getRate()); + SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance()); + SmartDashboard.putNumber("rDriveE rate", rDriveE.getRate()); + SmartDashboard.putBoolean("ballL", ballL.get()); + SmartDashboard.putBoolean("armL", armL.get()); + + PDP.updateTable(); + SmartDashboard.putData("PDP", PDP); } } diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java index cafbca1..c7029f4 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java @@ -1,14 +1,21 @@ package org.usfirst.frc.team4272.robot2016; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Timer; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; -import edu.wpi.first.wpilibj.Joystick; +import org.usfirst.frc.team4272.robotlib.PushButton; public class Teleop { private final HwRobot robot; + private final double armSoftStop = 95;//295; + + private final PushButton shootButton = new PushButton(); + private final Timer time = new Timer(); public Teleop(HwRobot robot) { this.robot = robot; + robot.armE.reset(); } private static double jsScale(Joystick j) { @@ -36,14 +43,38 @@ 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 < 0 ? 0.75*fBall : fBall, 1); + control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.85*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 (shootButton.update(oi.xbox.getButton(Button.X))) { + time.reset(); + time.start(); + } + if (oi.xbox.getButton(Button.X)) { + if (time.get() < 0.5) { + control.fBall = control.bBall = -0.5; + } else if (time.get() < 2.5) { + control.bBall = -0.5; + control.fBall = 1; + } else { + control.bBall = 1; + control.fBall = 1; + } + } + + //if (robot.armE.pidGet() > armSoftStop && control.arm > 0) + // control.arm = 0; /* Take pictures */ /* |