Drive train subsystem
package org.discobots.stronghold.subsystems;
import org.discobots.stronghold.HW;
import org.discobots.stronghold.commands.drive.ArcadeDriveCommand;
import org.discobots.stronghold.commands.drive.TankDriveCommand;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.TalonSRX;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
*
*/
public class DriveTrainSubsystem extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
/* Motors */
//TalonSRX frontRight, frontMiddleRight, backMiddleRight, backRight, frontLeft,frontMiddleLeft, backMiddleLeft,
//backLeft;
Talon mRight, mLeft;
RobotDrive robotDrive;
static final double CONSTANT_RAMP_LIMIT = 0.1; // ramping
// 0.05 = 4/10 seconds to full, 0.1 = 2/10 seconds to full
boolean allowRamped = false;
private double prevLeft = 0, prevRight = 0;
private double prevY = 0, prevX = 0, prevR;
int choice=0;
static double kSpeedScaling = 1.0;
public DriveTrainSubsystem() {
choice =-1; //default tank drive if no choice chosen (prevents null pointer exception)
}
public DriveTrainSubsystem(int choose) {
choice = choose;
switch(choose)
{
case 1: new TankDriveCommand();
case 2: new ArcadeDriveCommand();
default: new TankDriveCommand();
}
/* Motors */
mRight = new Talon(HW.motorRight);
mLeft = new Talon(HW.motorLeft);
/* Sensors */
/* RobotDrive */
robotDrive = new RobotDrive(mLeft, mRight);
//robotDrive = new RobotDrive(mRight, mRight);
}
public void setRamped(boolean a) {
this.allowRamped = a;
}
public boolean getRamped() {
return this.allowRamped;
}
public void tankDriveRamp(double leftStick, double rightStick) {
if (!allowRamped) {
tankDriveUnramped(leftStick, rightStick);
return;
}
double left = leftStick, right = -rightStick;
if (left - prevLeft > CONSTANT_RAMP_LIMIT) {
left = prevLeft + CONSTANT_RAMP_LIMIT;
} else if (prevLeft - left > CONSTANT_RAMP_LIMIT) {
left = prevLeft - CONSTANT_RAMP_LIMIT;
}
if (right - prevRight > CONSTANT_RAMP_LIMIT) {
right = prevRight + CONSTANT_RAMP_LIMIT;
} else if (prevRight - right > CONSTANT_RAMP_LIMIT) {
right = prevRight - CONSTANT_RAMP_LIMIT;
}
prevLeft = left;
prevRight = right;
robotDrive.tankDrive(left * kSpeedScaling, right * kSpeedScaling);
}
public void arcadeDriveRamp(double iy, double ix) {
if (!allowRamped) {
arcadeDriveUnramped(iy, ix);
return;
}
double ox = ix, oy = -iy;
if (oy - prevY > CONSTANT_RAMP_LIMIT) {
oy = prevY + CONSTANT_RAMP_LIMIT;
} else if (prevY - oy > CONSTANT_RAMP_LIMIT) {
oy = prevY - CONSTANT_RAMP_LIMIT;
}
if (ox - prevX > CONSTANT_RAMP_LIMIT) {
ox = prevX + CONSTANT_RAMP_LIMIT;
} else if (prevX - ox > CONSTANT_RAMP_LIMIT) {
ox = prevX - CONSTANT_RAMP_LIMIT;
}
prevX = ox;
prevY = oy;
robotDrive.arcadeDrive(ox * kSpeedScaling, oy * kSpeedScaling);
// RobotDrive is dumb arcadeDrive so parameters are switched
}
public void tankDriveUnramped(double leftStick, double rightStick) {
prevLeft = 0;
prevRight = 0;
prevX = 0;
prevY = 0;
prevR = 0;
robotDrive.tankDrive(leftStick * kSpeedScaling, -rightStick
* kSpeedScaling);
}
public void arcadeDriveUnramped(double y, double x) {
prevLeft = 0;
prevRight = 0;
prevX = 0;
prevY = 0;
prevR = 0;
robotDrive.arcadeDrive(x * kSpeedScaling, -y * kSpeedScaling);
// robotdrive is dumb arcadeDrive so params are switched
}
public void initDefaultCommand() {
if (!(choice>=0))
setDefaultCommand(new TankDriveCommand());
}
public double getSpeedScaling() {
return kSpeedScaling;
}
public void setSpeedScaling(double a) {
kSpeedScaling = a;
}
}
HW (Hardware)
package org.discobots.stronghold;
/**
* The RobotMap is a mapping from the ports sensors and actuators are wired into
* to a variable name. This provides flexibility changing wiring, makes checking
* the wiring easier and significantly reduces the number of magic numbers
* floating around.
*/
public class HW {
// For example to map the left and right motors, you could define the
// following variables to use with your drivetrain subsystem.
// public static int leftMotor = 1;
// public static int rightMotor = 2;
// If you are using multiple modules, make sure to define both the port
// number and the module. For example you with a rangefinder:
// public static int rangefinderPort = 1;
// public static int rangefinderModule = 1;
/* CAN */// Check roboRio web interface for these values
/* PWM */
public final static int motorRight = 0;
public final static int motorLeft = 1;
/* Pneumatics */
}
Robot
package org.discobots.stronghold;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import java.util.concurrent.TimeUnit;
import org.discobots.stronghold.commands.AutonomousCommand;
import org.discobots.stronghold.commands.drive.ArcadeDriveCommand;
import org.discobots.stronghold.commands.drive.TankDriveCommand;
//import org.discobots.stronghold.commands.ExampleCommand;
import org.discobots.stronghold.subsystems.DriveTrainSubsystem;
//import org.discobots.stronghold.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Robot extends IterativeRobot {
public static OI oi;
public static double totalTime;
public static long TeleopStartTime;
public static long loopExecutionTime = 0;
public static DriveTrainSubsystem driveTrainSub;
Command autonomousCommand,driveCommand;
SendableChooser chooser, autonChooser;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
public void robotInit() {
oi = new OI();
/* Subsystems */
driveTrainSub = new DriveTrainSubsystem();
autonChooser = new SendableChooser();
autonChooser.addDefault("Auton1", new AutonomousCommand());
autonChooser.addObject("Auton2", new AutonomousCommand());
SmartDashboard.putData("Choose Auton", autonChooser);
// dashboard init
Dashboard.init();
Dashboard.update();
}
/**
* This function is called once each time the robot enters Disabled mode.
* You can use it to reset any subsystem information you want to clear when
* the robot is disabled.
*/
public void disabledInit(){
for (long stop=System.nanoTime()+TimeUnit.SECONDS.toNanos(1);stop>System.nanoTime();) { //rumbles upon disable for 1 second
oi.setRumble(1);
}
}
public void disabledPeriodic() {
long start = System.currentTimeMillis();
Scheduler.getInstance().run();
Dashboard.update();
long end = System.currentTimeMillis();
loopExecutionTime = end - start;
}
/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
* or additional comparisons to the switch structure below with additional strings & commands.
*/
public void autonomousInit() {
autonomousCommand = (Command) autonChooser.getSelected();
/* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
switch(autoSelected) {
case "My Auto":
autonomousCommand = new MyAutoCommand();
break;
case "Default Auto":
default:
autonomousCommand = new ExampleCommand();
break;
} */
// schedule the autonomous command (example)
if (autonomousCommand != null) autonomousCommand.start();
}
/**
* This function is called periodically during autonomous
*/
public void autonomousPeriodic() {
long start = System.currentTimeMillis();
Scheduler.getInstance().run();
Dashboard.update();
long end = System.currentTimeMillis();
loopExecutionTime = end - start;
}
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (autonomousCommand != null) autonomousCommand.cancel();
driveCommand = (Command) autonChooser.getSelected();
for (long stop=System.nanoTime()+TimeUnit.SECONDS.toNanos(1);stop>System.nanoTime();) { //rumbles upon disable for 1 second
oi.setRumble(1);
TeleopStartTime = System.currentTimeMillis();
}
if(driveCommand != null)
driveCommand.start();
}
/**
* This function is called periodically during operator control
*/
public void teleopPeriodic() {
long start = System.currentTimeMillis(); //measures loop execution times
Scheduler.getInstance().run();
Dashboard.update();
long end = System.currentTimeMillis();
loopExecutionTime = end - start;
}
/**
* This function is called periodically during test mode
*/
public void testPeriodic() {
long start = System.currentTimeMillis();
LiveWindow.run();
Scheduler.getInstance().run();
Dashboard.update();
long end = System.currentTimeMillis();
loopExecutionTime = end - start;
totalTime = (double) ((System.currentTimeMillis() - TeleopStartTime)/1000);
}
}
Tank Drive Command
package org.discobots.stronghold.commands.drive;
import org.discobots.stronghold.Robot;
import edu.wpi.first.wpilibj.command.Command;
/**
*
*/
public class TankDriveCommand extends Command {
public TankDriveCommand() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
requires(Robot.driveTrainSub);
}
// Called just before this Command runs the first time
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
Robot.driveTrainSub.tankDriveRamp(Robot.oi.getRawAnalogStickALY(),Robot.oi.getRawAnalogStickARY());
}
// 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() {
Robot.driveTrainSub.tankDriveUnramped(0, 0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
Thanks