summaryrefslogtreecommitdiff
path: root/src/org
diff options
context:
space:
mode:
Diffstat (limited to 'src/org')
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/Autonomous.java25
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/Control.java15
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/HwRobot.java46
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/Robot.java263
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/Teleop.java54
5 files changed, 196 insertions, 207 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2015/Autonomous.java b/src/org/usfirst/frc/team4272/robot2015/Autonomous.java
new file mode 100644
index 0000000..5d48588
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/Autonomous.java
@@ -0,0 +1,25 @@
+package org.usfirst.frc.team4272.robot2015;
+
+import edu.wpi.first.wpilibj.Timer;
+
+public class Autonomous {
+ private final Timer time = new Timer();
+ private final HwRobot robot;
+
+ public Autonomous(HwRobot robot) {
+ this.robot = robot;
+ time.reset();
+ time.start();
+ robot.rDriveE.reset();
+ robot.lDriveE.reset();
+ }
+
+ public Control run(Control c) {
+ if (robot.rDriveE.getDistance() < (15.5*140*1.1)) {
+ c.rDrive = c.lDrive = -0.7;
+ } else {
+ c.rDrive = c.lDrive = 0;
+ }
+ return c;
+ }
+}
diff --git a/src/org/usfirst/frc/team4272/robot2015/Control.java b/src/org/usfirst/frc/team4272/robot2015/Control.java
new file mode 100644
index 0000000..ef5ae8e
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/Control.java
@@ -0,0 +1,15 @@
+package org.usfirst.frc.team4272.robot2015;
+
+import edu.wpi.first.wpilibj.Relay;
+
+public 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;
+ }
+}
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);
+ }
}
diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java
index f9569a6..22abd30 100644
--- a/src/org/usfirst/frc/team4272/robot2015/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java
@@ -1,19 +1,11 @@
package org.usfirst.frc.team4272.robot2015;
-import org.usfirst.frc.team4272.robotlib.Toggler;
-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;
/**
@@ -24,182 +16,93 @@ import java.io.PrintWriter;
* directory.
*/
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;
+ private final HwRobot robot = new HwRobot();
+ private final HwOI oi = new HwOI();
+ private final Control control = new Control();
+ private PrintWriter log;
+
+ 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() {
+ 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";
}
}
- 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();
- }
+ public void autonomousInit() {
+ auto = new Autonomous(robot);
+ }
+ public void autonomousPeriodic() {
+ try {
+ robot.run(auto.run(control));
+ doLog();
+ } catch (Exception e) {}
+ }
+
+ public void teleopInit() {
+ teleop = new Teleop();
+ }
+
+ public void teleopPeriodic() {
+ try {
+ robot.run(teleop.run(control, oi));
+ doLog();
+ } catch (Exception e) {}
+ }
- private Toggler grabButton = new Toggler();
- private Toggler pushButton = new Toggler();
- private Toggler ledsButton = new Toggler();
- public void teleopInit() {
- //robot.lDrive.reset();
- //robot.rDrive.reset();
- //robot.winch.reset();
- //robot.lDrive.enable();
- //robot.rDrive.enable();
- //robot.winch.enable();
- robot.grab.setEnabled(true);
- robot.push.setEnabled(true);
- oi = new HwOI();
- grabButton.update(false);
- 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() {
- /* This runs at 50Hz */
+ public void disabledInit() {
+ }
- /* Drive */
- control.lDrive = jsScale(oi.lStick);
- control.rDrive = jsScale(oi.rStick);
- /* Winch */
- 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)) {
- control.lIntake = Relay.Value.kReverse;
- } else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
- control.lIntake = Relay.Value.kForward;
- } else {
- control.lIntake = Relay.Value.kOff;
- }
-
- /* right intake */
- if (oi.xbox.getButton(Button.RB)) {
- control.rIntake=(Relay.Value.kReverse);
- } else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
- control.rIntake=(Relay.Value.kForward);
- } else {
- control.rIntake=(Relay.Value.kOff);
- }
-
- control.grab = grabButton.update(oi.xbox.getButton(Button.A));
- control.push = pushButton.update(oi.xbox.getButton(Button.B));
+ public void disabledPeriodic() {
+ }
- doRobot();
- }
-
- public void disabledInit() {
- //robot.lDrive.reset();
- //robot.rDrive.reset();
- //robot.winch.reset();
- robot.grab.setEnabled(false);
- robot.push.setEnabled(false);
- }
-
- public void disabledPeriodic() {
-
- }
-
- /**
- * This function is called periodically during test mode
- */
- public void testPeriodic() {
-
- }
-
+ /**
+ * This function is called periodically during test mode
+ */
+ public void testPeriodic() {
+ }
}
diff --git a/src/org/usfirst/frc/team4272/robot2015/Teleop.java b/src/org/usfirst/frc/team4272/robot2015/Teleop.java
new file mode 100644
index 0000000..541099c
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/Teleop.java
@@ -0,0 +1,54 @@
+package org.usfirst.frc.team4272.robot2015;
+
+import org.usfirst.frc.team4272.robotlib.Toggler;
+import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
+import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Relay;
+
+public class Teleop {
+ private Toggler grabButton = new Toggler();
+ private Toggler pushButton = new Toggler();
+
+ public Teleop() {
+ }
+
+ 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 Control run(Control control, HwOI oi) {
+ /* Drive */
+ control.lDrive = jsScale(oi.lStick);
+ control.rDrive = jsScale(oi.rStick);
+ /* Winch */
+ control.winch = oi.xbox.getAxis(Axis.LY);/* up is neg, down is pos */
+ if (Math.abs(control.winch) < 0.1) { control.winch = 0; }
+
+ /* left intake */
+ if (oi.xbox.getButton(Button.LB)) {
+ control.lIntake = Relay.Value.kReverse;
+ } else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
+ control.lIntake = Relay.Value.kForward;
+ } else {
+ control.lIntake = Relay.Value.kOff;
+ }
+
+ /* right intake */
+ if (oi.xbox.getButton(Button.RB)) {
+ control.rIntake=(Relay.Value.kReverse);
+ } else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
+ control.rIntake=(Relay.Value.kForward);
+ } else {
+ control.rIntake=(Relay.Value.kOff);
+ }
+
+ control.grab = grabButton.update(oi.xbox.getButton(Button.A));
+ control.push = pushButton.update(oi.xbox.getButton(Button.B));
+
+ return control;
+ }
+}