diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/HwRobot.java | 18 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Teleop.java | 5 |
2 files changed, 16 insertions, 7 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java index 0eb95fd..ec96bf4 100644 --- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java @@ -1,10 +1,11 @@ package org.usfirst.frc.team4272.robot2016; import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -//import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PIDOutput; -//import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.Talon; public class HwRobot { @@ -32,21 +33,32 @@ public class HwRobot { private PIDOutput lDriveM1 = new Talon(/*PWM*/1), lDriveM2 = new Talon(/*PWM*/0); private PIDOutput rDriveM1 = new Talon(/*PWM*/2), rDriveM2 = new Talon(/*PWM*/4); private PIDOutput armM = new Talon(/*PWM*/8); + private Encoder armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2); private PIDOutput fBallM = new PIDOutputSplitter(new Talon(/*PWM*/9), new Talon(/*PWM*/3)); private PIDOutput bBallM = new PIDOutputSplitter(new Talon(/*PWM*/6), new Talon(/*PWM*/7)); public HwRobot() { //lDriveE.setPIDSourceType(PIDSourceType.kRate); //rDriveE.setPIDSourceType(PIDSourceType.kRate); + armE.setPIDSourceType(PIDSourceType.kDisplacement); } public void run(Control c) { + double armEval = armE.pidGet(); // avoid reading twice + if (armEval < -295) + c.arm = 0; + lDriveM1.pidWrite( c.lDrive1); lDriveM2.pidWrite( c.lDrive2); rDriveM1.pidWrite(-c.rDrive1); rDriveM2.pidWrite(-c.rDrive2); - armM.pidWrite(-c.arm); + armM.pidWrite(c.arm); fBallM.pidWrite(-c.fBall); bBallM.pidWrite(-c.bBall); + + SmartDashboard.putNumber("fBall", c.fBall); + SmartDashboard.putNumber("bBall", c.bBall); + SmartDashboard.putNumber("armM", c.arm); + SmartDashboard.putNumber("armE", armEval); } } diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java index 7b3ca55..e69c8f0 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java @@ -3,7 +3,6 @@ package org.usfirst.frc.team4272.robot2016; //import org.usfirst.frc.team4272.robotlib.PushButton; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Joystick; public class Teleop { @@ -40,10 +39,8 @@ public class Teleop { - oneIf(oi.xbox.getButton(Button.B)), 1); control.fBall = control.fBall < 0 ? 0.5*control.fBall : control.fBall; - control.bBall = control.bBall < 0 ? 0.5*control.bBall : control.bBall; + control.bBall = control.bBall < 0 ? 0.75*control.bBall : control.bBall; - SmartDashboard.putNumber("fBall", control.fBall); - SmartDashboard.putNumber("bBall", control.bBall); /* Take pictures */ /* |