mirror of
https://github.com/Team4388/2018-Robot.git
synced 2026-06-09 00:38:04 -06:00
Updated snapshot as of 20180305-1715
This commit is contained in:
Dave Staudacher
@@ -21,13 +21,21 @@ public class Constants {
|
||||
public static double kDriveLowGearMaxSpeedInchesPerSec = 12.0 * 5.0;
|
||||
public static double kDriveStraightBasicMaxSpeedInchesPerSec = 72.0;
|
||||
public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0;
|
||||
public static double kDriveTurnBasicTankMotorOutput = 0.2;
|
||||
public static double kDriveTurnBasicTankMotorOutput = 0.5;
|
||||
public static double kDriveTurnBasicSingleMotorOutput = 0.15;
|
||||
public static double kElevatorWheelDiameterInches = 1;
|
||||
// Encoders
|
||||
public static int kDriveEncoderTicksPerRev = 4096;
|
||||
public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI);
|
||||
public static double kElevatorEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev /(kElevatorWheelDiameterInches * Math.PI) ;
|
||||
|
||||
// Elevator
|
||||
public static int kElevatorEncoderTickesPerRev = 256;
|
||||
public static double kElevatorInchesOfTravelPerRev = 3.75;
|
||||
// public static double kElevatorEncoderTicksPerInch = (double)kElevatorEncoderTickesPerRev / kElevatorInchesOfTravelPerRev;
|
||||
public static double kElevatorEncoderTicksPerInch = 126.36;
|
||||
public static double kElevatorBasicPercentOutputUp = -.9;
|
||||
public static double kElevatorBasicPercentOutputDown =.9;
|
||||
|
||||
// CONTROL LOOP GAINS
|
||||
|
||||
// PID gains for drive velocity loop (LOW GEAR)
|
||||
|
||||
@@ -187,7 +187,10 @@ public class OI
|
||||
//SmartDashboard.putData("move intake", new IntakePosition(true));
|
||||
//SmartDashboard.putData("move intake2", new IntakePosition(false));
|
||||
//SmartDashboard.putData("Move Stop", new ElevatorSetSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED));
|
||||
SmartDashboard.putData("move elevator", new ElevatorBasic(20, 15));
|
||||
|
||||
|
||||
SmartDashboard.putData("move elevator", new ElevatorBasic(20));
|
||||
|
||||
///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED));
|
||||
///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED));
|
||||
|
||||
|
||||
@@ -33,7 +33,8 @@ import org.usfirst.frc4388.robot.subsystems.*;
|
||||
import org.usfirst.frc4388.utility.ControlLooper;
|
||||
import org.usfirst.frc4388.robot.subsystems.Drive;
|
||||
import org.usfirst.frc4388.robot.subsystems.Carriage;
|
||||
import org.usfirst.frc4388.robot.subsystems.LED;;
|
||||
import org.usfirst.frc4388.robot.subsystems.LED;
|
||||
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;;
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
* functions corresponding to each mode, as described in the TimedRobot
|
||||
@@ -50,7 +51,6 @@ public class Robot extends IterativeRobot
|
||||
|
||||
//public static final Elevator elevator = new Elevator();
|
||||
public static final Carriage carriage = new Carriage();
|
||||
public static final ElevatorAuton elevatorAuton = new ElevatorAuton();
|
||||
public static final Elevator elevator = new Elevator();
|
||||
|
||||
|
||||
@@ -72,9 +72,15 @@ public class Robot extends IterativeRobot
|
||||
public static OperationMode operationMode = OperationMode.TEST;
|
||||
|
||||
private SendableChooser<OperationMode> operationModeChooser;
|
||||
private SendableChooser<Command> autonTaskChooser;
|
||||
private SendableChooser<Command> RRautonTaskChooser;
|
||||
private SendableChooser<Command> RLautonTaskChooser;
|
||||
private SendableChooser<Command> LRautonTaskChooser;
|
||||
private SendableChooser<Command> LLautonTaskChooser;
|
||||
|
||||
private Command autonomousCommand;
|
||||
private Command RRautonomousCommand;
|
||||
private Command RLautonomousCommand;
|
||||
private Command LRautonomousCommand;
|
||||
private Command LLautonomousCommand;
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be
|
||||
@@ -109,14 +115,61 @@ public class Robot extends IterativeRobot
|
||||
operationModeChooser.addObject("Competition", OperationMode.COMPETITION);
|
||||
SmartDashboard.putData("Operation Mode", operationModeChooser);
|
||||
|
||||
autonTaskChooser = new SendableChooser<Command>();
|
||||
|
||||
//autonTaskChooser.addDefault("Blue Gear Loading Side Plus Travel Forward", new BlueGearLoadingSideForwardAuton());
|
||||
autonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
|
||||
//autonTaskChooser.addObject("Right Switch Forward", new RightSwitchForward());
|
||||
autonTaskChooser.addDefault("RightStartLeftScore", new RightStartLeftScore());
|
||||
|
||||
SmartDashboard.putData("Auton Task", autonTaskChooser);
|
||||
|
||||
|
||||
|
||||
RRautonTaskChooser = new SendableChooser<Command>();
|
||||
|
||||
RRautonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
|
||||
RRautonTaskChooser.addDefault("RR Cross The Base Line", new CrossTheBaseLine());
|
||||
RRautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft());
|
||||
|
||||
|
||||
SmartDashboard.putData("RRAuton Task", RRautonTaskChooser);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
RLautonTaskChooser = new SendableChooser<Command>();
|
||||
|
||||
RLautonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
|
||||
RLautonTaskChooser.addDefault("RL Cross The Base Line", new CrossTheBaseLine());
|
||||
RLautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft());
|
||||
|
||||
|
||||
SmartDashboard.putData("RLAuton Task", RLautonTaskChooser);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
LLautonTaskChooser = new SendableChooser<Command>();
|
||||
|
||||
LLautonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
|
||||
LLautonTaskChooser.addDefault("LL Cross The Base Line", new CrossTheBaseLine());
|
||||
LLautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft());
|
||||
|
||||
|
||||
SmartDashboard.putData("LLAuton Task", LLautonTaskChooser);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
LRautonTaskChooser = new SendableChooser<Command>();
|
||||
|
||||
LRautonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
|
||||
LRautonTaskChooser.addDefault("LR Cross The Base Line", new CrossTheBaseLine());
|
||||
LRautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft());
|
||||
|
||||
|
||||
SmartDashboard.putData("LRAuton Task", LRautonTaskChooser);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//ledLights.setAllLightsOn(false);
|
||||
@@ -158,14 +211,36 @@ public class Robot extends IterativeRobot
|
||||
//elevator.resetElevatorEncoder();
|
||||
drive.resetGyro();
|
||||
drive.setIsRed(getAlliance().equals(Alliance.Red));
|
||||
if (autonomousCommand != null) autonomousCommand.start();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during autonomous
|
||||
*/
|
||||
|
||||
|
||||
String gameData;
|
||||
gameData = DriverStation.getInstance().getGameSpecificMessage();
|
||||
if(gameData.length() > 0)
|
||||
{
|
||||
if(gameData.charAt(0) == 'L')
|
||||
{
|
||||
if(gameData.charAt(1) == 'L')
|
||||
{
|
||||
if (LLautonomousCommand != null) LLautonomousCommand.start();
|
||||
} else {
|
||||
if (LRautonomousCommand != null) LRautonomousCommand.start();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if(gameData.charAt(1) == 'L')
|
||||
{
|
||||
if (RLautonomousCommand != null) RLautonomousCommand.start();
|
||||
} else {
|
||||
if (RRautonomousCommand != null) RRautonomousCommand.start();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void autonomousPeriodic() {
|
||||
Scheduler.getInstance().run();
|
||||
//Robot.elevator.setControlMode(DriveControlMode.RAW);
|
||||
updateStatus();
|
||||
}
|
||||
|
||||
@@ -174,7 +249,10 @@ public class Robot extends IterativeRobot
|
||||
// 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();
|
||||
if (RRautonomousCommand != null) RRautonomousCommand.cancel();
|
||||
if (RLautonomousCommand != null) RLautonomousCommand.cancel();
|
||||
if (LRautonomousCommand != null) LRautonomousCommand.cancel();
|
||||
if (LLautonomousCommand != null) LLautonomousCommand.cancel();
|
||||
//MotionProfileCache.getInstance().release();
|
||||
updateChoosers();
|
||||
controlLoop.start();
|
||||
@@ -185,6 +263,7 @@ public class Robot extends IterativeRobot
|
||||
//shooterFeed.setSpeed(0);
|
||||
//zarkerFeed.setSpeed(0);
|
||||
//shooter.setHopperPosition(HopperState.CLOSE);
|
||||
//Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
|
||||
updateStatus();
|
||||
}
|
||||
|
||||
@@ -207,7 +286,10 @@ public class Robot extends IterativeRobot
|
||||
|
||||
private void updateChoosers() {
|
||||
operationMode = (OperationMode)operationModeChooser.getSelected();
|
||||
autonomousCommand = (Command)autonTaskChooser.getSelected();
|
||||
RRautonomousCommand = (Command)RRautonTaskChooser.getSelected();
|
||||
RLautonomousCommand = (Command)RLautonTaskChooser.getSelected();
|
||||
LRautonomousCommand = (Command)LRautonTaskChooser.getSelected();
|
||||
LLautonomousCommand = (Command)LLautonTaskChooser.getSelected();
|
||||
}
|
||||
|
||||
public Alliance getAlliance() {
|
||||
@@ -216,6 +298,7 @@ public class Robot extends IterativeRobot
|
||||
|
||||
public void updateStatus() {
|
||||
drive.updateStatus(operationMode);
|
||||
elevator.updateStatus(operationMode);
|
||||
//carriage.updateStatus(operationMode);
|
||||
//shooter.updateStatus(operationMode);
|
||||
//shooterFeed.updateStatus(operationMode);
|
||||
|
||||
@@ -10,36 +10,36 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
|
||||
public class ElevatorBasic extends Command {
|
||||
private double m_targetInches;
|
||||
private double m_maxVelocityInchesPerSec;
|
||||
private double m_targetHeightInchesAboveFloor;
|
||||
private double m_speed;
|
||||
private boolean m_goingBackwards;
|
||||
private boolean m_goingUp;
|
||||
private double m_commandInitTimestamp;
|
||||
private double m_lastCommandExecuteTimestamp = 0.0;
|
||||
private double m_lastCommandExecutePeriod = 0.0;
|
||||
public static boolean isfinishedElevatorBasic;
|
||||
|
||||
public ElevatorBasic(double targetInches, double maxVelocityInchesPerSec) {
|
||||
public ElevatorBasic(double targetHeightInchesAboveFloor) {
|
||||
requires(Robot.elevator);
|
||||
m_targetInches = targetInches;
|
||||
m_maxVelocityInchesPerSec = maxVelocityInchesPerSec;
|
||||
m_goingBackwards = (m_targetInches < 0.0);
|
||||
m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor;
|
||||
}
|
||||
|
||||
protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) {
|
||||
double sign = (backwards ? -1.0 : 1.0);
|
||||
// Keep velocity above stiction limit (else bot will freeze and command will not complete)
|
||||
double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec);
|
||||
// Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick)
|
||||
return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec;
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
Robot.elevator.resetElevatorEncoder();
|
||||
protected void initialize()
|
||||
{
|
||||
Robot.elevator.setControlMode(DriveControlMode.RAW);
|
||||
|
||||
double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
|
||||
// start out at half speed, as crude way to reduce slippage
|
||||
m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards);
|
||||
m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight);
|
||||
System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN"));
|
||||
if (m_goingUp) {
|
||||
m_speed = Constants.kElevatorBasicPercentOutputUp;
|
||||
}
|
||||
else {
|
||||
m_speed = Constants.kElevatorBasicPercentOutputDown;
|
||||
}
|
||||
m_commandInitTimestamp = Timer.getFPGATimestamp();
|
||||
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
@@ -50,31 +50,31 @@ public class ElevatorBasic extends Command {
|
||||
m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
|
||||
}
|
||||
m_lastCommandExecuteTimestamp = currentTimestamp;
|
||||
double steer = 0.0;
|
||||
|
||||
|
||||
Robot.elevator.rawMoveSteer(m_speed, steer);
|
||||
Robot.elevator.rawSetOutput(m_speed);
|
||||
//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
boolean finished=false;
|
||||
double velocity = m_maxVelocityInchesPerSec;
|
||||
double position = (Robot.elevator.getelevatorPositionWorld());
|
||||
double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0);
|
||||
double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
|
||||
double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0);
|
||||
System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor);
|
||||
if (remaining < 0.0) {
|
||||
finished = true;
|
||||
} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms
|
||||
|
||||
}
|
||||
/*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms
|
||||
velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed
|
||||
} else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms
|
||||
velocity = m_maxVelocityInchesPerSec / 2.0; // half speed
|
||||
}
|
||||
}*/
|
||||
|
||||
|
||||
if (!finished) {
|
||||
m_speed = velocityToMoveSpeed(velocity, m_goingBackwards);
|
||||
SmartDashboard.putNumber("DSB Dist", position);
|
||||
SmartDashboard.putNumber("EB Dist", currentHeight);
|
||||
} else {
|
||||
SmartDashboard.putNumber("DSB finDist", position);
|
||||
SmartDashboard.putNumber("EB finDist", currentHeight);
|
||||
}
|
||||
return finished;
|
||||
}
|
||||
@@ -82,8 +82,12 @@ public class ElevatorBasic extends Command {
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
double currentTimestamp = Timer.getFPGATimestamp();
|
||||
SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp);
|
||||
Robot.elevator.rawMoveSteer(0.0, 0.0);
|
||||
SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp);
|
||||
|
||||
isfinishedElevatorBasic = isFinished();
|
||||
|
||||
Robot.elevator.rawSetOutput(0.0);
|
||||
|
||||
Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@ package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition;
|
||||
import org.usfirst.frc4388.robot.subsystems.ElevatorAuton;
|
||||
//import org.usfirst.frc4388.robot.subsystems.ElevatorAuton;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
@@ -30,12 +30,12 @@ public class ElevatorSetDeploy extends Command {
|
||||
secondTimerSet = false;
|
||||
if (position == IntakePosition.CUBE_INTAKE) {
|
||||
this.setTimeout(timeout);
|
||||
Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED);
|
||||
//Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED);
|
||||
isDeploy = true;
|
||||
}
|
||||
else if (position == IntakePosition.CUBE_DEPLOY) {
|
||||
this.setTimeout(timeout);
|
||||
Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED);
|
||||
//Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED);
|
||||
isDeploy = true;
|
||||
}
|
||||
else {
|
||||
@@ -63,7 +63,7 @@ public class ElevatorSetDeploy extends Command {
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
if (isDeploy == true) {
|
||||
Robot.elevatorAuton.setRaiseSpeed(0);
|
||||
//Robot.elevatorAuton.setRaiseSpeed(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -16,12 +16,12 @@ public class ElevatorSetSpeed extends Command {
|
||||
// Constructor with speed
|
||||
public ElevatorSetSpeed(double RaiseSpeed) {
|
||||
this.RaiseSpeed = RaiseSpeed;
|
||||
requires(Robot.elevatorAuton);
|
||||
// requires(Robot.elevatorAuton);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed);
|
||||
//Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
|
||||
@@ -1,49 +0,0 @@
|
||||
|
||||
package org.usfirst.frc4388.robot.commands.auton;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
|
||||
import org.usfirst.frc4388.robot.commands.DriveGyroReset;
|
||||
//import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightMP;
|
||||
//import org.usfirst.frc4388.robot.commands.IntakeSetPosition;
|
||||
//import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition;
|
||||
//import org.usfirst.frc4388.robot.commands.ShooterSetRpm;
|
||||
//import org.usfirst.frc4388.robot.commands.ShooterSetShotPosition;
|
||||
//import org.usfirst.frc4388.robot.commands.ShooterSetVoltageRampRate;
|
||||
import org.usfirst.frc4388.robot.subsystems.Drive;
|
||||
//import org.usfirst.frc4388.robot.subsystems.Shooter;
|
||||
//import org.usfirst.frc4388.robot.subsystems.Drive.SpeedShiftState;
|
||||
//import org.usfirst.frc4388.robot.subsystems.Shooter.ShotState;
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class BlueGearLoadingSideForwardAuton extends CommandGroup {
|
||||
|
||||
public BlueGearLoadingSideForwardAuton() {
|
||||
addSequential(new DriveGyroReset());
|
||||
//addSequential(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
|
||||
|
||||
//addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville
|
||||
//addSequential(new DriveAbsoluteTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
|
||||
//addSequential(new DriveStraightMP(10, Drive.MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC, true, true, -30));
|
||||
//addSequential(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
|
||||
// addSequential(new WaitCommand(1.0));
|
||||
//addSequential(new DriveStraightMP(40, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -10)); // 95 for 112" greenville
|
||||
//addSequential(new DriveAbsoluteTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
|
||||
/// addSequential(new DriveStraightMP(-10, Drive.MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC, true, true, -30));
|
||||
|
||||
// addSequential(new DriveStraightMP(40, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, 10));
|
||||
//addSequential(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
|
||||
|
||||
// addSequential(new WaitCommand(0.3));
|
||||
// addSequential(new DriveStraightMP(-20, Drive.MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -60));
|
||||
// addSequential(new DriveAbsoluteTurnMP(-173, Drive.MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
|
||||
// addSequential(new DriveSpeedShift(SpeedShiftState.HI));
|
||||
// addSequential(new DriveStraightMP(-254, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -173));
|
||||
}
|
||||
}
|
||||
+14
-14
@@ -6,6 +6,8 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightMP;
|
||||
import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
|
||||
import org.usfirst.frc4388.robot.commands.IntakePosition;
|
||||
import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
|
||||
import org.usfirst.frc4388.robot.subsystems.Carriage;
|
||||
@@ -18,27 +20,25 @@ import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class RightSwitchForward extends CommandGroup {
|
||||
public class CenterLeft extends CommandGroup {
|
||||
|
||||
public RightSwitchForward() {
|
||||
public CenterLeft() {
|
||||
addSequential(new DriveGyroReset());
|
||||
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveStraightBasic(-25, 25, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, 178, 90, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new WaitCommand(.1));
|
||||
addSequential(new DriveStraightBasic(125, 30, true, true, 0));
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new WaitCommand(.1));
|
||||
addSequential(new DriveTurnBasic(true, -90, 90, MPSoftwareTurnType.TANK));
|
||||
//addSequential(new DriveStraightBasic(5, 20, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(-10, 60, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(50, 60, true, true, 0));
|
||||
addSequential(new DriveTurnBasic(true, 28, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new ElevatorBasic(20));
|
||||
addSequential(new DriveStraightBasic(25, 60, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
addSequential(new WaitCommand(.6));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.9));
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
|
||||
//addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
package org.usfirst.frc4388.robot.commands.auton;
|
||||
|
||||
import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
|
||||
import org.usfirst.frc4388.robot.commands.DriveGyroReset;
|
||||
import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightMP;
|
||||
import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorBasic;
|
||||
import org.usfirst.frc4388.robot.commands.IntakePosition;
|
||||
import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
|
||||
import org.usfirst.frc4388.robot.subsystems.Carriage;
|
||||
import org.usfirst.frc4388.robot.subsystems.Drive;
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class CrossTheBaseLine extends CommandGroup {
|
||||
|
||||
public CrossTheBaseLine() {
|
||||
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveStraightBasic(-90, 60, true, true, 0));
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -6,6 +6,7 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightMP;
|
||||
import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorBasic;
|
||||
import org.usfirst.frc4388.robot.commands.IntakePosition;
|
||||
import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
|
||||
import org.usfirst.frc4388.robot.subsystems.Carriage;
|
||||
@@ -21,19 +22,24 @@ import edu.wpi.first.wpilibj.command.WaitCommand;
|
||||
public class RightStartLeftScore extends CommandGroup {
|
||||
|
||||
public RightStartLeftScore() {
|
||||
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveStraightBasic(-210, 60, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(-215, 60, true, true, 0));
|
||||
addSequential(new ElevatorBasic(5));
|
||||
addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(150, 60, true, true, 0));
|
||||
addSequential(new ElevatorBasic(30));
|
||||
addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(5, 60, true, true, 0));
|
||||
addSequential(new DriveStraightBasic(3, 60, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new WaitCommand(.2));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
|
||||
|
||||
addSequential(new DriveStraightBasic(-10, 60, true, true, 0));
|
||||
//addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville
|
||||
|
||||
}
|
||||
|
||||
@@ -6,12 +6,12 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
|
||||
import org.usfirst.frc4388.robot.commands.DriveStraightMP;
|
||||
import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorBasic;
|
||||
import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
|
||||
import org.usfirst.frc4388.robot.commands.IntakePosition;
|
||||
import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
|
||||
import org.usfirst.frc4388.robot.subsystems.Carriage;
|
||||
import org.usfirst.frc4388.robot.subsystems.Drive;
|
||||
import org.usfirst.frc4388.robot.subsystems.ElevatorAuton;
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
@@ -24,13 +24,13 @@ public class RightSwitchAuton extends CommandGroup {
|
||||
|
||||
public RightSwitchAuton() {
|
||||
addSequential(new DriveGyroReset());
|
||||
addSequential(new DriveSpeedShift(true));
|
||||
addSequential(new IntakePosition(true));
|
||||
addSequential(new DriveStraightBasic(-150, 60, true, true, 0));
|
||||
addSequential(new ElevatorSetSpeed(1.00));
|
||||
addSequential(new WaitCommand(.5));
|
||||
addSequential(new ElevatorSetSpeed(0.00));
|
||||
addSequential(new DriveStraightBasic(-130, 60, true, true, 0));
|
||||
addSequential(new ElevatorBasic(30));
|
||||
addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
|
||||
addSequential(new DriveStraightBasic(9, 60, true, true, 0));
|
||||
addSequential(new ElevatorBasic(30));
|
||||
addSequential(new DriveStraightBasic(20, 60, true, true, 0));
|
||||
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
|
||||
//addSequential(new WaitCommand(.1));
|
||||
addSequential(new IntakePosition(false));
|
||||
|
||||
@@ -59,7 +59,7 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
private double targetPPosition;
|
||||
|
||||
//Encoder ticks to inches for encoders
|
||||
public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch;
|
||||
public static final double ENCODER_TICKS_TO_INCHES = Constants.kElevatorEncoderTicksPerInch;
|
||||
|
||||
//control mode for joystick control
|
||||
private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
|
||||
@@ -82,6 +82,7 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
public boolean pressed;
|
||||
SensorCollection isPressed;
|
||||
|
||||
public boolean elevatorControlMode = false;
|
||||
// Motor controllers
|
||||
//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
|
||||
|
||||
@@ -93,10 +94,11 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
|
||||
elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
|
||||
|
||||
//Setting right elevator motor as follower
|
||||
elevatorRight.setInverted(false);
|
||||
|
||||
//Setting left elevator motor as follower
|
||||
elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID());
|
||||
elevatorLeft.setInverted(false);
|
||||
elevatorRight.setInverted(false);
|
||||
|
||||
//Limit Switch Left
|
||||
elevatorLeft.overrideLimitSwitchesEnable(true);
|
||||
@@ -192,7 +194,7 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
public void moveElevatorXbox()
|
||||
{
|
||||
double moveElevatorInput;
|
||||
double elevatorSafeZone = 1.2;
|
||||
double elevatorSafeZone = -30;
|
||||
|
||||
double elevatorPos = getEncoderElevatorPosition();
|
||||
|
||||
@@ -205,19 +207,23 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
|
||||
if(elevatorTuningPressed == true)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput * (0.5));
|
||||
elevatorRight.set(moveElevatorInput * 0.5);
|
||||
}
|
||||
else if(elevatorTuningPressed == false)
|
||||
{
|
||||
if(elevatorPos <= elevatorSafeZone /*&& elevatorPos >= 0*/)
|
||||
elevatorRight.set(moveElevatorInput);
|
||||
}
|
||||
|
||||
/*
|
||||
if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput * 0.65);
|
||||
elevatorRight.set(moveElevatorInput);
|
||||
}
|
||||
else if(elevatorPos > elevatorSafeZone)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput);
|
||||
elevatorRight.set(moveElevatorInput * 0.65);
|
||||
|
||||
|
||||
/*
|
||||
if(holdButtonPressed == true)
|
||||
{
|
||||
elevatorRight.set(-0.43 * (0.2));
|
||||
@@ -226,18 +232,20 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput * 0.75);
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
/*
|
||||
|
||||
else if(elevatorPos < 0)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput * 0.75);
|
||||
}
|
||||
|
||||
*/
|
||||
}
|
||||
|
||||
System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range
|
||||
|
||||
System.out.println(moveElevatorInput);
|
||||
}
|
||||
|
||||
// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range
|
||||
|
||||
//PID encoder position
|
||||
public double getEncoderElevatorPosition()
|
||||
@@ -245,6 +253,11 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
return elevatorRight.getPositionWorld();
|
||||
}
|
||||
|
||||
public double getElevatorHeightInchesAboveFloor()
|
||||
{
|
||||
return elevatorRight.getPositionWorld();
|
||||
}
|
||||
|
||||
public synchronized void setControlMode(DriveControlMode controlMode)
|
||||
{
|
||||
this.controlMode = controlMode;
|
||||
@@ -284,6 +297,10 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
|
||||
}
|
||||
*/
|
||||
public void rawSetOutput(double output){
|
||||
elevatorRight.set(/*ControlMode.PercentOutput,*/ output);
|
||||
}
|
||||
|
||||
public void holdInPos()
|
||||
{
|
||||
elevatorRight.set(-0.43 * 0.2);
|
||||
@@ -301,23 +318,30 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
|
||||
if(isPressed.isFwdLimitSwitchClosed() == true)
|
||||
{
|
||||
Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);
|
||||
if (controlMode == DriveControlMode.JOYSTICK) {
|
||||
Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);
|
||||
}
|
||||
pressed = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
|
||||
if (controlMode == DriveControlMode.STOP_MOTORS){
|
||||
{
|
||||
Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
pressed = false;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
public double getelevatorPositionWorld() {
|
||||
return elevatorRight.getPositionWorld();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void controlLoopUpdate()
|
||||
{
|
||||
@@ -344,6 +368,12 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
{
|
||||
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
|
||||
}
|
||||
/*
|
||||
else if(controlMode == DriveControlMode.RAW)
|
||||
{
|
||||
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
|
||||
}
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
@@ -355,7 +385,7 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
@Override
|
||||
public void periodic()
|
||||
{
|
||||
isSwitchPressed();
|
||||
// isSwitchPressed();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -386,6 +416,8 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
{
|
||||
try
|
||||
{
|
||||
SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor());
|
||||
//SmartDashboard.putData(pressed);
|
||||
}
|
||||
catch (Exception e)
|
||||
@@ -394,9 +426,7 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
}
|
||||
}
|
||||
}
|
||||
public void rawMoveSteer(double move, double steer) {
|
||||
m_drive.arcadeDrive(move, steer, false);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -1,120 +0,0 @@
|
||||
|
||||
package org.usfirst.frc4388.robot.subsystems;
|
||||
|
||||
import org.usfirst.frc4388.robot.RobotMap;
|
||||
import org.usfirst.frc4388.robot.commands.*;
|
||||
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
|
||||
import org.usfirst.frc4388.utility.CANTalonEncoder;
|
||||
import org.usfirst.frc4388.utility.ControlLoopable;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import org.usfirst.frc4388.robot.OI;
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
||||
import com.ctre.phoenix.motorcontrol.SensorCollection;
|
||||
|
||||
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class ElevatorAuton extends Subsystem {
|
||||
|
||||
private WPI_TalonSRX ElevatorRight;
|
||||
private WPI_TalonSRX ElevatorLeft;
|
||||
public static final double RAISE_ELEVATOR_SPEED = 1;
|
||||
public static final double LOWER_ELEVATOR_SPEED = -1;
|
||||
public static final double STOP_ELEVATOR_SPEED = 0;
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
public boolean pressed;
|
||||
SensorCollection isPressed;
|
||||
/////^^^^^^^^^ replace this line with the modes we need
|
||||
|
||||
|
||||
private boolean isFinished;
|
||||
//private CarriageControlMode controlMode = CarriageControlMode.JOYSTICK;
|
||||
private double PeriodMs;
|
||||
|
||||
|
||||
|
||||
|
||||
public ElevatorAuton() {
|
||||
try {
|
||||
ElevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR1_ID);
|
||||
ElevatorRight = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
|
||||
ElevatorRight.setInverted(false);
|
||||
ElevatorLeft.setInverted(false);
|
||||
|
||||
|
||||
|
||||
|
||||
//carriageLeft.set(CurrentLimit, value);
|
||||
|
||||
ElevatorRight.set(ControlMode.Follower, ElevatorLeft.getDeviceID());
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("An error occurred in the elevator constructor");
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public void setRaiseSpeed(double raiseSpeed) {
|
||||
ElevatorLeft.set(-raiseSpeed);
|
||||
}
|
||||
|
||||
// public synchronized void setControlMode(CarriageControlMode controlMode) {
|
||||
// this.controlMode = controlMode;
|
||||
// if (controlMode == CarriageControlMode.JOYSTICK) {
|
||||
|
||||
// carriageLeft.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
|
||||
// carriageRight.set(ControlMode.PercentOutput, 0);
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
/*public void controlLoopUpdate() {
|
||||
pressed = false;
|
||||
isPressed = ElevatorLeft.getSensorCollection();
|
||||
|
||||
elevator_input = Robot.oi.getOperatorJoystick().getRawAxis(RobotMap.OPERATOR_JOYSTICK_1_USB_ID);
|
||||
System.err.println(isPressed.isFwdLimitSwitchClosed());
|
||||
if (isPressed.isFwdLimitSwitchClosed() == true) {
|
||||
ElevatorLeft.set(0);
|
||||
ElevatorRight.set(0);
|
||||
pressed = true;
|
||||
} else {
|
||||
|
||||
ElevatorLeft.set(elevator_input);
|
||||
ElevatorRight.set(elevator_input);
|
||||
pressed = false;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
public void periodic() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
// public void setPeriodMs(long periodMs) {
|
||||
// this.PeriodMs = periodMs;
|
||||
// }
|
||||
public void initDefaultCommand() {
|
||||
}
|
||||
public void isFinished() {
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user