diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2017')
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 47 | ||||
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Control.java | 12 | ||||
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/HwOI.java | 10 | ||||
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/HwRobot.java | 87 | ||||
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Robot.java | 61 | ||||
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Teleop.java | 111 |
6 files changed, 328 insertions, 0 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java new file mode 100644 index 0000000..f3ce43f --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -0,0 +1,47 @@ +package org.usfirst.frc.team4272.robot2017; + +public class Autonomous { + private final HwRobot robot; + + public Autonomous(HwRobot robot) { + this.robot = robot; + robot.lDriveE.reset(); + robot.rDriveE.reset(); + } + + public Control run(Control c) { + double lDist = robot.lDriveE.getDistance(); + double rDist = robot.rDriveE.getDistance(); + + double speed = 0.6; + double thresh = 10; + double target = 15; + double max = 16; + + if (lDist < thresh) { + c.lDrive = speed; + } else if (lDist < max) { + c.lDrive = speed*Math.sqrt((target-lDist)/(target-thresh)); + } else { + c.lDrive = 0; + } + + if (rDist < thresh) { + c.rDrive = speed; + } else if (rDist < max) { + c.rDrive = speed*Math.sqrt((target-rDist)/(target-thresh)); + } else { + c.rDrive = 0; + } + + if (Math.abs(lDist - rDist) > 0.25) { + if (lDist > rDist) { + c.lDrive *= 0.2; + } else { + c.rDrive *= 0.2; + } + } + + return c; + } +} diff --git a/src/org/usfirst/frc/team4272/robot2017/Control.java b/src/org/usfirst/frc/team4272/robot2017/Control.java new file mode 100644 index 0000000..0039c72 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2017/Control.java @@ -0,0 +1,12 @@ +package org.usfirst.frc.team4272.robot2017; + +public class Control { + double lDrive = 0, + rDrive = 0, + camRotate = 0, + camTilt = 0, + climber = 0; + boolean highGear = false, + gedOut = false; + public Control() {} +} diff --git a/src/org/usfirst/frc/team4272/robot2017/HwOI.java b/src/org/usfirst/frc/team4272/robot2017/HwOI.java new file mode 100644 index 0000000..fcdfac6 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2017/HwOI.java @@ -0,0 +1,10 @@ +package org.usfirst.frc.team4272.robot2017; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; + +public class HwOI { + public Joystick lStick = new Joystick(0); + public Joystick rStick = new Joystick(1); + public XboxController xbox = new XboxController(2); +} diff --git a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java new file mode 100644 index 0000000..1cb1ee9 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java @@ -0,0 +1,87 @@ +package org.usfirst.frc.team4272.robot2017; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; +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 edu.wpi.first.wpilibj.networktables.NetworkTable; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter; +import org.usfirst.frc.team4272.robotlib.PIDServo; + +public class HwRobot { + /* Relay == a Spike */ + /* PCM = Pneumatics Control Module */ + + /* All of the numbered inputs are in the classes: + * - DIO: 0-9 + * - Relay: 0-3 + * - Analog In: 0-3 + * - PWM: 0-9 + * - PCM: 0-7 (the PCM is connected via CAN). + * - CAN + * + * For completeness, the roboRIO also has: i2c, RS-232, SPI, + * RSL, 2xUSB-A, an accelerometer, and an expansion port. + * + * And, for communication: USB-B and Ethernet. + */ + + // naming convention: {side_letter}{Thing}{E|M (encoder/motor)} + + private final PIDOutput + rDriveM = new Talon(/*PWM*/0), /* PDP 2, 3 */ + lDriveM = new Talon(/*PWM*/1), /* PDP 12, 13 */ + climber = new Talon(/*PWM*/3), /* PDP 4 */ + camRotate = new PIDServo(8), + camTilt = new PIDServo(9); + public final Encoder lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false); + public final Encoder rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true); + public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */ + + private double maxRate = 0; + + public DoubleSolenoid + shifter = new DoubleSolenoid(/*PCM*/4, /*PCM*/5), + ged = new DoubleSolenoid(/*PCM*/6, /*PCM*/7); + + public HwRobot() { + lDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/); + rDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/); + + lDriveE.setPIDSourceType(PIDSourceType.kRate); + rDriveE.setPIDSourceType(PIDSourceType.kRate); + //PDP.initTable(NetworkTable.getTable("PDP")); + } + + public void run(Control c) { + lDriveM.pidWrite(c.lDrive); + rDriveM.pidWrite(-c.rDrive); + climber.pidWrite(c.climber); + camRotate.pidWrite(c.camRotate); + camTilt.pidWrite(c.camTilt); + + shifter.set(c.highGear ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward); + ged.set(c.gedOut ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward); + + double rate = Math.abs((lDriveE.getRate()+rDriveE.getRate())/2); + SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance()); + SmartDashboard.putNumber("lDriveE rate", Math.abs(lDriveE.getRate())); + SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance()); + SmartDashboard.putNumber("rDriveE rate", Math.abs(rDriveE.getRate())); + SmartDashboard.putNumber("rate", rate); + if (rate > maxRate) + maxRate = rate; + SmartDashboard.putNumber("maxRate", maxRate); + + SmartDashboard.putBoolean("highGear", c.highGear); + SmartDashboard.putBoolean("gedOut", c.gedOut); + + //PDP.updateTable(); + //SmartDashboard.putData("PDP", PDP); + } +} diff --git a/src/org/usfirst/frc/team4272/robot2017/Robot.java b/src/org/usfirst/frc/team4272/robot2017/Robot.java new file mode 100644 index 0000000..afeba04 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2017/Robot.java @@ -0,0 +1,61 @@ + +package org.usfirst.frc.team4272.robot2017; + +import edu.wpi.first.wpilibj.IterativeRobot; + +/** + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the IterativeRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the manifest file in the resource + * directory. + */ +public class Robot extends IterativeRobot { + private final HwRobot robot = new HwRobot(); + private final HwOI oi = new HwOI(); + private final Control control = new Control(); + + private Autonomous auto; + private Teleop teleop; + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + public void robotInit() { + } + + public void autonomousInit() { + try { + auto = new Autonomous(robot); + } catch (Exception e) {} + } + public void autonomousPeriodic() { + try { + robot.run(auto.run(control)); + } catch (Exception e) {} + } + + public void teleopInit() { + try { + teleop = new Teleop(robot); + } catch (Exception e) {} + } + + public void teleopPeriodic() { + try { + robot.run(teleop.run(control, oi)); + } catch (Exception e) {} + } + + public void disabledInit() { + } + + public void disabledPeriodic() { + } + + /** + * This function is called periodically during test mode + */ + public void testPeriodic() { + } +} diff --git a/src/org/usfirst/frc/team4272/robot2017/Teleop.java b/src/org/usfirst/frc/team4272/robot2017/Teleop.java new file mode 100644 index 0000000..b7ff063 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2017/Teleop.java @@ -0,0 +1,111 @@ +package org.usfirst.frc.team4272.robot2017; + +import edu.wpi.first.wpilibj.GenericHID.Hand; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Preferences; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import org.usfirst.frc.team4272.robotlib.PushButton; +import org.usfirst.frc.team4272.robotlib.RollingAvg; + +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(); + private double lastTime; + private double currentTime; + private boolean state = false; + private PIDSource lRate, rRate; + private Preferences prefs = Preferences.getInstance(); + + public Teleop(HwRobot robot) { + this.robot = robot; + this.lRate = new RollingAvg(5, robot.lDriveE); + this.rRate = new RollingAvg(5, robot.rDriveE); + } + + private static double jsScale(Joystick j) { + double y = j.getY();/* +:forward; -:backward */ + double z = j.getZ();/* +:more-sensitive; -:less-sensitive */ + return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y); + } + + private static double bound(double min, double v, double max) { + return Math.min(Math.max(v, min), max); + } + + private static double oneIf(boolean b) { + return b ? 1 : 0; + } + + public Control run(Control control, HwOI oi) { + /* drive */ + control.lDrive = -jsScale(oi.lStick); + control.rDrive = -jsScale(oi.rStick); + + /* shifting */ + double shiftUp = prefs.getDouble("shiftUp", 3.3*3.28); + double shiftDown = prefs.getDouble("shiftDown", 2.7*3.28); + if (oi.lStick.getTrigger()) { + control.highGear = false; + } else if (oi.rStick.getTrigger()) { + control.highGear = true; + } else { + double speed = Math.abs((lRate.pidGet() + rRate.pidGet()) / 2); + if (!control.highGear) { + if (speed > shiftUp) + control.highGear = true; + } else { + if (speed < shiftDown) + control.highGear = false; + } + } + + /* auto gear wiggle */ + if (oi.rStick.getRawButton(2) || oi.lStick.getRawButton(2)) { + currentTime = Timer.getMatchTime(); + if (currentTime-lastTime > 0.15) { + state = !state; + lastTime = currentTime; + } + if (state) { + control.lDrive = 0.5; + control.rDrive = 0; + } else { + control.lDrive = 0; + control.rDrive = 0.5; + } + } + + /* climber */ + if (Math.abs(oi.xbox.getY(Hand.kLeft)) > 0.2) { + control.climber = oi.xbox.getY(Hand.kLeft); + } else { + control.climber = 0; + } + + /* gear place */ + if (oi.xbox.getAButton()) { + control.gedOut = true; + } + if (oi.xbox.getBButton()) { + control.gedOut = false; + } + + /* camera */ + if (oi.xbox.getTriggerAxis(Hand.kRight) > 0) { + control.camRotate = 1; + } else if (oi.xbox.getTriggerAxis(Hand.kLeft) > 0) { + control.camRotate = -1; + } else { + control.camRotate = 0; + } + control.camTilt = oi.xbox.getX(Hand.kRight); + + return control; + } +} |