diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/HwRobot.java')
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/HwRobot.java | 18 |
1 files changed, 15 insertions, 3 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); } } |