summaryrefslogtreecommitdiff
path: root/src/org
diff options
context:
space:
mode:
Diffstat (limited to 'src/org')
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/HwRobot.java10
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/Robot.java148
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java30
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);
}
}