Skip to content

Commit

Permalink
Added brake/coast mode to Drivetrain, Elevator
Browse files Browse the repository at this point in the history
  • Loading branch information
briansemrau committed Feb 22, 2018
1 parent 2e974da commit 324ceac
Show file tree
Hide file tree
Showing 7 changed files with 95 additions and 26 deletions.
3 changes: 2 additions & 1 deletion src/org/usfirst/frc/team4737/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -54,7 +55,7 @@ public void robotInit() {
*/
@Override
public void disabledInit() {

new RelaxDrivetrain().start();
}

@Override
Expand Down
39 changes: 39 additions & 0 deletions src/org/usfirst/frc/team4737/robot/commands/RelaxDrivetrain.java
Original file line number Diff line number Diff line change
@@ -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() {
}

}
49 changes: 25 additions & 24 deletions src/org/usfirst/frc/team4737/robot/commands/RelaxElevator.java
Original file line number Diff line number Diff line change
@@ -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() {
}

}
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
19 changes: 18 additions & 1 deletion src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -28,16 +29,32 @@ 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() {
// Set the default command for a subsystem here.
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
*
Expand Down
9 changes: 9 additions & 0 deletions src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

}

0 comments on commit 324ceac

Please sign in to comment.