mirror of
https://github.com/Team4388/2018-Robot.git
synced 2026-06-09 00:38:04 -06:00
Dave Staudacher
433 lines
12 KiB
Java
433 lines
12 KiB
Java
package org.usfirst.frc4388.robot.subsystems;
|
|
|
|
import java.util.ArrayList;
|
|
|
|
import org.usfirst.frc4388.controller.XboxController;
|
|
import org.usfirst.frc4388.robot.Constants;
|
|
import org.usfirst.frc4388.robot.Robot;
|
|
import org.usfirst.frc4388.robot.RobotMap;
|
|
import org.usfirst.frc4388.robot.commands.*;
|
|
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
|
|
import org.usfirst.frc4388.utility.CANTalonEncoder;
|
|
import org.usfirst.frc4388.utility.ControlLoopable;
|
|
import org.usfirst.frc4388.utility.PIDParams;
|
|
import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
|
|
|
|
import com.ctre.phoenix.motorcontrol.ControlMode;
|
|
|
|
import edu.wpi.first.wpilibj.DigitalInput;
|
|
import edu.wpi.first.wpilibj.Encoder;
|
|
import edu.wpi.first.wpilibj.PIDController;
|
|
import edu.wpi.first.wpilibj.SpeedControllerGroup;
|
|
import edu.wpi.first.wpilibj.command.Subsystem;
|
|
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
|
|
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
|
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
|
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
|
import com.ctre.phoenix.motorcontrol.SensorCollection;
|
|
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
|
|
|
public class Elevator extends Subsystem implements ControlLoopable
|
|
{
|
|
//PID encoder and motor
|
|
private CANTalonEncoder elevatorRight;
|
|
private WPI_TalonSRX elevatorLeft;
|
|
|
|
//PID controller Max Scale
|
|
private SoftwarePIDPositionController pidPositionControllerMaxScale;
|
|
//private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0);
|
|
private PIDParams PositionPMaxScale;
|
|
|
|
//PID controller Max Scale
|
|
private SoftwarePIDPositionController pidPositionControllerLowScale;
|
|
//private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0);
|
|
private PIDParams PositionPLowScale;
|
|
|
|
//PID controller Max Scale
|
|
private SoftwarePIDPositionController pidPositionControllerSwitch;
|
|
//private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0);
|
|
private PIDParams PositionPSwitch;
|
|
|
|
//PID controller Max Scale
|
|
private SoftwarePIDPositionController pidPositionControllerLowest;
|
|
//private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0);
|
|
private PIDParams PositionPLowest;
|
|
|
|
//PID target
|
|
private double targetPPosition;
|
|
|
|
//Encoder ticks to inches for encoders
|
|
public static final double ENCODER_TICKS_TO_INCHES = Constants.kElevatorEncoderTicksPerInch;
|
|
|
|
//control mode for joystick control
|
|
private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
|
|
|
|
private double periodMs;
|
|
|
|
//Non Linear Joystick
|
|
public static final double STICK_DEADBAND = 0.02;
|
|
public static final double MOVE_NON_LINEARITY = 1.0;
|
|
private int moveNonLinear = 0;
|
|
private double moveScale = 1.0;
|
|
private double moveTrim = 0.0;
|
|
|
|
private boolean isFinished;
|
|
private DifferentialDrive m_drive;
|
|
|
|
//private LimitSwitchSource limitSwitch = new DigitalInput(1);
|
|
LimitSwitchSource limitSwitchSource;
|
|
|
|
public boolean pressed;
|
|
SensorCollection isPressed;
|
|
|
|
public boolean elevatorControlMode = false;
|
|
// Motor controllers
|
|
//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
|
|
|
|
public Elevator()
|
|
{
|
|
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);
|
|
|
|
elevatorRight.setInverted(false);
|
|
|
|
//Setting left elevator motor as follower
|
|
elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID());
|
|
elevatorLeft.setInverted(false);
|
|
|
|
//Limit Switch Left
|
|
elevatorLeft.overrideLimitSwitchesEnable(true);
|
|
elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
|
elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
|
|
|
//Limit Switch Right
|
|
elevatorRight.overrideLimitSwitchesEnable(true);
|
|
elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
|
elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
|
|
|
}
|
|
catch(Exception e)
|
|
{
|
|
System.err.println("You thought the code would work, but it was me, error. An error occurred in the Elevator Construtor");
|
|
}
|
|
}
|
|
|
|
private double nonlinearStickCalcPositive(double move, double moveNonLinearity)
|
|
{
|
|
return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity);
|
|
}
|
|
|
|
private double nonlinearStickCalcNegative(double move, double moveNonLinearity)
|
|
{
|
|
return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity);
|
|
}
|
|
|
|
private boolean inDeadZone(double input)
|
|
{
|
|
boolean inDeadZone;
|
|
if (Math.abs(input) < STICK_DEADBAND)
|
|
{
|
|
inDeadZone = true;
|
|
}
|
|
else
|
|
{
|
|
inDeadZone = false;
|
|
}
|
|
|
|
return inDeadZone;
|
|
}
|
|
|
|
private double limitValue(double value)
|
|
{
|
|
if (value > 1.0)
|
|
{
|
|
value = 1.0;
|
|
}
|
|
else if (value < -1.0)
|
|
{
|
|
value = -1.0;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity)
|
|
{
|
|
if (inDeadZone(move))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
move += trim;
|
|
move *= scale;
|
|
move = limitValue(move);
|
|
|
|
int iterations = Math.abs(nonLinearFactor);
|
|
for (int i = 0; i < iterations; i++)
|
|
{
|
|
if (nonLinearFactor > 0)
|
|
{
|
|
move = nonlinearStickCalcPositive(move, wheelNonLinearity);
|
|
}
|
|
else
|
|
{
|
|
move = nonlinearStickCalcNegative(move, wheelNonLinearity);
|
|
}
|
|
}
|
|
return move;
|
|
}
|
|
|
|
public void setElevatorMode()
|
|
{
|
|
setControlMode(DriveControlMode.JOYSTICK);
|
|
}
|
|
|
|
public void resetElevatorEncoder()
|
|
{
|
|
elevatorRight.setSelectedSensorPosition(0, 0, 0);
|
|
}
|
|
|
|
public void moveElevatorXbox()
|
|
{
|
|
double moveElevatorInput;
|
|
double elevatorSafeZone = -30;
|
|
|
|
double elevatorPos = getEncoderElevatorPosition();
|
|
|
|
moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis();
|
|
|
|
//double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY);
|
|
|
|
boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON);
|
|
boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON);
|
|
|
|
if(elevatorTuningPressed == true)
|
|
{
|
|
elevatorRight.set(moveElevatorInput * 0.5);
|
|
}
|
|
else if(elevatorTuningPressed == 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);
|
|
}
|
|
|
|
*/
|
|
|
|
System.out.println(moveElevatorInput);
|
|
}
|
|
|
|
// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range
|
|
|
|
//PID encoder position
|
|
public double getEncoderElevatorPosition()
|
|
{
|
|
return elevatorRight.getPositionWorld();
|
|
}
|
|
|
|
public double getElevatorHeightInchesAboveFloor()
|
|
{
|
|
return elevatorRight.getPositionWorld();
|
|
}
|
|
|
|
public synchronized void setControlMode(DriveControlMode controlMode)
|
|
{
|
|
this.controlMode = controlMode;
|
|
|
|
isFinished = false;
|
|
}
|
|
/*
|
|
public void setElevatorPIDMaxScale(double ElevatorPosition, 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);
|
|
}
|
|
|
|
public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError)
|
|
{
|
|
double elevatorTargetPos = 0;
|
|
this.targetPPosition = elevatorTargetPos;
|
|
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
|
|
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
|
|
}
|
|
|
|
public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError)
|
|
{
|
|
double elevatorTargetPos = 0;
|
|
this.targetPPosition = elevatorTargetPos;
|
|
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
|
|
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
|
|
}
|
|
|
|
public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError)
|
|
{
|
|
double elevatorTargetPos = 0;
|
|
this.targetPPosition = elevatorTargetPos;
|
|
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
|
|
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
|
|
}
|
|
*/
|
|
public void rawSetOutput(double output){
|
|
elevatorRight.set(/*ControlMode.PercentOutput,*/ output);
|
|
}
|
|
|
|
public void holdInPos()
|
|
{
|
|
elevatorRight.set(-0.43 * 0.2);
|
|
}
|
|
|
|
public void stopMotors()
|
|
{
|
|
elevatorRight.set(0);
|
|
}
|
|
|
|
public void isSwitchPressed()
|
|
{
|
|
pressed = false;
|
|
isPressed = elevatorRight.getSensorCollection();
|
|
|
|
if(isPressed.isFwdLimitSwitchClosed() == true)
|
|
{
|
|
if (controlMode == DriveControlMode.JOYSTICK) {
|
|
Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);
|
|
}
|
|
pressed = true;
|
|
}
|
|
else
|
|
{
|
|
if (controlMode == DriveControlMode.STOP_MOTORS){
|
|
{
|
|
Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
|
|
}
|
|
|
|
pressed = false;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
|
|
|
|
|
|
|
|
|
|
|
|
@Override
|
|
public void controlLoopUpdate()
|
|
{
|
|
if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB)
|
|
{
|
|
moveElevatorXbox();
|
|
}
|
|
else if (!isFinished)
|
|
{
|
|
//PID control mode
|
|
if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE)
|
|
{
|
|
isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition());
|
|
}
|
|
else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE)
|
|
{
|
|
isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition());
|
|
}
|
|
else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH)
|
|
{
|
|
isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition());
|
|
}
|
|
else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST)
|
|
{
|
|
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
|
|
}
|
|
/*
|
|
else if(controlMode == DriveControlMode.RAW)
|
|
{
|
|
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
|
|
}
|
|
*/
|
|
}
|
|
}
|
|
|
|
@Override
|
|
public void initDefaultCommand()
|
|
{
|
|
}
|
|
|
|
@Override
|
|
public void periodic()
|
|
{
|
|
// isSwitchPressed();
|
|
}
|
|
|
|
@Override
|
|
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);
|
|
|
|
this.periodMs = periodMs;
|
|
}
|
|
|
|
public synchronized boolean isFinished()
|
|
{
|
|
return isFinished;
|
|
}
|
|
|
|
public double getPeriodMs()
|
|
{
|
|
return periodMs;
|
|
}
|
|
|
|
public void updateStatus(Robot.OperationMode operationMode)
|
|
{
|
|
if (operationMode == Robot.OperationMode.TEST)
|
|
{
|
|
try
|
|
{
|
|
SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0));
|
|
SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor());
|
|
//SmartDashboard.putData(pressed);
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
System.err.println("Drivetrain update status error");
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|