diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2015/HwRobot.java')
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2015/HwRobot.java | 46 |
1 files changed, 19 insertions, 27 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java index cab834e..0186565 100644 --- a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java @@ -7,7 +7,6 @@ import org.usfirst.frc.team4272.robotlib.LimitSwitchedPIDOutput; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PIDOutput; -import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSource.PIDSourceParameter; import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.Talon; @@ -30,44 +29,37 @@ public class HwRobot { * And, for communication: USB-B and Ethernet. */ - static class OutDumper implements PIDOutput { - private String p; - public OutDumper(String prefix) { p = prefix; } - public void pidWrite(double v) { - System.out.println(p+": "+v); - } - } - static class InDumper implements PIDSource { - private String p; - public final PIDSource r; - public InDumper(String prefix, PIDSource real) { p = prefix; r = real; } - public double pidGet() { - double v = r.pidGet(); - System.out.println(p+": "+v); - return v; - } - } public Encoder lDriveE = new Encoder(/*DIO*/2,/*DIO*/3); - public PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0), + private PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0), new Talon(/*PWM*/1)); public Encoder rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true); - public PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2), + private 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( + private static DigitalInput winchT = new DigitalInput(/*DIO*/6); + private static DigitalInput winchB = new DigitalInput(/*DIO*/7); + private PIDOutput winchM = new LimitSwitchedPIDOutput( new PIDOutputSplitter(new Talon(/*PWM*/4), new Talon(/*PWM*/5)), 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); + private DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3); + private DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1); + private Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth); + private Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth); public HwRobot() { lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); winchE.setPIDSourceParameter(PIDSourceParameter.kRate); } + + public void run(Control c) { + lDriveM.pidWrite( c.lDrive); + rDriveM.pidWrite(-c.rDrive); + winchM.pidWrite(-c.winch); + lIntake.set(c.lIntake); + rIntake.set(c.rIntake); + grab.setForward(c.grab); + push.setForward(!c.push); + } } |