summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Autonomous.java12
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/HwRobot.java19
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Teleop.java22
3 files changed, 33 insertions, 20 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
index 0a7452c..1d61448 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
@@ -8,12 +8,12 @@ public class Autonomous {
}
public Control run(Control c) {
- //if (!robot.armL.get()) {
- // c.arm = -0.2;
- //} else {
- // c.arm = 0;
- // robot.armE.reset();
- //}
+ if (robot.armL.get()) {
+ c.arm = -0.2;
+ } else {
+ c.arm = 0;
+ robot.armE.reset();
+ }
return c;
}
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
index ebc4efc..7d63f86 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -29,27 +29,25 @@ public class HwRobot {
// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
- //public final Encoder lDriveE = new Encoder(/*DIO*/2,/*DIO*/3);
- //public final Encoder rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true);
private final PIDOutput lDriveM1 = new Talon(/*PWM*/1), lDriveM2 = new Talon(/*PWM*/0);
private final PIDOutput rDriveM1 = new Talon(/*PWM*/2), rDriveM2 = new Talon(/*PWM*/4);
private final PIDOutput armM = new Talon(/*PWM*/8);
- public final Encoder armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2);
private final PIDOutput fBallM = new PIDOutputSplitter(new Talon(/*PWM*/9), new Talon(/*PWM*/3));
private final PIDOutput bBallM = new PIDOutputSplitter(new Talon(/*PWM*/6), new Talon(/*PWM*/7));
+
+ public final Encoder armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2, true);
public final DigitalInput armL = new DigitalInput(/*DIO*/3);
public final DigitalInput ballL = new DigitalInput(/*DIO*/4);
+ public final Encoder rDriveE = new Encoder(/*DIO*/5,/*DIO*/6);
+ public final Encoder lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true);
public HwRobot() {
- //lDriveE.setPIDSourceType(PIDSourceType.kRate);
- //rDriveE.setPIDSourceType(PIDSourceType.kRate);
armE.setPIDSourceType(PIDSourceType.kDisplacement);
+ lDriveE.setPIDSourceType(PIDSourceType.kRate);
+ rDriveE.setPIDSourceType(PIDSourceType.kRate);
}
public void run(Control c) {
- if (armE.pidGet() < -295)
- c.arm = 0;
-
lDriveM1.pidWrite( c.lDrive1);
lDriveM2.pidWrite( c.lDrive2);
rDriveM1.pidWrite(-c.rDrive1);
@@ -61,6 +59,11 @@ public class HwRobot {
SmartDashboard.putNumber("fBall", c.fBall);
SmartDashboard.putNumber("bBall", c.bBall);
SmartDashboard.putNumber("armM", c.arm);
+
SmartDashboard.putNumber("armE", armE.pidGet());
+ SmartDashboard.putNumber("lDriveE", lDriveE.pidGet());
+ SmartDashboard.putNumber("rDriveE", rDriveE.pidGet());
+ SmartDashboard.putBoolean("ballL", ballL.get());
+ SmartDashboard.putBoolean("armL", armL.get());
}
}
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index cafbca1..19dfc6e 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -6,9 +6,11 @@ import edu.wpi.first.wpilibj.Joystick;
public class Teleop {
private final HwRobot robot;
+ private final double armSoftStop = 95;//295;
public Teleop(HwRobot robot) {
this.robot = robot;
+ robot.armE.reset();
}
private static double jsScale(Joystick j) {
@@ -36,14 +38,22 @@ public class Teleop {
double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger);
double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B));
- boolean ballL = false; //robot.ballL.get();
- control.fBall = bound(-oneIf(!ballL), fBall, 1);
- control.bBall = bound(-oneIf(!ballL), bBall < 0 ? 0.75*bBall : bBall, 1);
+ boolean ballL = robot.ballL.get();
+ control.fBall = bound(-oneIf(ballL), fBall, 1);
+ control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.75*bBall : bBall, 1);
/* I'm eyeballing 10 degrees ≈ 50 clicks */
- //if (robot.armE.pidGet() < 50 && robot.armL.get()) {
- // robot.armE.reset();
- //}
+ if (robot.armE.pidGet() < 50 && !robot.armL.get()) {
+ robot.armE.reset();
+ }
+
+ if (oi.xbox.getButton(Button.LBumper))
+ control.arm = 1-(robot.armE.pidGet()/armSoftStop);
+ else if (oi.xbox.getButton(Button.RBumper))
+ control.arm = robot.armE.pidGet()/armSoftStop;
+
+ //if (robot.armE.pidGet() > armSoftStop && control.arm > 0)
+ // control.arm = 0;
/* Take pictures */
/*