Team4388/2018-Robot
1
0
Fork 0
mirror of https://github.com/Team4388/2018-Robot.git synced 2026-06-08 16:28:06 -06:00
Code Issues Packages Projects Releases Wiki Activity

update from denver

Browse Source 
hope this works
(mostly atos fixed, centers and laft un tested)
This commit is contained in:
Luke Staudacher
2018-09-22 20:13:56 -06:00
parent 9ab79a69fa
commit 02285e5af3
13 changed files with 924 additions and 524 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Drive.java
+853
View File
@@ -0,0 +1,853 @@
package org.usfirst.frc4388.robot.subsystems;
import java.util.ArrayList;
import java.util.Set;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.OI;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.RobotMap;
import org.usfirst.frc4388.utility.AdaptivePurePursuitController;
import org.usfirst.frc4388.utility.BHRMathUtils;
import org.usfirst.frc4388.utility.CANTalonEncoder;
import org.usfirst.frc4388.utility.ControlLoopable;
import org.usfirst.frc4388.utility.Kinematics;
import org.usfirst.frc4388.utility.MMTalonPIDController;
import org.usfirst.frc4388.utility.MPSoftwarePIDController;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
import org.usfirst.frc4388.utility.MPTalonPIDController;
import org.usfirst.frc4388.utility.MPTalonPIDPathController;
import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController;
//import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
import org.usfirst.frc4388.utility.MotionProfilePoint;
//import org.usfirst.frc4388.utility.MotionProfileBoxCar;
import org.usfirst.frc4388.utility.PIDParams;
import org.usfirst.frc4388.utility.Path;
import org.usfirst.frc4388.utility.PathGenerator;
import org.usfirst.frc4388.utility.RigidTransform2d;
import org.usfirst.frc4388.utility.Rotation2d;
import org.usfirst.frc4388.utility.SoftwarePIDController;
import org.usfirst.frc4388.utility.Translation2d;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
//import com.ctre.PigeonImu;
//import com.ctre.PigeonImu.CalibrationMode;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.kauailabs.navx.frc.AHRS;
//import edu.wpi.first.wpilibj.DigitalInput;
//import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
//import edu.wpi.first.wpilibj.Solenoid;
//import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
*
*/
public class Drive extends Subsystem implements ControlLoopable
{
public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW};
public static enum SpeedShiftState { HI, LO };
public static enum ClimberState { DEPLOYED, RETRACTED };
public static final double TRACK_WIDTH_INCHES = Constants.kTrackWidthInches;
public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch;
public static final double CLIMB_SPEED = 0.45;
public static final double VOLTAGE_RAMP_RATE = 150; // Volts per second
public static final double OPEN_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full
public static final double CLOSED_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full
// Motion profile max velocities and accel times
public static final double MAX_TURN_RATE_DEG_PER_SEC = 320;
public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC = 120; //72;
public static final double MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC = 200;
public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 320;
public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 400;
public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC = 270;
public static final double MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC = 400;
public static final double MP_GEAR_DEPLOY_VELOCITY_INCHES_PER_SEC = 25;
public static final double MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC = 80;
public static final double MP_STRAIGHT_T1 = 600;
public static final double MP_STRAIGHT_T2 = 300;
public static final double MP_TURN_T1 = 600;
public static final double MP_TURN_T2 = 300;
public static final double MP_MAX_TURN_T1 = 400;
public static final double MP_MAX_TURN_T2 = 200;
// Motor controllers
private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
private CANTalonEncoder leftDrive1;
private WPI_TalonSRX leftDrive2;
// private WPI_TalonSRX leftDrive3;
private CANTalonEncoder rightDrive1;
private WPI_TalonSRX rightDrive2;
// private WPI_TalonSRX rightDrive3;
private DifferentialDrive m_drive;
//PID encoder and motor
private CANTalonEncoder elevatorRight;
private WPI_TalonSRX elevatorLeft;
//private DigitalInput hopperSensorRed;
//private DigitalInput hopperSensorBlue;
private double climbSpeed;
private boolean isRed = true;
private double periodMs;
private double lastControlLoopUpdatePeriod = 0.0;
private double lastControlLoopUpdateTimestamp = 0.0;
// Pneumatics
//private Solenoid speedShift;
// Input devices
public static final int DRIVER_INPUT_JOYSTICK_ARCADE = 0;
public static final int DRIVER_INPUT_JOYSTICK_TANK = 1;
public static final int DRIVER_INPUT_JOYSTICK_CHEESY = 2;
public static final int DRIVER_INPUT_XBOX_CHEESY = 3;
public static final int DRIVER_INPUT_XBOX_ARCADE_LEFT = 4;
public static final int DRIVER_INPUT_XBOX_ARCADE_RIGHT = 5;
public static final int DRIVER_INPUT_WHEEL = 6;
public static final double STEER_NON_LINEARITY = 0.5;
public static final double MOVE_NON_LINEARITY = 1.0;
public static final double STICK_DEADBAND = 0.02;
private int m_moveNonLinear = 0;
private int m_steerNonLinear = -3;
private double m_moveScale = 1.0;
private double m_steerScale = 1.0;
private double m_moveInput = 0.0;
private double m_steerInput = 0.0;
private double m_moveOutput = 0.0;
private double m_steerOutput = 0.0;
private double m_moveTrim = 0.0;
private double m_steerTrim = 0.0;
private boolean isFinished;
private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
private MPTalonPIDController mpStraightController;
private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7); // 4 colsons
// private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03); // 4 omni
private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0);
private MMTalonPIDController mmStraightController;
private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24);
private MPSoftwarePIDController mpTurnController; // p i d a v g izone
private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100); // 4 colson wheels
// private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100); // 4 omni
private SoftwarePIDController pidTurnController;
//private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008
private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.0, 0.0, 0, 0, 0.0, 100); //i=0.0008
private double targetPIDAngle;
private MPTalonPIDPathController mpPathController;
private PIDParams mpPathPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.28, 100); // 4 omni g=.3
//PID target
private double targetPIDPosition;
private MPTalonPIDPathVelocityController mpPathVelocityController;
private PIDParams mpPathVelocityPIDParams = new PIDParams(0.5, 0.001, 5, 0.44);
private AdaptivePurePursuitController adaptivePursuitController;
private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44);
private RigidTransform2d zeroPose = new RigidTransform2d(new Translation2d(0,0), Rotation2d.fromDegrees(0));
private RigidTransform2d currentPose = zeroPose;
private RigidTransform2d lastPose = zeroPose;
private double left_encoder_prev_distance_ = 0;
private double right_encoder_prev_distance_ = 0;
//private PigeonImu gyroPigeon;
//private double[] yprPigeon = new double[3];
private AHRS gyroNavX;
private boolean useGyroLock;
private double gyroLockAngleDeg;
//private double kPGyro = 0.04;
private double kPGyro = 0.0625;
private boolean isCalibrating = false;
private double gyroOffsetDeg = 0;
public Drive() {
try {
leftDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder);
leftDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID);
// leftDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID);
rightDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder);
rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID);
// rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID);
//gyroPigeon = new PigeonImu(leftDrive2);
gyroNavX = new AHRS(SPI.Port.kMXP);
//hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID);
//hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID);
//leftDrive1.clearStickyFaults();
//leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
//leftDrive1.setNominalClosedLoopVoltage(12.0);
leftDrive1.clearStickyFaults(0);
leftDrive1.setInverted(false);//false on comp 2108, false on microbot
leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot
leftDrive1.setSafetyEnabled(false);
//leftDrive1.setCurrentLimit(15);
//leftDrive1.enableCurrentLimit(true);
leftDrive1.setNeutralMode(NeutralMode.Brake);
leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
leftDrive1.configNominalOutputForward(+1.0f, 0);
leftDrive1.configNominalOutputReverse(-1.0f, 0);
// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
// Driver.reportError("Could not detect left drive encoder encoder!", false);
// }
leftDrive2.setInverted(false);//false on comp 2108, false on microbot
leftDrive2.setSafetyEnabled(false);
leftDrive2.setNeutralMode(NeutralMode.Brake);
leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID());
//rightDrive1.clearStickyFaults();
//rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
//rightDrive1.setNominalClosedLoopVoltage(12.0);
rightDrive1.clearStickyFaults(0);
rightDrive1.setInverted(true);//true on comp 2108, false on microbot
rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
rightDrive1.setSafetyEnabled(false);
rightDrive1.setNeutralMode(NeutralMode.Brake);
rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
rightDrive1.configNominalOutputForward(+1.0f, 0);
rightDrive1.configNominalOutputReverse(-1.0f, 0);
// if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
// DriverStation.reportError("Could not detect right drive encoder encoder!", false);
// }
rightDrive2.setInverted(true);//True on comp 2108, false on microbot
rightDrive2.setSafetyEnabled(false);
rightDrive2.setNeutralMode(NeutralMode.Brake);
rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID());
motorControllers.add(leftDrive1);
motorControllers.add(rightDrive1);
//m_drive = new RobotDrive(leftDrive1, rightDrive1);
//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
//m_drive.setSafetyEnabled(false);
m_drive = new DifferentialDrive(leftDrive1, rightDrive1);
//m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearLeft, true); //TODO URGENT: verify
//m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify
m_drive.setSafetyEnabled(false);
//speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID);
}
catch (Exception e) {
System.err.println("An error occurred in the DriveTrain constructor");
}
}
public void setToBrakeOnNeutral(boolean brakeVsCoast) {
if (brakeVsCoast) {
leftDrive1.setNeutralMode(NeutralMode.Brake);
leftDrive2.setNeutralMode(NeutralMode.Brake);
rightDrive1.setNeutralMode(NeutralMode.Brake);
rightDrive2.setNeutralMode(NeutralMode.Brake);
} else {
leftDrive1.setNeutralMode(NeutralMode.Coast);
leftDrive2.setNeutralMode(NeutralMode.Coast);
rightDrive1.setNeutralMode(NeutralMode.Coast);
rightDrive2.setNeutralMode(NeutralMode.Coast);
}
}
@Override
public void initDefaultCommand() {
}
public double getGyroAngleDeg() {
//return getGyroPigeonAngleDeg();
return getGyroNavXAngleDeg();
}
//public double getGyroPigeonAngleDeg() {
// gyroPigeon.GetYawPitchRoll(yprPigeon);
// return -yprPigeon[0] + gyroOffsetDeg;
//}
public double getGyroNavXAngleDeg() {
return gyroNavX.getAngle() + gyroOffsetDeg;
}
public void resetGyro() {
//gyroPigeon.SetYaw(0);
gyroNavX.zeroYaw();
}
public void resetEncoders() {
mpStraightController.resetZeroPosition();
}
public void calibrateGyro() {
//gyroPigeon.EnterCalibrationMode(CalibrationMode.Temperature);
}
public void endGyroCalibration() {
if (isCalibrating == true) {
isCalibrating = false;
}
}
public void setGyroOffset(double offsetDeg) {
gyroOffsetDeg = offsetDeg;
}
//public boolean isHopperSensorRedOn() {
// return hopperSensorRed.get();
//}
//public boolean isHopperSensorBlueOn() {
// return hopperSensorBlue.get();
//}
//public boolean isHopperSensorOn() {
// if (isRed() == true) {
// return isHopperSensorRedOn();
// }
// else {
// return isHopperSensorBlueOn();
// }
//}
public void setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
mmStraightController.setPID(mmStraightPIDParams);
mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true);
setControlMode(DriveControlMode.MOTION_MAGIC);
}
public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
mpStraightController.setPID(mpStraightPIDParams);
mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true);
setControlMode(DriveControlMode.MP_STRAIGHT);
}
//public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
// double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
// mpStraightController.setPID(mpStraightPIDParams);
// mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true);
// setControlMode(DriveControlMode.MP_STRAIGHT);
//}
public void setRelativeTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES);
setControlMode(DriveControlMode.MP_TURN);
}
//public void setRelativeTurnMPCached(String key, MPSoftwareTurnType turnType) {
// mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES);
// setControlMode(DriveControlMode.MP_TURN);
//}
public void setRelativeMaxTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_MAX_TURN_T1, MP_MAX_TURN_T2, turnType, TRACK_WIDTH_INCHES);
setControlMode(DriveControlMode.MP_TURN);
}
public void setAbsoluteTurnMP(double absoluteTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) {
mpTurnController.setMPTurnTarget(getGyroAngleDeg(), BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), absoluteTurnAngleDeg), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES);
setControlMode(DriveControlMode.MP_TURN);
}
//public void setAbsoluteTurnMPCached(String key, MPSoftwareTurnType turnType) {
// mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES);
// setControlMode(DriveControlMode.MP_TURN);
//}
public void setRelativeTurnPID(double relativeTurnAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) {
this.targetPIDAngle = relativeTurnAngleDeg + getGyroAngleDeg();
pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType);
setControlMode(DriveControlMode.PID_TURN);
}
public void setPathMP(PathGenerator path) {
mpPathController.setPID(mpPathPIDParams);
mpPathController.setMPPathTarget(path);
setControlMode(DriveControlMode.MP_PATH);
}
public void setPathVelocityMP(PathGenerator path) {
mpPathVelocityController.setPID(mpPathPIDParams);
mpPathVelocityController.setMPPathTarget(path);
setControlMode(DriveControlMode.MP_PATH_VELOCITY);
}
public void setPathAdaptivePursuit(Path path, boolean reversed) {
currentPose = zeroPose;
lastPose = zeroPose;
left_encoder_prev_distance_ = 0;
right_encoder_prev_distance_ = 0;
adaptivePursuitController.setPID(adaptivePursuitPIDParams);
adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25);
setControlMode(DriveControlMode.ADAPTIVE_PURSUIT);
}
public void setDriveHold(boolean status) {
if (status) {
setControlMode(DriveControlMode.HOLD);
}
else {
setControlMode(DriveControlMode.JOYSTICK);
}
}
public void updatePose() {
double left_distance = leftDrive1.getPositionWorld();
double right_distance = rightDrive1.getPositionWorld();
Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg());
lastPose = currentPose;
currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle);
left_encoder_prev_distance_ = left_distance;
right_encoder_prev_distance_ = right_distance;
}
public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) {
return Kinematics.integrateForwardKinematics(lastPose, left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle);
}
/**
* Path Markers are an optional functionality that name the various
* Waypoints in a Path with a String. This can make defining set locations
* much easier.
*
* @return Set of Strings with Path Markers that the robot has crossed.
*/
public synchronized Set<String> getPathMarkersCrossed() {
if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) {
return null;
} else {
return adaptivePursuitController.getMarkersCrossed();
}
}
public synchronized void setControlMode(DriveControlMode controlMode) {
this.controlMode = controlMode;
if (controlMode == DriveControlMode.JOYSTICK) {
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
}
else if (controlMode == DriveControlMode.MANUAL) {
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
}
else if (controlMode == DriveControlMode.CLIMB) {
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
}
else if (controlMode == DriveControlMode.HOLD) {
mpStraightController.setPID(mpHoldPIDParams);
//leftDrive1.changeControlMode(TalonControlMode.Position);
//leftDrive1.setPosition(0);
//leftDrive1.set(0);
//rightDrive1.changeControlMode(TalonControlMode.Position);
//rightDrive1.setPosition(0);
//rightDrive1.set(0);
leftDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
leftDrive1.set(ControlMode.Position, 0);
rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
rightDrive1.set(ControlMode.Position, 0);
}
isFinished = false;
}
public synchronized void controlLoopUpdate() {
// Measure *actual* update period
double currentTimestamp = Timer.getFPGATimestamp();
if (lastControlLoopUpdateTimestamp > 0.001) { // ie, if this is NOT the first time
lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
}
lastControlLoopUpdateTimestamp = currentTimestamp;
// Do the update
if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) {
driveWithJoystick();
}
else if (!isFinished) {
if (controlMode == DriveControlMode.MP_STRAIGHT) {
isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.MP_TURN) {
isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.PID_TURN) {
isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.MP_PATH) {
isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) {
isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.MOTION_MAGIC) {
isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) {
updatePose();
isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
}
}
}
public void setSpeed(double speed) {
if (speed == 0) {
setControlMode(DriveControlMode.JOYSTICK);
}
else {
setControlMode(DriveControlMode.MANUAL);
rightDrive1.set(-speed);
leftDrive1.set(speed);
}
}
public void setClimbSpeed(double climbSpeed) {
this.climbSpeed = climbSpeed;
if (climbSpeed == 0) {
setControlMode(DriveControlMode.JOYSTICK);
}
else {
setControlMode(DriveControlMode.CLIMB);
}
}
public void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) {
if (snapToAbsolute0or180) {
gyroLockAngleDeg = BHRMathUtils.adjustAccumAngleToClosest180(getGyroAngleDeg());
}
else {
gyroLockAngleDeg = getGyroAngleDeg();
}
this.useGyroLock = useGyroLock;
}
public void driveWithJoystick() {
if(m_drive == null) return;
// switch(m_controllerMode) {
// case CONTROLLER_JOYSTICK_ARCADE:
// m_moveInput = OI.getInstance().getJoystick1().getY();
// m_steerInput = OI.getInstance().getJoystick1().getX();
// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
// break;
// case CONTROLLER_JOYSTICK_TANK:
// m_moveInput = OI.getInstance().getJoystick1().getY();
// m_steerInput = OI.getInstance().getJoystick2().getY();
// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
// m_steerOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
// m_steerInput, m_moveNonLinear, MOVE_NON_LINEARITY);
// m_drive.tankDrive(m_moveOutput, m_steerOutput);
// break;
// case CONTROLLER_JOYSTICK_CHEESY:
// m_moveInput = OI.getInstance().getJoystick1().getY();
// m_steerInput = OI.getInstance().getJoystick2().getX();
// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
// break;
// case CONTROLLER_XBOX_CHEESY:
// boolean turbo = OI.getInstance().getDriveTrainController()
// .getLeftJoystickButton();
// boolean slow = OI.getInstance().getDriveTrainController()
// .getRightJoystickButton();
// double speedToUseMove, speedToUseSteer;
// if (turbo && !slow) {
// speedToUseMove = m_moveScaleTurbo;
// speedToUseSteer = m_steerScaleTurbo;
// } else if (!turbo && slow) {
// speedToUseMove = m_moveScaleSlow;
// speedToUseSteer = m_steerScaleSlow;
// } else {
// speedToUseMove = m_moveScale;
// speedToUseSteer = m_steerScale;
// }
// m_moveInput =
// OI.getInstance().getDriveTrainController().getLeftYAxis();
// m_steerInput =
// OI.getInstance().getDriveTrainController().getRightXAxis();
m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis();
m_steerInput = OI.getInstance().getDriverController().getRightXAxis();
if (controlMode == DriveControlMode.JOYSTICK) {
m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
}
else if (controlMode == DriveControlMode.CLIMB) {
m_moveOutput = climbSpeed;
}
if (useGyroLock) { // If currently in gyro lock mode,
if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) { // but stopped driving or started turning,
setGyroLock(false, false); // turn off gyro lock mode
}
} else { // If not yet in gyro lock mode,
if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) { // but just started driving without turning,
setGyroLock(true, false); // gyro lock to current heading
}
}
if (useGyroLock) {
double yawError = gyroLockAngleDeg - getGyroAngleDeg();
m_steerOutput = kPGyro * yawError;
} else {
m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
}
m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75);
// break;
// case CONTROLLER_XBOX_ARCADE_RIGHT:
// m_moveInput =
// OI.getInstance().getDrivetrainController().getRightYAxis();
// m_steerInput =
// OI.getInstance().getDrivetrainController().getRightXAxis();
// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
// break;
// case CONTROLLER_XBOX_ARCADE_LEFT:
// m_moveInput =
// OI.getInstance().getDrivetrainController().getLeftYAxis();
// m_steerInput =
// OI.getInstance().getDrivetrainController().getLeftXAxis();
// m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
// m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
// m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
// m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
// m_drive.arcadeDrive(m_moveOutput, m_steerOutput);
// break;
// }
}
public void rawMoveSteer(double move, double steer) {
m_drive.arcadeDrive(move, steer, false);
}
public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) {
leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput);
rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput);
}
private boolean inDeadZone(double input) {
boolean inDeadZone;
if (Math.abs(input) < STICK_DEADBAND) {
inDeadZone = true;
} else {
inDeadZone = false;
}
return inDeadZone;
}
public double adjustForSensitivity(double scale, double trim,
double steer, int nonLinearFactor, double wheelNonLinearity) {
if (inDeadZone(steer))
return 0;
steer += trim;
steer *= scale;
steer = limitValue(steer);
int iterations = Math.abs(nonLinearFactor);
for (int i = 0; i < iterations; i++) {
if (nonLinearFactor > 0) {
steer = nonlinearStickCalcPositive(steer, wheelNonLinearity);
} else {
steer = nonlinearStickCalcNegative(steer, wheelNonLinearity);
}
}
return steer;
}
private double limitValue(double value) {
if (value > 1.0) {
value = 1.0;
} else if (value < -1.0) {
value = -1.0;
}
return value;
}
private double nonlinearStickCalcPositive(double steer,
double steerNonLinearity) {
return Math.sin(Math.PI / 2.0 * steerNonLinearity * steer)
/ Math.sin(Math.PI / 2.0 * steerNonLinearity);
}
private double nonlinearStickCalcNegative(double steer,
double steerNonLinearity) {
return Math.asin(steerNonLinearity * steer)
/ Math.asin(steerNonLinearity);
}
//public void setShiftState(SpeedShiftState state) {
// if(state == SpeedShiftState.HI) {
// speedShift.set(true);
// }
// else if(state == SpeedShiftState.LO) {
// speedShift.set(false);
// }
//}
public synchronized boolean isFinished() {
return isFinished;
}
public synchronized void setFinished(boolean isFinished) {
this.isFinished = isFinished;
}
@Override
public void setPeriodMs(long periodMs) {
//mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers);
mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers);
mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers);
pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers);
mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers);
mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers);
adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers);
this.periodMs = periodMs;
}
public double getPeriodMs() {
return periodMs;
}
public boolean isRed() {
return isRed;
}
public void setIsRed(boolean status) {
isRed = status;
}
public static double rotationsToInches(double rotations) {
return rotations * (Constants.kDriveWheelDiameterInches * Math.PI);
}
public static double rpmToInchesPerSecond(double rpm) {
return rotationsToInches(rpm) / 60;
}
public static double inchesToRotations(double inches) {
return inches / (Constants.kDriveWheelDiameterInches * Math.PI);
}
public static double inchesPerSecondToRpm(double inches_per_second) {
return inchesToRotations(inches_per_second) * 60;
}
public double getLeftPositionWorld() {
return leftDrive1.getPositionWorld();
}
public double getRightPositionWorld() {
return rightDrive1.getPositionWorld();
}
public void updateStatus(Robot.OperationMode operationMode) {
if (operationMode == Robot.OperationMode.TEST) {
try {
SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld());
SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld());
SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12);
SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12);
//SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID));
//SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID));
// SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID));
//SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID));
//SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID));
// SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID));
//SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn());
//SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn());
SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD);
//SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg());
SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg());
MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint();
double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0;
SmartDashboard.putNumber("Gyro Delta", delta);
//SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating);
SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating());
SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg);
SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg());
SmartDashboard.putNumber("Steer Output", m_steerOutput);
SmartDashboard.putNumber("Move Output", m_moveOutput);
SmartDashboard.putNumber("Steer Input", m_steerInput);
SmartDashboard.putNumber("Move Input", m_moveInput);
}
catch (Exception e) {
System.err.println("Drivetrain update status error");
}
}
else {
}
}
}
src/buttons/XBoxTriggerButton.java
-4
View File
@@ -4,10 +4,6 @@ import org.usfirst.frc4388.controller.XboxController;
import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.Button;
/**
*
* @author bselle
*/
public class XBoxTriggerButton extends Button public class XBoxTriggerButton extends Button
{ {
public static final int RIGHT_TRIGGER = 0; public static final int RIGHT_TRIGGER = 0;
src/org/usfirst/frc4388/controller/XboxController.java
+1 -5
View File
@@ -1,12 +1,8 @@
/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package org.usfirst.frc4388.controller; package org.usfirst.frc4388.controller;
import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Joystick;
//TODO Code support for Start/Back/XBox(?) buttons.
/** /**
* This is a wrapper for the WPILib Joystick class that represents an XBox * This is a wrapper for the WPILib Joystick class that represents an XBox
* controller. * controller.
src/org/usfirst/frc4388/robot/Constants.java
+2 -13
View File
@@ -1,16 +1,14 @@
package org.usfirst.frc4388.robot; package org.usfirst.frc4388.robot;
//import org.usfirst.frc4388.utility.ConstantsBase;
/** /**
* A list of constants used by the rest of the robot code. This include physics * A list of constants used by the rest of the robot code. This include physics
* constants as well as constants determined through calibrations. * constants as well as constants determined through calibrations.
*/ */
//public class Constants extends ConstantsBase {
public class Constants { public class Constants {
// Wheels
//public static double kDriveWheelDiameterInches = 5.9;
public static double kDriveWheelDiameterInches = 6.04; public static double kDriveWheelDiameterInches = 6.04;
public static double kTrackLengthInches = 10; public static double kTrackLengthInches = 10;
public static double kTrackWidthInches = 26.5; public static double kTrackWidthInches = 26.5;
@@ -33,7 +31,6 @@ public class Constants {
// Elevator // Elevator
public static int kElevatorEncoderTickesPerRev = 256; public static int kElevatorEncoderTickesPerRev = 256;
public static double kElevatorInchesOfTravelPerRev = 3.75; public static double kElevatorInchesOfTravelPerRev = 3.75;
// public static double kElevatorEncoderTicksPerInch = (double)kElevatorEncoderTickesPerRev / kElevatorInchesOfTravelPerRev;
public static double kElevatorEncoderTicksPerInch = 126.36; public static double kElevatorEncoderTicksPerInch = 126.36;
public static double kElevatorBasicPercentOutputUp = -0.85; public static double kElevatorBasicPercentOutputUp = -0.85;
public static double kElevatorBasicPercentOutputDown =.7; public static double kElevatorBasicPercentOutputDown =.7;
@@ -72,12 +69,4 @@ public class Constants {
public static double kPathFollowingMaxVel = 120.0; // inches/sec public static double kPathFollowingMaxVel = 120.0; // inches/sec
public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2 public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2
// @Override
// public String getFileLocation() {
// return "~/constants.txt";
// }
// static {
// new Constants().loadFromFile();
// }
} }
src/org/usfirst/frc4388/robot/OI.java
+4 -387
View File
@@ -3,8 +3,6 @@
package org.usfirst.frc4388.robot; package org.usfirst.frc4388.robot;
//
//import org.usfirst.frc4388.buttons.XBoxDPadTriggerButton;
import buttons.XBoxTriggerButton; import buttons.XBoxTriggerButton;
import org.usfirst.frc4388.controller.IHandController; import org.usfirst.frc4388.controller.IHandController;
import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.controller.XboxController;
@@ -44,39 +42,6 @@ public class OI
m_driverXbox = new XboxController(RobotMap.DRIVER_JOYSTICK_1_USB_ID); m_driverXbox = new XboxController(RobotMap.DRIVER_JOYSTICK_1_USB_ID);
m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID); m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID);
//ELevator PID buttons
//JoystickButton elevatorMaxScaleHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON);
//elevatorMaxScaleHeight.whenPressed(new PositionPIDMaxScaleHeight(50));
//JoystickButton elevatorLowScaleHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
//elevatorLowScaleHeight.whenPressed(new PositionPIDLowScaleHeight(50));
//JoystickButton elevatorSwitchHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
//elevatorSwitchHeight.whenPressed(new PositionPIDLowScaleHeight(50));
/*
JoystickButton elevatorLowestHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
elevatorLowestHeight.whenPressed(new PositionPIDLowScaleHeight(50));
*/
//JoystickButton shiftDrivetrain = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
//shiftDrivetrain.whenPressed(new DriveSpeedShift(Drive.SpeedShiftState.HI));
//shiftDrivetrain.whenReleased(new DriveSpeedShift(Drive.SpeedShiftState.LO));
//JoystickButton longShot = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
//longShot.whenPressed(new ShootOn(ShotState.FAR, ShooterFeed.SHOOTER_FEED_SHOOT_FAR_SPEED, false));
//longShot.whenReleased(new ShootOff());
//XBoxTriggerButton shortShot = new XBoxTriggerButton(m_driverXbox, XBoxTriggerButton.RIGHT_TRIGGER);
//shortShot.whenPressed(new ShootOn(ShotState.CLOSE, ShooterFeed.SHOOTER_FEED_SHOOT_CLOSE_SPEED, false));
//shortShot.whenReleased(new ShootOff());
XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER); XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER);
CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
CarriageIntake.whenReleased(new IntakeSetSpeed(0.0)); CarriageIntake.whenReleased(new IntakeSetSpeed(0.0));
@@ -85,21 +50,6 @@ public class OI
CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
CarriageEject.whenReleased(new IntakeSetSpeed(0.0)); CarriageEject.whenReleased(new IntakeSetSpeed(0.0));
//JoystickButton gearIntakeDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.X_BUTTON);
//gearIntakeDown.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_INTAKE));
//gearIntakeDown.whenReleased(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
//JoystickButton gearIntakeRetract = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
//gearIntakeRetract.whenPressed(new IntakeSetPosition(IntakePosition.RETRACT));
//JoystickButton gearIntakePresent = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.B_BUTTON);
//gearIntakePresent.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
//JoystickButton gearIntakeDeploy = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.A_BUTTON);
//gearIntakeDeploy.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
climbUp.whenPressed(new InitiateClimber(true)); climbUp.whenPressed(new InitiateClimber(true));
climbUp.whenReleased(new InitiateClimber(false)); climbUp.whenReleased(new InitiateClimber(false));
@@ -113,349 +63,16 @@ public class OI
shiftDown.whenPressed(new LEDIndicators(false)); shiftDown.whenPressed(new LEDIndicators(false));
//XBoxDPadTriggerButton climberReverse = new XBoxDPadTriggerButton(m_driverXbox, XBoxDPadTriggerButton.DOWN); //Operator Xbox
//climberReverse.whenPressed(new ClimberSetSpeed(0.48, 0.0));
//climberReverse.whenReleased(new ClimberSetSpeed(0.0, 0.0));
//JoystickButton toggleShooter = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.START_BUTTON);
//toggleShooter.whenPressed(new ShooterSetToggle(Shooter.SHOOTER_STAGE1_RPM_CLOSE, Shooter.SHOOTER_STAGE2_RPM_CLOSE));
JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
openIntake.whenPressed(new IntakePosition(true)); openIntake.whenPressed(new IntakePosition(true));
JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
CloseIntake.whenPressed(new IntakePosition(false)); CloseIntake.whenPressed(new IntakePosition(false));
//XBoxTriggerButton shortShotOperator = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER); SmartDashboard.putData("PRE GAME!!!!", new PreGame());
//shortShotOperator.whenPressed(new ShootOn(ShotState.CLOSE, ShooterFeed.SHOOTER_FEED_SHOOT_CLOSE_SPEED, true));
//shortShotOperator.whenReleased(new ShootOff());
//XBoxTriggerButton intakeGearOperator = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER);
//intakeGearOperator.whenPressed(new GearIntakeRollerSetSpeed(GearIntake.GEAR_INTAKE_LOAD_SPEED));
//intakeGearOperator.whenReleased(new GearIntakeRollerSetSpeed(0.0));
//JoystickButton gearIntakeEjectOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
//gearIntakeEjectOperator.whenPressed(new GearIntakeRollerSetSpeed(GearIntake.GEAR_INTAKE_EJECT_SPEED));
//gearIntakeEjectOperator.whenReleased(new GearIntakeRollerSetSpeed(0.0));
//JoystickButton gearIntakeDownOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
//gearIntakeDownOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_INTAKE));
//gearIntakeDownOperator.whenReleased(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
//JoystickButton gearIntakeRetractOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON);
//gearIntakeRetractOperator.whenPressed(new IntakeSetPosition(IntakePosition.RETRACT));
//JoystickButton gearIntakePresentOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
//gearIntakePresentOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
//JoystickButton gearIntakeDeployOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
//gearIntakeDeployOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
//XBoxDPadTriggerButton climberForwardOperator = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.UP);
//climberForwardOperator.whenPressed(new ClimberSetSpeed(ShooterFeed.CLIMB_SPEED, 0.0));
//climberForwardOperator.whenReleased(new ClimberSetSpeed(0.0, 0.0));
//XBoxDPadTriggerButton climberReverseOperator = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.DOWN);
//climberReverseOperator.whenPressed(new ClimberSetSpeed(0.48, 0.0));
//climberReverseOperator.whenReleased(new ClimberSetSpeed(0.0, 0.0));
//XBoxDPadTriggerButton hopperOpen = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.LEFT);
//hopperOpen.whenPressed(new ShooterSetHopperPosition(HopperState.OPEN));
//XBoxDPadTriggerButton hopperClose = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.RIGHT);
//hopperClose.whenPressed(new ShooterSetHopperPosition(HopperState.CLOSE));
//JoystickButton toggleShooterOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON);
//toggleShooterOperator.whenPressed(new ShooterSetToggle(Shooter.SHOOTER_STAGE1_RPM_CLOSE, Shooter.SHOOTER_STAGE2_RPM_CLOSE));
// Gear sensor switch
// GearSensorAnalogSwitch gearSwitch = new GearSensorAnalogSwitch();
// gearSwitch.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
// SmartDashboard
// Button driveMP = new InternalButton();
// driveMP.whenPressed(new DriveStraightMP(40, 20, true, false, 0));
// SmartDashboard.putData("Drive Straight", driveMP);
//SmartDashboard.putData("Drive Straight 30in", new DriveStraightMP(30, 20, true, true, 0));
//SmartDashboard.putData("Drive Straight 60in", new DriveStraightMP(60, 20, true, true, 0));
//SmartDashboard.putData("Basic Straight 60in", new DriveStraightBasic(60, 30, true, true, 0));
//SmartDashboard.putData("Basic Straight -60in", new DriveStraightBasic(-60, 30, true, true, 0));
//SmartDashboard.putData("Turn abs 90 TANK", new DriveTurnBasic(true, 90, 90, MPSoftwareTurnType.TANK));
// SmartDashboard.putData("Turn rel -90 RIGHT ONLY", new DriveTurnBasic(false, -90, 90, MPSoftwareTurnType.RIGHT_SIDE_ONLY));
//SmartDashboard.putData("RUN INTAKE", new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
// SmartDashboard.putData("Stop INTAKE", new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
//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));
///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED));
///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); // forward , back to right
//SmartDashboard.putData("MP Move", new DriveStraightMP(-50, 30, true, true, 0)); // forward, back to the right
//SmartDashboard.putData("MP Move", new DriveStraightMP(-40, 30, true, true, 0)); //same af -50
//SmartDashboard.putData("MP Move", new DriveStraightMP(-30, 30, true, true, 0));// same as above shorter
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 20, true, true, 0));// could not break ininita
//SmartDashboard.putData("MP Move", new DriveStraightMP(100, 20, true, true, 0));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
//SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0));
//List<Waypoint> waypoints = new ArrayList<>();
//waypoints.add(new Waypoint(new Translation2d(0, 0), 35.0));
//waypoints.add(new Waypoint(new Translation2d(-35, 0), 35.0));
//Path.addCircleArc(waypoints, -30.0, 45.0, 10, "hopperSensorOn");
//waypoints.add(new Waypoint(new Translation2d(-85, -30), 35.0));
// List<Waypoint> waypoints = new ArrayList<>();
// waypoints.add(new Waypoint(new Translation2d(-96, 0), 50.0));
// waypoints.add(new Waypoint(new Translation2d(0, 0), 35.0));
// waypoints.add(new Waypoint(new Translation2d(-96, 0), 50.0));
// waypoints.add(new Waypoint(new Translation2d(-61, 0), 50.0));
// waypoints.add(new Waypoint(new Translation2d(-84.93, -10.16), 50.0));
// waypoints.add(new Waypoint(new Translation2d(-41, 0), 35.0));
// waypoints.add(new Waypoint(new Translation2d(-65, -23), 35.0));
// waypoints.add(new Waypoint(new Translation2d(-70, -39), 35.0));
// waypoints.add(new Waypoint(new Translation2d(-29, 0), 40.0));
// Path.addCircleArc(waypoints, -30.0, 45.0, 10, null);
// waypoints.add(new Waypoint(new Translation2d(-68.6, -26.5), 40.0));
//Button driveAP = new InternalButton();
//driveAP.whenPressed(new DrivePathAdaptivePursuit(new Path(waypoints), true));
//SmartDashboard.putData("Drive Adaptive Pursuit", driveAP);
//Button driveMM = new InternalButton();
//driveMM.whenPressed(new DriveStraightMM(10, 450, 450, true, false, 0));
//SmartDashboard.putData("Drive StraightMM", driveMM);
//Button turnRelativePID = new InternalButton();
//turnRelativePID.whenPressed(new DriveRelativeTurnPID(-10, MPSoftwareTurnType.RIGHT_SIDE_ONLY));
//SmartDashboard.putData("Turn Relative 10 PID", turnRelativePID);
//Button turnRelativePID7 = new InternalButton();
//turnRelativePID7.whenPressed(new DriveRelativeTurnPID(7, MPSoftwareTurnType.LEFT_SIDE_ONLY));
//SmartDashboard.putData("Turn Relative 7 PID", turnRelativePID7);
//Button turnRelativeMP = new InternalButton();
//turnRelativeMP.whenPressed(new DriveRelativeTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY ));
//SmartDashboard.putData("Turn Relative 10", turnRelativeMP);
/*
Button turnAbsoluteMP = new InternalButton();
turnAbsoluteMP.whenPressed(new DriveAbsoluteTurnMP(45, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY));
SmartDashboard.putData("Turn Absolute 45", turnAbsoluteMP);
Button turnRelativePID = new InternalButton();
turnRelativePID.whenPressed(new DriveRelativeTurnPID(45, MPSoftwareTurnType.TANK));
SmartDashboard.putData("Turn Relative 45", turnRelativePID);
*/
// Button gyroReset = new InternalButton();
// gyroReset.whenPressed(new DriveGyroReset());
// SmartDashboard.putData("Gyro Reset", gyroReset);
SmartDashboard.putData("Reset Gyro", new DriveGyroReset());
//SmartDashboard.putData("reset Encoders", new drive.resetEncoders());
SmartDashboard.putData("clampy boi", new IntakePosition(true));
// Camera
//Button cameraUpdateDashboard = new InternalButton();
//cameraUpdateDashboard.whenPressed(new CameraUpdateDashboard());
//SmartDashboard.putData("Camera Update", cameraUpdateDashboard);
//Button cameraSaveImage = new InternalButton();
//cameraSaveImage.whenPressed(new CameraSaveImage());
//SmartDashboard.putData("Camera Save", cameraSaveImage);
//Button cameraTurnToBestTarget = new InternalButton();
//cameraTurnToBestTarget.whenPressed(new CameraTurnToBestTarget());
//SmartDashboard.putData("Camera Turn To Best", cameraTurnToBestTarget);
//Button cameraUpdateBestTarget = new InternalButton();
//cameraUpdateBestTarget.whenPressed(new CameraUpdateBestTarget());
//SmartDashboard.putData("Camera Update Best", cameraUpdateBestTarget);
//Button incrementCameraOffsetPos = new InternalButton();
//incrementCameraOffsetPos.whenPressed(new CameraIncrementOffset(0.5));
//SmartDashboard.putData("Camera Offset Pos", incrementCameraOffsetPos);
//Button incrementCameraOffsetNeg = new InternalButton();
//incrementCameraOffsetNeg.whenPressed(new CameraIncrementOffset(-0.5));
//SmartDashboard.putData("Camera Offset Neg", incrementCameraOffsetNeg);
// Button cameraReadImage = new InternalButton();
// cameraReadImage.whenPressed(new CameraReadAndProcessImage());
// SmartDashboard.putData("Camera Read", cameraReadImage);
//
// Button cameraReadImageTurnToBestTarget = new InternalButton();
// cameraReadImageTurnToBestTarget.whenPressed(new CameraReadImageTurnToBestTarget());
// SmartDashboard.putData("Camera Read Turn", cameraReadImageTurnToBestTarget);
//
// Button magicCarpetOn05 = new InternalButton();
// magicCarpetOn05.whenPressed(new ZarkerFeedSetSpeed(0.5));
// SmartDashboard.putData("Magic Carpet 0.5", magicCarpetOn05);
//
// Button magicCarpetOff = new InternalButton();
// magicCarpetOff.whenPressed(new ZarkerFeedSetSpeed(0.0));
// SmartDashboard.putData("Magic Carpet Off", magicCarpetOff);
//
// Button shooterFeedOn10 = new InternalButton();
// shooterFeedOn10.whenPressed(new ShooterFeedSetSpeed(1.0));
// SmartDashboard.putData("Shooter Feed 1.0", shooterFeedOn10);
//
// Button shooterFeedOn05 = new InternalButton();
// shooterFeedOn05.whenPressed(new ShooterFeedSetSpeed(0.5));
// SmartDashboard.putData("Shooter Feed 0.5", shooterFeedOn05);
//
// Button shooterFeedOff = new InternalButton();
// shooterFeedOff.whenPressed(new ShooterFeedSetSpeed(0.0));
// SmartDashboard.putData("Shooter Feed Off", shooterFeedOff);
//
// Button shooterStage2RPMOnDash = new InternalButton();
// shooterStage2RPMOnDash.whenPressed(new ShooterStage2SetRpmDashboard());
// SmartDashboard.putData("Shooter Stage 2 Dash RPM", shooterStage2RPMOnDash);
//
// Button shooterStage2On10 = new InternalButton();
// shooterStage2On10.whenPressed(new ShooterStage2SetSpeed(1.0));
// SmartDashboard.putData("Shooter Stage 2 - 1.0", shooterStage2On10);
//
// Button shooterStage2On09 = new InternalButton();
// shooterStage2On09.whenPressed(new ShooterStage2SetSpeed(0.9));
// SmartDashboard.putData("Shooter Stage 2 - 0.9", shooterStage2On09);
//
// Button shooterStage2On08 = new InternalButton();
// shooterStage2On08.whenPressed(new ShooterStage2SetSpeed(0.8));
// SmartDashboard.putData("Shooter Stage 2 - 0.8", shooterStage2On08);
//
// Button shooterStage2On07 = new InternalButton();
// shooterStage2On07.whenPressed(new ShooterStage2SetSpeed(0.7));
// SmartDashboard.putData("Shooter Stage 2 - 0.7", shooterStage2On07);
//
// Button shooterStage2On06 = new InternalButton();
// shooterStage2On06.whenPressed(new ShooterStage2SetSpeed(0.6));
// SmartDashboard.putData("Shooter Stage 2 - 0.6", shooterStage2On06);
//
// Button shooterStage2On05 = new InternalButton();
// shooterStage2On05.whenPressed(new ShooterStage2SetSpeed(0.5));
// SmartDashboard.putData("Shooter Stage 2 - 0.5", shooterStage2On05);
//
// Button shooterStage2Off = new InternalButton();
// shooterStage2Off.whenPressed(new ShooterStage2SetSpeed(0.0));
// SmartDashboard.putData("Shooter Stage 2 - Off", shooterStage2Off);
//
// Button shooterStage1RPMOnDash = new InternalButton();
// shooterStage1RPMOnDash.whenPressed(new ShooterStage1SetRpmDashboard());
// SmartDashboard.putData("Shooter Stage 1 Dash RPM", shooterStage1RPMOnDash);
//
// Button shooterStage1On05 = new InternalButton();
// shooterStage1On05.whenPressed(new ShooterStage1SetSpeed(0.5));
// SmartDashboard.putData("Shooter Stage 1 - 0.5", shooterStage1On05);
//
// Button shooterStage1Off = new InternalButton();
// shooterStage1Off.whenPressed(new ShooterStage1SetSpeed(0.0));
// SmartDashboard.putData("Shooter Stage 1 - Off", shooterStage1Off);
//
// Button shooterBothRPMOnDash = new InternalButton();
// shooterBothRPMOnDash.whenPressed(new ShooterSetRpmDashboard());
// SmartDashboard.putData("Shooter Both Dash RPM", shooterBothRPMOnDash);
//
// Button shooterBothVBusOnDash = new InternalButton();
// shooterBothVBusOnDash.whenPressed(new ShooterSetSpeedDashboard());
// SmartDashboard.putData("Shooter Both Dash VBus", shooterBothVBusOnDash);
//
// Button allButLiftOff = new InternalButton();
// allButLiftOff.whenPressed(new ShooterAllOff());
// SmartDashboard.putData("Shooter All Off", allButLiftOff);
//
// Button allButLiftOn = new InternalButton();
// allButLiftOn.whenPressed(new ShooterAllOn());
// SmartDashboard.putData("Shooter All On", allButLiftOn);
//
// Button hopperShake = new InternalButton();
// hopperShake.whenPressed(new ShooterSetHopperShake(2, 10, 1));
// SmartDashboard.putData("Hopper Shake Button", hopperShake);
//
// Button intakeOn10 = new InternalButton();
// intakeOn10.whenPressed(new GearIntakeRollerSetSpeed(1.0));
// SmartDashboard.putData("Intake 1.0", intakeOn10);
//
// Button intakeOn08 = new InternalButton();
// intakeOn08.whenPressed(new GearIntakeRollerSetSpeed(0.8));
// SmartDashboard.putData("Intake 0.8", intakeOn08);
//
// Button intakeOn05 = new InternalButton();
// intakeOn05.whenPressed(new GearIntakeRollerSetSpeed(0.5));
// SmartDashboard.putData("Intake 0.5", intakeOn05);
//
// Button intakeOn04 = new InternalButton();
// intakeOn04.whenPressed(new GearIntakeRollerSetSpeed(0.4));
// SmartDashboard.putData("Intake 0.4", intakeOn04);
//
// Button intakeOff = new InternalButton();
// intakeOff.whenPressed(new GearIntakeRollerSetSpeed(0.0));
// SmartDashboard.putData("Intake Off", intakeOff);
//
// Button shooterShotPositionClose = new InternalButton();
// shooterShotPositionClose.whenPressed(new ShooterSetShotPosition(Shooter.ShotState.CLOSE));
// SmartDashboard.putData("Shooter Close Shot", shooterShotPositionClose);
//
// Button shooterShotPositionFar = new InternalButton();
// shooterShotPositionFar.whenPressed(new ShooterSetShotPosition(Shooter.ShotState.FAR));
// SmartDashboard.putData("Shooter Far Shot", shooterShotPositionFar);
//
// Button climberOn08 = new InternalButton();
// climberOn08.whenPressed(new ClimberSetSpeed(0.8, 0.0));
// climberOn08.whenReleased(new ClimberSetSpeed(0.0, 0.0));
// SmartDashboard.putData("Climber 0.8", climberOn08);
//
// Button climberSetMaxAmps = new InternalButton();
// climberSetMaxAmps.whenPressed(new ClimberSetMaxAmps(0.8, 20));
// SmartDashboard.putData("Climber Max Amps", climberSetMaxAmps);
//
//jaci.pathfinder.Waypoint[] points = new jaci.pathfinder.Waypoint[] {
// new jaci.pathfinder.Waypoint(0, 0, 0),
// new jaci.pathfinder.Waypoint(-90, -16, Pathfinder.d2r(-45))
//};
//PathGenerator path = new PathGenerator(points, 0.01, 160, 200.0, 700.0);
//Button pathTest = new InternalButton();
//pathTest.whenPressed(new DrivePathMP(path));
//SmartDashboard.putData("Path Test", pathTest);
// Button ledsOn = new InternalButton();
// ledsOn.whenPressed(new LEDLightsSet(true));
// SmartDashboard.putData("LEDs On", ledsOn);
//
// Button ledsOff = new InternalButton();
// ledsOff.whenPressed(new LEDLightsSet(false));
// SmartDashboard.putData("LEDs Off", ledsOff);
//
// Button intakeGearRoller = new InternalButton();
// intakeGearRoller.whenPressed(new GearIntakeRollerSetSpeed(0.3));
// SmartDashboard.putData("Gear Intake Clean Speed", intakeGearRoller);
//
// Button intakeGearStopRoller = new InternalButton();
// intakeGearStopRoller.whenPressed(new GearIntakeRollerSetSpeed(0.0));
// SmartDashboard.putData("Gear Intake Clean Off Speed", intakeGearStopRoller);
} catch (Exception e) { } catch (Exception e) {
System.err.println("An error occurred in the OI constructor"); System.err.println("An error occurred in the OI constructor");
} }
src/org/usfirst/frc4388/robot/Robot.java
+8 -82
View File
@@ -1,18 +1,6 @@
// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.
package org.usfirst.frc4388.robot; package org.usfirst.frc4388.robot;
//import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
@@ -35,13 +23,7 @@ import org.usfirst.frc4388.robot.subsystems.Drive;
import org.usfirst.frc4388.robot.subsystems.Carriage; 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;; 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
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the build.properties file in
* the project.
*/
public class Robot extends IterativeRobot public class Robot extends IterativeRobot
{ {
@@ -49,7 +31,7 @@ public class Robot extends IterativeRobot
public static final Drive drive = new Drive(); public static final Drive drive = new Drive();
//public static final Elevator elevator = new Elevator();
public static final Carriage carriage = new Carriage(); public static final Carriage carriage = new Carriage();
public static final Elevator elevator = new Elevator(); public static final Elevator elevator = new Elevator();
@@ -58,15 +40,8 @@ public class Robot extends IterativeRobot
public static final Climber climber = new Climber(); public static final Climber climber = new Climber();
public static final Pnumatics pnumatics = new Pnumatics(); public static final Pnumatics pnumatics = new Pnumatics();
//public static FlashyBlinky flashyBlinky;
//public static Sensors sensors;
public static final long periodMS = 10; public static final long periodMS = 10;
public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS); public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS);
//public static final PowerDistributionPanel pdp = new PowerDistributionPanel();
//public static final String ElevatorAuton = null;
public static enum OperationMode { TEST, COMPETITION }; public static enum OperationMode { TEST, COMPETITION };
public static OperationMode operationMode = OperationMode.TEST; public static OperationMode operationMode = OperationMode.TEST;
@@ -82,33 +57,20 @@ public class Robot extends IterativeRobot
private Command LRautonomousCommand; private Command LRautonomousCommand;
private Command LLautonomousCommand; private Command LLautonomousCommand;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
public void robotInit() public void robotInit()
{ {
//Printing game data to riolog //Printing game data to riolog
String gameData = DriverStation.getInstance().getGameSpecificMessage(); String gameData = DriverStation.getInstance().getGameSpecificMessage();
System.out.println(gameData); System.out.println(gameData);
CameraServer.getInstance().startAutomaticCapture(); CameraServer.getInstance().startAutomaticCapture();
CameraServer.getInstance().putVideo("res", 640, 480); CameraServer.getInstance().putVideo("res", 300, 220);
try { try {
//drive = new Drive();
//controlLoop = new ControlLooper("Main control loop", periodMS);
oi = OI.getInstance(); oi = OI.getInstance();
//camera.initialize();
controlLoop.addLoopable(drive); controlLoop.addLoopable(drive);
controlLoop.addLoopable(elevator); controlLoop.addLoopable(elevator);
// Waypoint[] points = new Waypoint[] {
// new Waypoint(0, 0, 0),
// new Waypoint(-95, -9, Pathfinder.d2r(-27))
// };
//
// PathGenerator boilerPath = new PathGenerator(points, 0.01, 120, 200.0, 700.0);
operationModeChooser = new SendableChooser<OperationMode>(); operationModeChooser = new SendableChooser<OperationMode>();
operationModeChooser.addDefault("Test", OperationMode.TEST); operationModeChooser.addDefault("Test", OperationMode.TEST);
@@ -200,11 +162,6 @@ public class Robot extends IterativeRobot
} }
} }
/**
* 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(){ public void disabledInit(){
} }
@@ -214,23 +171,12 @@ public class Robot extends IterativeRobot
updateStatus(); updateStatus();
} }
/**
* 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() { public void autonomousInit() {
updateChoosers(); updateChoosers();
// Schedule the autonomous command (example)
controlLoop.start(); controlLoop.start();
drive.endGyroCalibration(); drive.endGyroCalibration();
drive.resetEncoders(); drive.resetEncoders();
//elevator.resetElevatorEncoder();
drive.resetGyro(); drive.resetGyro();
drive.setIsRed(getAlliance().equals(Alliance.Red)); drive.setIsRed(getAlliance().equals(Alliance.Red));
@@ -265,46 +211,30 @@ public class Robot extends IterativeRobot
public void autonomousPeriodic() { public void autonomousPeriodic() {
Scheduler.getInstance().run(); Scheduler.getInstance().run();
//Robot.elevator.setControlMode(DriveControlMode.RAW);
updateStatus(); updateStatus();
} }
public void teleopInit() { 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 (RRautonomousCommand != null) RRautonomousCommand.cancel(); if (RRautonomousCommand != null) RRautonomousCommand.cancel();
if (RLautonomousCommand != null) RLautonomousCommand.cancel(); if (RLautonomousCommand != null) RLautonomousCommand.cancel();
if (LRautonomousCommand != null) LRautonomousCommand.cancel(); if (LRautonomousCommand != null) LRautonomousCommand.cancel();
if (LLautonomousCommand != null) LLautonomousCommand.cancel(); if (LLautonomousCommand != null) LLautonomousCommand.cancel();
drive.setToBrakeOnNeutral(false); // coast to avoid tipping when driver stops suddenly drive.setToBrakeOnNeutral(false);
//MotionProfileCache.getInstance().release();
updateChoosers(); updateChoosers();
controlLoop.start(); controlLoop.start();
drive.resetEncoders(); drive.resetEncoders();
drive.endGyroCalibration(); drive.endGyroCalibration();
//shooter.setStage1Speed(0);
//shooter.setStage2Speed(0);
//shooterFeed.setSpeed(0);
//zarkerFeed.setSpeed(0);
//shooter.setHopperPosition(HopperState.CLOSE);
//Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
updateStatus(); updateStatus();
} }
/**
* This function is called periodically during operator control
*/
public void teleopPeriodic() public void teleopPeriodic()
{ {
Scheduler.getInstance().run(); Scheduler.getInstance().run();
updateStatus(); updateStatus();
} }
/**
* This function is called periodically during test mode
*/
public void testPeriodic() { public void testPeriodic() {
LiveWindow.run(); LiveWindow.run();
updateStatus(); updateStatus();
@@ -325,11 +255,7 @@ public class Robot extends IterativeRobot
public void updateStatus() { public void updateStatus() {
drive.updateStatus(operationMode); drive.updateStatus(operationMode);
elevator.updateStatus(operationMode); elevator.updateStatus(operationMode);
//carriage.updateStatus(operationMode);
//shooter.updateStatus(operationMode);
//shooterFeed.updateStatus(operationMode);
//zarkerFeed.updateStatus(operationMode);
//camera.updateStatus(operationMode);
} }
} }
src/org/usfirst/frc4388/robot/RobotMap.java
+5 -17
View File
@@ -1,28 +1,23 @@
package org.usfirst.frc4388.robot; package org.usfirst.frc4388.robot;
/**
* 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 RobotMap { public class RobotMap {
// USB Port IDs // USB Port IDs
public static final int DRIVER_JOYSTICK_1_USB_ID = 0; public static final int DRIVER_JOYSTICK_1_USB_ID = 0;
public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; public static final int OPERATOR_JOYSTICK_1_USB_ID = 1;
//Controller IDs
// MOTORS // MOTORS
public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2; public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2;
public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3; public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3;
// public static final int DRIVETRAIN_LEFT_MOTOR3_CAN_ID = 2;
public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4;
public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5;
// public static final int DRIVETRAIN_RIGHT_MOTOR3_CAN_ID = 13;
//carriage motors
public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8; public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8;
public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9; public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9;
@@ -37,12 +32,5 @@ public class RobotMap {
public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1;
public static final int OPEN_INTAKE_PCM_ID = 4; public static final int OPEN_INTAKE_PCM_ID = 4;
public static final int CLOSE_INTAKE_PCM_ID = 5; public static final int CLOSE_INTAKE_PCM_ID = 5;
//public static final int HOPPER_POSITION_PCM_ID = 5;
// DIO
//public static final int HOPPER_SENSOR_RED_DIO_ID = 4;
//public static final int HOPPER_SENSOR_BLUE_DIO_ID = 5;
// Analog
//public static final int GEAR_SENSOR_ANALOG_ID = 0;
} }
src/org/usfirst/frc4388/robot/commands/PreGame.java
+26
View File
@@ -0,0 +1,26 @@
package org.usfirst.frc4388.robot.commands;
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.IntakePosition;
import edu.wpi.first.wpilibj.command.CommandGroup;
/**
*
*/
public class PreGame extends CommandGroup {
public PreGame() {
addSequential(new DriveSpeedShift(true));
addSequential(new IntakePosition(true));
addSequential(new DriveStraightBasic(-10, 50, true, true, 0));
}
}
src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java
+8 -10
View File
@@ -28,8 +28,8 @@ public class Cube2Right extends CommandGroup {
addSequential(new DriveSpeedShift(true)); addSequential(new DriveSpeedShift(true));
addSequential(new IntakePosition(true)); addSequential(new IntakePosition(true));
addSequential(new DriveStraightBasic(-300, 70, true, true, 0)); addSequential(new DriveStraightBasic(-259, 70, true, true, 0));
addSequential(new DriveStraightBasic(1, 30, true, true, 0));
addSequential(new ElevatorBasic(70)); addSequential(new ElevatorBasic(70));
addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK)); addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK));
addSequential(new WaitCommand(.5)); addSequential(new WaitCommand(.5));
@@ -39,24 +39,22 @@ public class Cube2Right extends CommandGroup {
addSequential(new WaitCommand(.5)); addSequential(new WaitCommand(.5));
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
addSequential(new DriveStraightBasic(-10, 30, true, true, 0)); addSequential(new DriveStraightBasic(-16, 40, true, true, 0));
addParallel(new ElevatorBasic(4)); addParallel(new ElevatorBasic(4));
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
addSequential(new DriveTurnBasic(true, -69, 100, MPSoftwareTurnType.TANK)); addSequential(new DriveTurnBasic(true, -65, 100, MPSoftwareTurnType.TANK));
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_FAST_SPEED)); addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_FAST_SPEED));
///////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////
addSequential(new DriveStraightBasic(10, 30, true, true, 0)); addSequential(new DriveStraightBasic(110, 70, true, true, 0));
addSequential(new DriveStraightBasic(80, 60, true, true, 0)); addSequential(new DriveStraightBasic(42, 60, true, true, 0));
addSequential(new DriveStraightBasic(33, 20, true, true, 0));
///////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////
addSequential(new WaitCommand(.5));
addSequential(new IntakePosition(true)); addSequential(new IntakePosition(true));
//addSequential(new WaitCommand(.3));
addSequential(new WaitCommand(.2));
addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
addParallel(new TimeoutBecaseYea()); addParallel(new TimeoutBecaseYea());
addSequential(new ElevatorBasic(28)); addSequential(new ElevatorBasic(28));
src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java
+2 -2
View File
@@ -38,9 +38,9 @@ public class RightStartLeftScore extends CommandGroup {
addSequential(new DriveStraightBasic(155, 60, true, true, 0)); addSequential(new DriveStraightBasic(155, 60, true, true, 0));
addSequential(new ElevatorBasic(30)); addSequential(new ElevatorBasic(30));
addSequential(new WaitCommand(.2)); addSequential(new WaitCommand(.2));
addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK)); addSequential(new DriveTurnBasic(true, -85, 150, MPSoftwareTurnType.TANK));
addParallel(new TimeoutBecaseYea()); addParallel(new TimeoutBecaseYea());
addSequential(new DriveStraightBasic(10, 60, true, true, 0)); addSequential(new DriveStraightBasic(15, 60, true, true, 0));
addSequential(new WaitCommand(3)); addSequential(new WaitCommand(3));
src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java
+2 -1
View File
@@ -31,10 +31,11 @@ public class ScaleFrom3 extends CommandGroup {
addSequential(new DriveStraightBasic(-10, 20, true, true, 0)); addSequential(new DriveStraightBasic(-10, 20, true, true, 0));
addSequential(new DriveStraightBasic(-270, 45, true, true, 0)); addSequential(new DriveStraightBasic(-270, 60, true, true, 0));
addSequential(new ElevatorBasic(70)); addSequential(new ElevatorBasic(70));
addSequential(new DriveStraightBasic(-30, 20, true, true, 0)); addSequential(new DriveStraightBasic(-30, 20, true, true, 0));
addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK)); addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK));
addSequential(new DriveStraightBasic(10, 20, true, true, 0));
//addSequential(new DriveStraightBasic(5, 20, true, true, 0)); //addSequential(new DriveStraightBasic(5, 20, true, true, 0));
addSequential(new WaitCommand(.5)); addSequential(new WaitCommand(.5));
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java
+1 -1
View File
@@ -24,7 +24,7 @@ public class TimeoutBecaseYea extends CommandGroup {
public TimeoutBecaseYea() public TimeoutBecaseYea()
{ {
addSequential(new WaitCommand(1.5)); addSequential(new WaitCommand(1.3));
addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
//addSequential(new WaitCommand(.1)); //addSequential(new WaitCommand(.1));
addSequential(new IntakePosition(false)); addSequential(new IntakePosition(false));
src/org/usfirst/frc4388/robot/subsystems/Drive.java
+12 -2
View File
@@ -39,6 +39,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.kauailabs.navx.frc.AHRS; import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.DoubleSolenoid;
//import edu.wpi.first.wpilibj.DigitalInput; //import edu.wpi.first.wpilibj.DigitalInput;
//import edu.wpi.first.wpilibj.DriverStation; //import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SPI;
@@ -110,6 +111,7 @@ public class Drive extends Subsystem implements ControlLoopable
//private DigitalInput hopperSensorBlue; //private DigitalInput hopperSensorBlue;
private double climbSpeed; private double climbSpeed;
private boolean isRed = true; private boolean isRed = true;
@@ -208,6 +210,7 @@ public class Drive extends Subsystem implements ControlLoopable
rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID);
// rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); // rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID);
elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
//gyroPigeon = new PigeonImu(leftDrive2); //gyroPigeon = new PigeonImu(leftDrive2);
gyroNavX = new AHRS(SPI.Port.kMXP); gyroNavX = new AHRS(SPI.Port.kMXP);
@@ -687,8 +690,15 @@ public class Drive extends Subsystem implements ControlLoopable
} }
public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) {
leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput);
rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput); if (elevatorRight.getSelectedSensorPosition(0) >= 3550) {
leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5);
rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5);
}
else /*(elevatorRight.getSelectedSensorPosition(0) < 3550)*/ {
leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput);
rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput);
}
} }
private boolean inDeadZone(double input) { private boolean inDeadZone(double input) {