added compressor code
This commit is contained in:
parent
e7f3b43157
commit
3f55c7b2b8
@ -16,6 +16,7 @@ public class Robot extends IterativeRobot {
|
|||||||
chooser = new SendableChooser();
|
chooser = new SendableChooser();
|
||||||
SmartDashboard.putData("Auto mode", chooser);
|
SmartDashboard.putData("Auto mode", chooser);
|
||||||
SmartDashboard.putData("MainArm",CommandBase.mainArm.getPIDController());
|
SmartDashboard.putData("MainArm",CommandBase.mainArm.getPIDController());
|
||||||
|
SmartDashboard.putBoolean("CompressorEnabled",true);
|
||||||
}
|
}
|
||||||
public void disabledInit() {
|
public void disabledInit() {
|
||||||
}
|
}
|
||||||
@ -35,11 +36,13 @@ public class Robot extends IterativeRobot {
|
|||||||
if (autonomousCommand != null) {
|
if (autonomousCommand != null) {
|
||||||
autonomousCommand.cancel();
|
autonomousCommand.cancel();
|
||||||
}
|
}
|
||||||
|
CommandBase.pneumatics.setCompressorEnabled(true);
|
||||||
}
|
}
|
||||||
public void teleopPeriodic() {
|
public void teleopPeriodic() {
|
||||||
Scheduler.getInstance().run();
|
Scheduler.getInstance().run();
|
||||||
SmartDashboard.putNumber("ArmAngleRaw",CommandBase.mainArm.getRaw());
|
SmartDashboard.putNumber("ArmAngleRaw",CommandBase.mainArm.getRaw());
|
||||||
SmartDashboard.putNumber("ArmAngleDegrees",CommandBase.mainArm.getDegrees());
|
SmartDashboard.putNumber("ArmAngleDegrees",CommandBase.mainArm.getDegrees());
|
||||||
|
CommandBase.pneumatics.setCompressorEnabled(SmartDashboard.getBoolean("CompressorEnabled"));
|
||||||
System.out.println(CommandBase.mainArm.getDegrees());
|
System.out.println(CommandBase.mainArm.getDegrees());
|
||||||
}
|
}
|
||||||
public void testPeriodic() {
|
public void testPeriodic() {
|
||||||
|
@ -2,15 +2,18 @@ package org.usfirst.frc.team2059.robot.commands;
|
|||||||
import org.usfirst.frc.team2059.robot.subsystems.DriveBase;
|
import org.usfirst.frc.team2059.robot.subsystems.DriveBase;
|
||||||
import org.usfirst.frc.team2059.robot.subsystems.EncoderBase;
|
import org.usfirst.frc.team2059.robot.subsystems.EncoderBase;
|
||||||
import org.usfirst.frc.team2059.robot.subsystems.MainArm;
|
import org.usfirst.frc.team2059.robot.subsystems.MainArm;
|
||||||
|
import org.usfirst.frc.team2059.robot.subsystems.Pneumatics;
|
||||||
import edu.wpi.first.wpilibj.command.Command;
|
import edu.wpi.first.wpilibj.command.Command;
|
||||||
public abstract class CommandBase extends Command {
|
public abstract class CommandBase extends Command {
|
||||||
public static EncoderBase encoderBase;
|
public static EncoderBase encoderBase;
|
||||||
public static DriveBase driveBase;
|
public static DriveBase driveBase;
|
||||||
public static MainArm mainArm;
|
public static MainArm mainArm;
|
||||||
|
public static Pneumatics pneumatics;
|
||||||
public static void init() {
|
public static void init() {
|
||||||
encoderBase = new EncoderBase();
|
encoderBase = new EncoderBase();
|
||||||
driveBase = new DriveBase();
|
driveBase = new DriveBase();
|
||||||
mainArm = new MainArm();
|
mainArm = new MainArm();
|
||||||
|
pneumatics = new Pneumatics();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// vim: sw=2:ts=2:sts=2
|
// vim: sw=2:ts=2:sts=2
|
||||||
|
@ -0,0 +1,43 @@
|
|||||||
|
package org.usfirst.frc.team2059.robot.subsystems;
|
||||||
|
import org.usfirst.frc.team2059.robot.RobotMap;
|
||||||
|
import org.usfirst.frc.team2059.robot.commands.Drive;
|
||||||
|
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||||
|
import edu.wpi.first.wpilibj.Compressor;
|
||||||
|
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||||
|
public class Pneumatics extends Subsystem {
|
||||||
|
Compressor compressor = new Compressor(RobotMap.pcmID);
|
||||||
|
DoubleSolenoid shooterSolenoid = new DoubleSolenoid(RobotMap.shooterSolenoidOne, RobotMap.shooterSolenoidTwo);
|
||||||
|
DoubleSolenoid armStopSolenoid = new DoubleSolenoid(RobotMap.armStopSolenoidOne, RobotMap.armStopSolenoidTwo);
|
||||||
|
boolean shooterState, armStopState;
|
||||||
|
public void initDefaultCommand() {
|
||||||
|
}
|
||||||
|
public void setCompressorEnabled(boolean state){
|
||||||
|
compressor.setClosedLoopControl(state);
|
||||||
|
}
|
||||||
|
public void setShooterState(boolean state){
|
||||||
|
if(state){
|
||||||
|
shooterSolenoid.set(DoubleSolenoid.Value.kForward);
|
||||||
|
}else{
|
||||||
|
shooterSolenoid.set(DoubleSolenoid.Value.kReverse);
|
||||||
|
}
|
||||||
|
shooterState=state;
|
||||||
|
}
|
||||||
|
public void setArmStopState(boolean state){
|
||||||
|
if(state){
|
||||||
|
armStopSolenoid.set(DoubleSolenoid.Value.kForward);
|
||||||
|
}else{
|
||||||
|
armStopSolenoid.set(DoubleSolenoid.Value.kReverse);
|
||||||
|
}
|
||||||
|
armStopState=state;
|
||||||
|
}
|
||||||
|
public boolean getShooterState(){
|
||||||
|
return shooterState;
|
||||||
|
}
|
||||||
|
public boolean getArmStopState(){
|
||||||
|
return armStopState;
|
||||||
|
}
|
||||||
|
public boolean getCompressorEnabled(){
|
||||||
|
return compressor.enabled();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// vim: sw=2:ts=2:sts=2
|
Loading…
Reference in New Issue
Block a user