diff options
Diffstat (limited to 'src/org')
3 files changed, 140 insertions, 48 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java index 22a7afc..cab834e 100644 --- a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java @@ -1,6 +1,5 @@ package org.usfirst.frc.team4272.robot2015; -import org.mckenzierobotics.lib.robot.PIDController; import org.mckenzierobotics.lib.robot.PIDOutputSplitter; import org.usfirst.frc.team4272.robotlib.DoubleSolenoid; import org.usfirst.frc.team4272.robotlib.LimitSwitchedPIDOutput; @@ -55,18 +54,17 @@ public class HwRobot { public PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2), new Talon(/*PWM*/3)); public Encoder winchE = new Encoder(/*DIO*/4,/*DIO*/5);//), + public static DigitalInput winchT = new DigitalInput(/*DIO*/6); + public static DigitalInput winchB = new DigitalInput(/*DIO*/7); public PIDOutput winchM = new LimitSwitchedPIDOutput( new PIDOutputSplitter(new Talon(/*PWM*/4), new Talon(/*PWM*/5)), - /*bottom*/new DigitalInput(/*DIO*/6), - /*top*/ new DigitalInput(/*DIO*/7), - true); + winchT, false, winchB, false); public DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3); public DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1); public Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth); public Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth); - public Relay lights = new Relay(/*Relay*/2, Relay.Direction.kForward); - + public HwRobot() { lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java index 95a47b7..f9569a6 100644 --- a/src/org/usfirst/frc/team4272/robot2015/Robot.java +++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java @@ -6,8 +6,16 @@ import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Relay; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.Timer; +import java.io.File; +import java.io.FileWriter; +import java.io.BufferedWriter; +import java.io.IOException; +import java.io.PrintWriter; /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the IterativeRobot @@ -18,19 +26,98 @@ import edu.wpi.first.wpilibj.Relay; public class Robot extends IterativeRobot { private HwRobot robot = new HwRobot(); private HwOI oi; + + private PrintWriter log; + + private class Control { + double lDrive, rDrive, winch; + Relay.Value lIntake, rIntake; + boolean grab, push; + public Control() { + lDrive = rDrive = winch = 0; + grab = push = false; + lIntake = rIntake = Relay.Value.kOff; + } + } + Control control = new Control(); + /** * This function is run when the robot is first started up and should be * used for any initialization code. */ public void robotInit() { - + try { + inLog(); + } catch (Exception e) {} + } + + private void inLog() throws IOException { + log = new PrintWriter(new FileWriter("/usr/local/frc/share/lukeshu.log", true)); + log.println("\ntime" + +",c:lDrive,c:rDrive,c:winch,c:lIntake,c:rIntake,c:grab,c:push" + +",i:lDrive.dist,i:lDrive.rate" + +",i:rDrive.dist,i:rDrive.rate" + +",i:winch.dist"); + } + + private void doLog() throws Exception { + log.println(0 + +","+control.lDrive + +","+control.rDrive + +","+control.winch + +","+fmtRelay(control.lIntake) + +","+fmtRelay(control.rIntake) + +","+control.grab + +","+control.push + +","+robot.lDriveE.getDistance() + +","+robot.lDriveE.getRate() + +","+robot.rDriveE.getDistance() + +","+robot.rDriveE.getRate() + +","+robot.winchE.getRate() + ); + } + + private String fmtRelay(Relay.Value v) { + if (v == Relay.Value.kForward) { + return "kForward"; + } else if (v == Relay.Value.kReverse) { + return "kReverse"; + } else if (v == Relay.Value.kOn) { + return "kOn"; + } else if (v == Relay.Value.kOff) { + return "kOff"; + } else { + return "unknown"; + } + } + + private void doRobot() { + try { + robot.lDriveM.pidWrite( control.lDrive); + robot.rDriveM.pidWrite(-control.rDrive); + robot.winchM.pidWrite(-control.winch); + robot.lIntake.set(control.lIntake); + robot.rIntake.set(control.rIntake); + robot.grab.setForward(control.grab); + robot.push.setForward(!control.push); + doLog(); + } catch (Exception e) {} } + private Timer autotime = new Timer(); public void autonomousInit() { - + autotime.reset(); + autotime.start(); + robot.rDriveE.reset(); + robot.lDriveE.reset(); } public void autonomousPeriodic() { - + if (/*autotime.get() < 4*/robot.rDriveE.getDistance() < (15.5*140*1.1)) { + control.rDrive = control.lDrive = -0.7; + } else { + control.rDrive = control.lDrive = 0; + } + doRobot(); } private Toggler grabButton = new Toggler(); @@ -50,41 +137,50 @@ public class Robot extends IterativeRobot { pushButton.update(false); ledsButton.update(false); } + + 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); + } + public void teleopPeriodic() { - /* Drive */ - robot.lDriveM.pidWrite(-oi.lStick.getY()*Math.abs(oi.lStick.getY())); - robot.rDriveM.pidWrite( oi.rStick.getY()*Math.abs(oi.rStick.getY())); - + /* This runs at 50Hz */ + + /* Drive */ + control.lDrive = jsScale(oi.lStick); + control.rDrive = jsScale(oi.rStick); /* Winch */ - double w = oi.xbox.getAxis(Axis.LY); - if (Math.abs(w) < 0.1) { w = 0; } - robot.winchM.pidWrite(-w); + control.winch = oi.xbox.getAxis(Axis.LY);/* up is neg, down is pos */ + if (Math.abs(control.winch) < 0.1) { control.winch = 0; } + + SmartDashboard.putNumber("winch M", -control.winch); + SmartDashboard.putNumber("winch E", robot.winchE.get()); + SmartDashboard.putBoolean("winch T", robot.winchT.get()); + SmartDashboard.putBoolean("winch B", robot.winchB.get()); /* left intake */ if (oi.xbox.getButton(Button.LB)) { - robot.lIntake.set(Relay.Value.kReverse); + control.lIntake = Relay.Value.kReverse; } else if (oi.xbox.getAxis(Axis.LT) > 0.75) { - robot.lIntake.set(Relay.Value.kForward); + control.lIntake = Relay.Value.kForward; } else { - robot.lIntake.set(Relay.Value.kOff); + control.lIntake = Relay.Value.kOff; } + /* right intake */ if (oi.xbox.getButton(Button.RB)) { - robot.rIntake.set(Relay.Value.kReverse); + control.rIntake=(Relay.Value.kReverse); } else if (oi.xbox.getAxis(Axis.RT) > 0.75) { - robot.rIntake.set(Relay.Value.kForward); + control.rIntake=(Relay.Value.kForward); } else { - robot.rIntake.set(Relay.Value.kOff); + control.rIntake=(Relay.Value.kOff); } - /* grab */ - robot.grab.setForward(grabButton.update(oi.xbox.getButton(Button.A))); - robot.push.setForward(pushButton.update(oi.xbox.getButton(Button.B))); - if (ledsButton.update(oi.xbox.getButton(Button.RT))) { - robot.lights.set(Relay.Value.kForward); - } else { - robot.lights.set(Relay.Value.kOff); - } + control.grab = grabButton.update(oi.xbox.getButton(Button.A)); + control.push = pushButton.update(oi.xbox.getButton(Button.B)); + + doRobot(); } public void disabledInit() { @@ -95,6 +191,10 @@ public class Robot extends IterativeRobot { robot.push.setEnabled(false); } + public void disabledPeriodic() { + + } + /** * This function is called periodically during test mode */ diff --git a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java index 73073c2..aa3aeaf 100644 --- a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java +++ b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java @@ -6,33 +6,27 @@ import edu.wpi.first.wpilibj.PIDOutput; public class LimitSwitchedPIDOutput implements PIDOutput { private final PIDOutput out; private final DigitalInput forward; - private final DigitalInput backward; - private final boolean i_dont_know_which_way_is_which; + private final DigitalInput bakward; + private final boolean for_pressed; + private final boolean bak_pressed; - public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward, boolean i_dont_know_which_way_is_which) { + public LimitSwitchedPIDOutput(PIDOutput out, + DigitalInput forward, boolean for_pressed, + DigitalInput backward, boolean back_pressed) { this.out = out; this.forward = forward; - this.backward = backward; - this.i_dont_know_which_way_is_which = i_dont_know_which_way_is_which; + this.bakward = backward; + this.for_pressed = for_pressed; + this.bak_pressed = back_pressed; } public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) { - this(out, forward,backward, false); + this(out, forward, true, backward, true); } public void pidWrite(double v) { - if (i_dont_know_which_way_is_which) { - if (forward.get() || backward.get()) { - v = 0; - } - } else { - if (forward.get()) { - v = Math.min(v, 0); - } - if (backward.get()) { - v = Math.max(v, 0); - } - } + if (forward.get() == for_pressed) { v = Math.min(v, 0); } + if (bakward.get() == bak_pressed) { v = Math.max(v, 0); } out.pidWrite(v); } } |