diff --git a/src/org/usfirst/frc/team4737/robot/Robot.java b/src/org/usfirst/frc/team4737/robot/Robot.java index ec10d02..55b37ab 100644 --- a/src/org/usfirst/frc/team4737/robot/Robot.java +++ b/src/org/usfirst/frc/team4737/robot/Robot.java @@ -7,6 +7,7 @@ package org.usfirst.frc.team4737.robot; +import org.usfirst.frc.team4737.robot.commands.RelaxDrivetrain; import org.usfirst.frc.team4737.robot.subsystems.Climber; import org.usfirst.frc.team4737.robot.subsystems.Drivetrain; import org.usfirst.frc.team4737.robot.subsystems.Elevator; @@ -54,7 +55,7 @@ public void robotInit() { */ @Override public void disabledInit() { - + new RelaxDrivetrain().start(); } @Override diff --git a/src/org/usfirst/frc/team4737/robot/commands/RelaxDrivetrain.java b/src/org/usfirst/frc/team4737/robot/commands/RelaxDrivetrain.java new file mode 100644 index 0000000..549fb4e --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/RelaxDrivetrain.java @@ -0,0 +1,39 @@ +package org.usfirst.frc.team4737.robot.commands; + +import org.usfirst.frc.team4737.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class RelaxDrivetrain extends Command { + + public RelaxDrivetrain() { + requires(Robot.DRIVETRAIN); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.DRIVETRAIN.setCoastMode(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } + +} diff --git a/src/org/usfirst/frc/team4737/robot/commands/RelaxElevator.java b/src/org/usfirst/frc/team4737/robot/commands/RelaxElevator.java index 332cab5..d26a266 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/RelaxElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/RelaxElevator.java @@ -1,38 +1,39 @@ package org.usfirst.frc.team4737.robot.commands; +import org.usfirst.frc.team4737.robot.Robot; + import edu.wpi.first.wpilibj.command.Command; /** * */ -public class RelaxElevator extends StopElevator { +public class RelaxElevator extends Command { public RelaxElevator() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - super(); + requires(Robot.ELEVATOR); } // Called just before this Command runs the first time -// protected void initialize() { -// } -// -// // Called repeatedly when this Command is scheduled to run -// protected void execute() { -// } -// -// // Make this return true when this Command no longer needs to run execute() -// protected boolean isFinished() { -// return false; -// } -// -// // Called once after isFinished returns true -// protected void end() { -// } -// -// // Called when another command which requires one or more of the same -// // subsystems is scheduled to run -// protected void interrupted() { -// } + protected void initialize() { + Robot.ELEVATOR.setCoastMode(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } } diff --git a/src/org/usfirst/frc/team4737/robot/commands/StopElevator.java b/src/org/usfirst/frc/team4737/robot/commands/StopElevator.java index 004726a..a86647e 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/StopElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/StopElevator.java @@ -15,6 +15,7 @@ public StopElevator() { // Called just before this Command runs the first time protected void initialize() { + Robot.ELEVATOR.setBrakeMode(); } // Called repeatedly when this Command is scheduled to run diff --git a/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java b/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java index e2515d5..350fcac 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java +++ b/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java @@ -16,6 +16,7 @@ public TeleopTankDrive() { // Called just before this Command runs the first time protected void initialize() { + Robot.DRIVETRAIN.setBrakeMode(); } // Called repeatedly when this Command is scheduled to run diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java index 0c3dfb7..dddb5ba 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java @@ -3,6 +3,7 @@ import org.usfirst.frc.team4737.robot.RobotMap; import org.usfirst.frc.team4737.robot.commands.TeleopTankDrive; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.Subsystem; @@ -28,9 +29,11 @@ public Drivetrain() { leftBackSlave.follow(leftFrontMaster); rightBackSlave.follow(rightFrontMaster); + + leftFrontMaster.configOpenloopRamp(0.5, 30); + rightFrontMaster.configOpenloopRamp(0.5, 30); drive = new DifferentialDrive(leftFrontMaster, rightFrontMaster); - } public void initDefaultCommand() { @@ -38,6 +41,20 @@ public void initDefaultCommand() { setDefaultCommand(new TeleopTankDrive()); } + public void setBrakeMode() { + leftFrontMaster.setNeutralMode(NeutralMode.Brake); + leftBackSlave.setNeutralMode(NeutralMode.Brake); + rightFrontMaster.setNeutralMode(NeutralMode.Brake); + rightBackSlave.setNeutralMode(NeutralMode.Brake); + } + + public void setCoastMode() { + leftFrontMaster.setNeutralMode(NeutralMode.Coast); + leftBackSlave.setNeutralMode(NeutralMode.Coast); + rightFrontMaster.setNeutralMode(NeutralMode.Coast); + rightBackSlave.setNeutralMode(NeutralMode.Coast); + } + /** * Controls the drivetrain using two tank-drive joystick inputs * diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java index 523f972..47a6943 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java @@ -4,6 +4,7 @@ import org.usfirst.frc.team4737.robot.commands.StopElevator; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.Subsystem; @@ -50,5 +51,13 @@ public double getMotorCurrent() { public void setSpeed(double speed) { motor.set(ControlMode.PercentOutput, speed); } + + public void setBrakeMode() { + motor.setNeutralMode(NeutralMode.Brake); + } + + public void setCoastMode() { + motor.setNeutralMode(NeutralMode.Coast); + } }