mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 00:28:05 -06:00
Merge branch 'master' into Wrist
This commit is contained in:
mayabartels
@@ -29,11 +29,11 @@ public class Constants {
|
||||
public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI);
|
||||
|
||||
// Elevator
|
||||
public static int kElevatorEncoderTickesPerRev = 256;
|
||||
public static double kElevatorInchesOfTravelPerRev = 3.75;
|
||||
public static double kElevatorEncoderTicksPerInch = 126.36;
|
||||
public static double kElevatorBasicPercentOutputUp = -0.85;
|
||||
public static double kElevatorBasicPercentOutputDown =.7;
|
||||
public static int kArmEncoderTickesPerRev = 256;
|
||||
public static double kArmInchesOfTravelPerRev = 3.75;
|
||||
public static double kArmEncoderTicksPerInch = 126.36;
|
||||
public static double kArmBasicPercentOutputUp = -0.85;
|
||||
public static double kArmBasicPercentOutputDown =.7;
|
||||
|
||||
// CONTROL LOOP GAINS
|
||||
|
||||
|
||||
@@ -32,10 +32,7 @@ public class Robot extends IterativeRobot
|
||||
|
||||
|
||||
|
||||
public static final Elevator elevator = new Elevator();
|
||||
|
||||
|
||||
|
||||
public static final Arm arm = new Arm();
|
||||
public static final Climber climber = new Climber();
|
||||
public static final Pnumatics pnumatics = new Pnumatics();
|
||||
public static final Wrist wrist = new Wrist();
|
||||
@@ -68,7 +65,7 @@ public class Robot extends IterativeRobot
|
||||
oi = OI.getInstance();
|
||||
|
||||
controlLoop.addLoopable(drive);
|
||||
controlLoop.addLoopable(elevator);
|
||||
controlLoop.addLoopable(arm);
|
||||
|
||||
|
||||
operationModeChooser = new SendableChooser<OperationMode>();
|
||||
@@ -150,14 +147,15 @@ public class Robot extends IterativeRobot
|
||||
LLautonomousCommand = (Command)LLautonTaskChooser.getSelected();
|
||||
}
|
||||
|
||||
public Alliance getAlliance() {
|
||||
public Alliance getAlliance()
|
||||
{
|
||||
return m_ds.getAlliance();
|
||||
}
|
||||
|
||||
public void updateStatus() {
|
||||
public void updateStatus()
|
||||
{
|
||||
drive.updateStatus(operationMode);
|
||||
elevator.updateStatus(operationMode);
|
||||
|
||||
arm.updateStatus(operationMode);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -21,9 +21,9 @@ public class RobotMap {
|
||||
public static final int WRIST_LEFT_MOTOR_CAN_ID = 8;
|
||||
public static final int WRITST_RIGHT_MOTOR_CAN_ID = 9;
|
||||
|
||||
//Elevator Motors
|
||||
public static final int ELEVATOR_MOTOR1_ID = 6;
|
||||
public static final int ELEVATOR_MOTOR2_ID = 7;
|
||||
//Arm Motors
|
||||
public static final int ARM_MOTOR1_ID = 6;
|
||||
public static final int ARM_MOTOR2_ID = 7;
|
||||
public static final int CLIMBER_CAN_ID = 10;
|
||||
|
||||
|
||||
|
||||
+11
-11
@@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.command.Command;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
|
||||
public class ElevatorBasic extends Command {
|
||||
public class ArmBasic extends Command {
|
||||
private double m_targetHeightInchesAboveFloor;
|
||||
private double m_speed;
|
||||
private boolean m_goingUp;
|
||||
@@ -18,25 +18,25 @@ public class ElevatorBasic extends Command {
|
||||
private double m_lastCommandExecutePeriod = 0.0;
|
||||
public static boolean isfinishedElevatorBasic;
|
||||
|
||||
public ElevatorBasic(double targetHeightInchesAboveFloor) {
|
||||
requires(Robot.elevator);
|
||||
public ArmBasic(double targetHeightInchesAboveFloor) {
|
||||
requires(Robot.arm);
|
||||
m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor;
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize()
|
||||
{
|
||||
Robot.elevator.setControlMode(DriveControlMode.RAW);
|
||||
Robot.arm.setControlMode(DriveControlMode.RAW);
|
||||
|
||||
double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
|
||||
double currentHeight = Robot.arm.getArmHeightInchesAboveFloor();
|
||||
// start out at half speed, as crude way to reduce slippage
|
||||
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;
|
||||
m_speed = Constants.kArmBasicPercentOutputUp;
|
||||
}
|
||||
else {
|
||||
m_speed = Constants.kElevatorBasicPercentOutputDown;
|
||||
m_speed = Constants.kArmBasicPercentOutputDown;
|
||||
}
|
||||
m_commandInitTimestamp = Timer.getFPGATimestamp();
|
||||
|
||||
@@ -50,14 +50,14 @@ System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targe
|
||||
m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
|
||||
}
|
||||
m_lastCommandExecuteTimestamp = currentTimestamp;
|
||||
Robot.elevator.rawSetOutput(m_speed);
|
||||
Robot.arm.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 currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
|
||||
double currentHeight = Robot.arm.getArmHeightInchesAboveFloor();
|
||||
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) {
|
||||
@@ -86,9 +86,9 @@ System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , ta
|
||||
|
||||
isfinishedElevatorBasic = isFinished();
|
||||
|
||||
Robot.elevator.rawSetOutput(0.0);
|
||||
Robot.arm.rawSetOutput(0.0);
|
||||
|
||||
Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
|
||||
Robot.arm.setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
+2
-2
@@ -9,12 +9,12 @@ import edu.wpi.first.wpilibj.command.Command;
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class ElevatorSetSpeed extends Command {
|
||||
public class ArmSetSpeed extends Command {
|
||||
|
||||
private double RaiseSpeed;
|
||||
|
||||
// Constructor with speed
|
||||
public ElevatorSetSpeed(double RaiseSpeed) {
|
||||
public ArmSetSpeed(double RaiseSpeed) {
|
||||
this.RaiseSpeed = RaiseSpeed;
|
||||
// requires(Robot.elevatorAuton);
|
||||
}
|
||||
+87
-113
@@ -31,11 +31,11 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.SensorCollection;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
|
||||
public class Elevator extends Subsystem implements ControlLoopable
|
||||
public class Arm extends Subsystem implements ControlLoopable
|
||||
{
|
||||
//PID encoder and motor
|
||||
private CANTalonEncoder elevatorRight;
|
||||
private WPI_TalonSRX elevatorLeft;
|
||||
private CANTalonEncoder armMotor;
|
||||
//private WPI_TalonSRX ArmLeft;
|
||||
|
||||
//PID controller Max Scale
|
||||
private SoftwarePIDPositionController pidPositionControllerMaxScale;
|
||||
@@ -61,7 +61,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.kElevatorEncoderTicksPerInch;
|
||||
public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch;
|
||||
|
||||
//control mode for joystick control
|
||||
private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
|
||||
@@ -84,46 +84,47 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
public boolean pressed;
|
||||
SensorCollection isPressed;
|
||||
|
||||
public boolean elevatorControlMode = false;
|
||||
public boolean armControlMode = false;
|
||||
// Motor controllers
|
||||
//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
|
||||
|
||||
public Elevator()
|
||||
public Arm()
|
||||
{
|
||||
try
|
||||
{
|
||||
//PID elevator encoder and talon
|
||||
elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
|
||||
elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
|
||||
//PID Arm encoder and talon
|
||||
armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
|
||||
|
||||
//ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID);
|
||||
|
||||
elevatorRight.setInverted(false);
|
||||
//ArmMotor.setInverted(false);
|
||||
|
||||
//Setting left elevator motor as follower
|
||||
elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID());
|
||||
elevatorLeft.setInverted(false);
|
||||
elevatorLeft.setNeutralMode(NeutralMode.Brake);
|
||||
elevatorRight.setNeutralMode(NeutralMode.Brake);
|
||||
elevatorRight.setSensorPhase(true);
|
||||
//Setting left Arm motor as follower
|
||||
//ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID());
|
||||
//ArmLeft.setInverted(false);
|
||||
//ArmLeft.setNeutralMode(NeutralMode.Brake);
|
||||
armMotor.setNeutralMode(NeutralMode.Brake);
|
||||
armMotor.setSensorPhase(true);
|
||||
//Limit Switch Left
|
||||
//elevatorLeft.overrideLimitSwitchesEnable(true);
|
||||
elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
//ArmLeft.overrideLimitSwitchesEnable(true);
|
||||
//ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
//ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
|
||||
//Limit Switch Right
|
||||
//elevatorRight.overrideLimitSwitchesEnable(true);
|
||||
//elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
//elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
//ArmMotor.overrideLimitSwitchesEnable(true);
|
||||
//ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
//ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
|
||||
|
||||
//Change This boi
|
||||
|
||||
// elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here
|
||||
//elevatorRight.configReverseSoftLimitThreshold(5, 0);
|
||||
//elevatorRight.configForwardSoftLimitEnable(true, 0);
|
||||
//elevatorRight.configReverseSoftLimitEnable(true, 0);
|
||||
// ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here
|
||||
//ArmMotor.configReverseSoftLimitThreshold(5, 0);
|
||||
//ArmMotor.configForwardSoftLimitEnable(true, 0);
|
||||
//ArmMotor.configReverseSoftLimitEnable(true, 0);
|
||||
|
||||
//sos
|
||||
//elevatorRight.enableLimitSwitch(true, true);
|
||||
//ArmMotor.enableLimitSwitch(true, true);
|
||||
|
||||
|
||||
|
||||
@@ -132,7 +133,7 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
}
|
||||
catch(Exception e)
|
||||
{
|
||||
System.err.println("You thought the code would work, but it was me, error. An error occurred in the Elevator Construtor");
|
||||
System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -200,80 +201,53 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
return move;
|
||||
}
|
||||
|
||||
public void setElevatorMode()
|
||||
public void setArmMode()
|
||||
{
|
||||
setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
public void resetElevatorEncoder()
|
||||
public void resetArmEncoder()
|
||||
{
|
||||
elevatorRight.setSelectedSensorPosition(0, 0, 0);
|
||||
armMotor.setSelectedSensorPosition(0, 0, 0);
|
||||
}
|
||||
|
||||
public void moveElevatorXbox()
|
||||
public void moveArmXbox()
|
||||
{
|
||||
double moveElevatorInput;
|
||||
double elevatorSafeZone = 15;
|
||||
double moveArmInput;
|
||||
double armSafeZone = 15;
|
||||
|
||||
double elevatorPos = getEncoderElevatorPosition();
|
||||
double armPos = getEncoderArmPosition();
|
||||
|
||||
moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis();
|
||||
moveArmInput = Robot.oi.getOperatorController().getLeftYAxis();
|
||||
|
||||
//double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY);
|
||||
//double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY);
|
||||
|
||||
boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON);
|
||||
boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON);
|
||||
boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON);
|
||||
|
||||
if(elevatorTuningPressed == true)
|
||||
if(armTuningPressed == true)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput * 0.5);
|
||||
armMotor.set(moveArmInput * 0.5);
|
||||
}
|
||||
else if(elevatorTuningPressed == false)
|
||||
else if(armTuningPressed == false)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput);
|
||||
}
|
||||
|
||||
/*
|
||||
if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput);
|
||||
}
|
||||
else if(elevatorPos > elevatorSafeZone)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput * 0.65);
|
||||
|
||||
|
||||
if(holdButtonPressed == true)
|
||||
{
|
||||
elevatorRight.set(-0.43 * (0.2));
|
||||
}
|
||||
else if(holdButtonPressed == false)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput * 0.75);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
else if(elevatorPos < 0)
|
||||
{
|
||||
elevatorRight.set(moveElevatorInput * 0.75);
|
||||
}
|
||||
*/
|
||||
armMotor.set(moveArmInput);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range
|
||||
// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range
|
||||
|
||||
|
||||
//PID encoder position
|
||||
public double getEncoderElevatorPosition()
|
||||
public double getEncoderArmPosition()
|
||||
{
|
||||
return elevatorRight.getPositionWorld();
|
||||
return armMotor.getPositionWorld();
|
||||
}
|
||||
|
||||
public double getElevatorHeightInchesAboveFloor()
|
||||
public double getArmHeightInchesAboveFloor()
|
||||
{
|
||||
return elevatorRight.getPositionWorld();
|
||||
return armMotor.getPositionWorld();
|
||||
}
|
||||
|
||||
public synchronized void setControlMode(DriveControlMode controlMode)
|
||||
@@ -283,61 +257,61 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
isFinished = false;
|
||||
}
|
||||
/*
|
||||
public void setElevatorPIDMaxScale(double ElevatorPosition, double maxError, double minError)
|
||||
public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError)
|
||||
{
|
||||
double elevatorTargetPos = 0;
|
||||
this.targetPPosition = elevatorTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); ///////TARGET POSITION WHERE??
|
||||
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
|
||||
double ArmTargetPos = 0;
|
||||
this.targetPPosition = ArmTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE??
|
||||
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
|
||||
}
|
||||
|
||||
public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError)
|
||||
public void setArmPIDLowScale(double ArmPosition, double maxError, double minError)
|
||||
{
|
||||
double elevatorTargetPos = 0;
|
||||
this.targetPPosition = elevatorTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
|
||||
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
|
||||
double ArmTargetPos = 0;
|
||||
this.targetPPosition = ArmTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
|
||||
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
|
||||
}
|
||||
|
||||
public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError)
|
||||
public void setArmPIDSwitch(double ArmPosition, double maxError, double minError)
|
||||
{
|
||||
double elevatorTargetPos = 0;
|
||||
this.targetPPosition = elevatorTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
|
||||
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
|
||||
double ArmTargetPos = 0;
|
||||
this.targetPPosition = ArmTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
|
||||
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
|
||||
}
|
||||
|
||||
public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError)
|
||||
public void setArmPIDLowest(double ArmPosition, double maxError, double minError)
|
||||
{
|
||||
double elevatorTargetPos = 0;
|
||||
this.targetPPosition = elevatorTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
|
||||
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
|
||||
double ArmTargetPos = 0;
|
||||
this.targetPPosition = ArmTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
|
||||
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
|
||||
}
|
||||
*/
|
||||
public void rawSetOutput(double output){
|
||||
elevatorRight.set(/*ControlMode.PercentOutput,*/ output);
|
||||
armMotor.set(/*ControlMode.PercentOutput,*/ output);
|
||||
}
|
||||
|
||||
public void holdInPos()
|
||||
{
|
||||
elevatorRight.set(-0.43 * 0.2);
|
||||
armMotor.set(-0.43 * 0.2);
|
||||
}
|
||||
|
||||
public void stopMotors()
|
||||
{
|
||||
elevatorRight.set(0);
|
||||
armMotor.set(0);
|
||||
}
|
||||
|
||||
public void isSwitchPressed()
|
||||
{
|
||||
pressed = false;
|
||||
isPressed = elevatorRight.getSensorCollection();
|
||||
isPressed = armMotor.getSensorCollection();
|
||||
|
||||
if(isPressed.isFwdLimitSwitchClosed() == true)
|
||||
{
|
||||
if (controlMode == DriveControlMode.JOYSTICK) {
|
||||
Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);
|
||||
Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS);
|
||||
}
|
||||
pressed = true;
|
||||
}
|
||||
@@ -345,7 +319,7 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
{
|
||||
if (controlMode == DriveControlMode.STOP_MOTORS){
|
||||
{
|
||||
Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
|
||||
Robot.arm.setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
pressed = false;
|
||||
@@ -365,31 +339,31 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
{
|
||||
if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB)
|
||||
{
|
||||
moveElevatorXbox();
|
||||
moveArmXbox();
|
||||
}
|
||||
else if (!isFinished)
|
||||
{
|
||||
//PID control mode
|
||||
if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE)
|
||||
{
|
||||
isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition());
|
||||
isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE)
|
||||
{
|
||||
isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition());
|
||||
isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH)
|
||||
{
|
||||
isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition());
|
||||
isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST)
|
||||
{
|
||||
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
|
||||
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
/*
|
||||
else if(controlMode == DriveControlMode.RAW)
|
||||
{
|
||||
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
|
||||
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
*/
|
||||
}
|
||||
@@ -410,10 +384,10 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
public void setPeriodMs(long periodMs)
|
||||
{
|
||||
//PID controller
|
||||
pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, elevatorRight);
|
||||
pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, elevatorRight);
|
||||
pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, elevatorRight);
|
||||
pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, elevatorRight);
|
||||
pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor);
|
||||
pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor);
|
||||
pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor);
|
||||
pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor);
|
||||
|
||||
this.periodMs = periodMs;
|
||||
}
|
||||
@@ -434,8 +408,8 @@ public class Elevator extends Subsystem implements ControlLoopable
|
||||
{
|
||||
try
|
||||
{
|
||||
SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor());
|
||||
SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor());
|
||||
//SmartDashboard.putData(pressed);
|
||||
}
|
||||
catch (Exception e)
|
||||
@@ -210,7 +210,6 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_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);
|
||||
gyroNavX = new AHRS(SPI.Port.kMXP);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user