mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 00:28:05 -06:00
Slight edit
This commit is contained in:
mayabartels
@@ -1,240 +1,423 @@
|
||||
package org.usfirst.frc4388.robot.subsystems;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.lang.Math;
|
||||
|
||||
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.Loop;
|
||||
import org.usfirst.frc4388.utility.MPTalonPIDController;
|
||||
import org.usfirst.frc4388.utility.ControlLoopable;
|
||||
import org.usfirst.frc4388.utility.PIDParams;
|
||||
import org.usfirst.frc4388.utility.TalonSRXEncoder;
|
||||
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.can.TalonSRX;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.SensorCollection;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class Arm extends Subsystem implements Loop
|
||||
public class Arm extends Subsystem implements ControlLoopable
|
||||
{
|
||||
private static Arm instance;
|
||||
//PID encoder and motor
|
||||
private CANTalonEncoder armMotor;
|
||||
//private WPI_TalonSRX ArmLeft;
|
||||
|
||||
public static enum ArmControlMode {PID, JOYSTICK_MANUAL };
|
||||
public static final double DEGREE_START_OFFSET = 70;
|
||||
|
||||
public static final double RADIUS_OF_ARM = 43.3;
|
||||
public static final double OFF_SET_BELOW = 49.3;
|
||||
|
||||
// One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks
|
||||
public static final double ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3))-(DEGREE_START_OFFSET);
|
||||
public static final double X_POSITION_MATH = ((RADIUS_OF_ARM)*(Math.cos(ENCODER_TICKS_TO_DEGREES)))+(OFF_SET_BELOW);
|
||||
public static final double Y_POSITION_MATH = (RADIUS_OF_ARM)*(Math.sin(ENCODER_TICKS_TO_DEGREES));
|
||||
//PID controller Max Scale
|
||||
private SoftwarePIDPositionController pidPositionControllerMaxScale;
|
||||
//private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0);
|
||||
private PIDParams PositionPMaxScale;
|
||||
|
||||
public double ARM_ANGLE_DEGREES = 0;
|
||||
|
||||
// Defined speeds
|
||||
public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
|
||||
//PID controller Max Scale
|
||||
private SoftwarePIDPositionController pidPositionControllerLowScale;
|
||||
//private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0);
|
||||
private PIDParams PositionPLowScale;
|
||||
|
||||
// Defined positions
|
||||
public static final double MIN_POSITION_INCHES = 0.0;
|
||||
public static final double MAX_POSITION_INCHES = 83.4;
|
||||
public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
|
||||
|
||||
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
// Motion profile max velocities and accel times
|
||||
public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60;
|
||||
public static final double MP_T2 = 150; // Fast = 150
|
||||
//PID controller Max Scale
|
||||
private SoftwarePIDPositionController pidPositionControllerSwitch;
|
||||
//private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0);
|
||||
private PIDParams PositionPSwitch;
|
||||
|
||||
// Motor controllers
|
||||
private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
|
||||
//PID controller Max Scale
|
||||
private SoftwarePIDPositionController pidPositionControllerLowest;
|
||||
//private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0);
|
||||
private PIDParams PositionPLowest;
|
||||
|
||||
private CANTalonEncoder motor1;
|
||||
private TalonSRX motor2;
|
||||
//PID target
|
||||
private double targetPPosition;
|
||||
|
||||
//Encoder ticks to inches for encoders
|
||||
public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch;
|
||||
|
||||
//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;
|
||||
|
||||
// PID controller and params
|
||||
private MPTalonPIDController mpController;
|
||||
|
||||
public static int PID_SLOT = 0;
|
||||
public static int MP_SLOT = 1;
|
||||
|
||||
private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);
|
||||
public static final double KF_UP = 0.005;
|
||||
public static final double KF_DOWN = 0.0;
|
||||
public static final double PID_ERROR_INCHES = 1.0;
|
||||
private long periodMs = (long)(Constants.kLooperDt * 1000.0);
|
||||
|
||||
|
||||
// Misc
|
||||
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
|
||||
private boolean isFinished;
|
||||
private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL;
|
||||
private double targetPositionInchesPID = 0;
|
||||
private boolean firstMpPoint;
|
||||
private DifferentialDrive m_drive;
|
||||
|
||||
//private LimitSwitchSource limitSwitch = new DigitalInput(1);
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
|
||||
public Arm() {
|
||||
try {
|
||||
motor1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder);
|
||||
//motor2 = CANTallon.createPermanentSlaveTalon(RobotMap.ARM_MOTOR_2_CAN_ID, RobotMap.ELEVATOR_MOTOR_1_CAN_ID);
|
||||
public boolean pressed;
|
||||
SensorCollection isPressed;
|
||||
|
||||
public boolean armControlMode = false;
|
||||
// Motor controllers
|
||||
//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
|
||||
|
||||
public Arm()
|
||||
{
|
||||
try
|
||||
{
|
||||
//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);
|
||||
|
||||
motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("An error occurred in the DriveTrain constructor");
|
||||
}
|
||||
}
|
||||
@Override
|
||||
public void initDefaultCommand() {
|
||||
}
|
||||
|
||||
public void resetZeroPosition(double position) {
|
||||
mpController.resetZeroPosition(position);
|
||||
}
|
||||
|
||||
private synchronized void setArmControlMode(ArmControlMode controlMode) {
|
||||
this.armControlMode = controlMode;
|
||||
}
|
||||
|
||||
private synchronized ArmControlMode getArmControlMode() {
|
||||
return this.armControlMode;
|
||||
}
|
||||
//ArmMotor.setInverted(false);
|
||||
|
||||
//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
|
||||
//ArmLeft.overrideLimitSwitchesEnable(true);
|
||||
//ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
//ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
|
||||
//Limit Switch Right
|
||||
//ArmMotor.overrideLimitSwitchesEnable(true);
|
||||
//ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
//ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
|
||||
|
||||
//Change This boi
|
||||
|
||||
// ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here
|
||||
//ArmMotor.configReverseSoftLimitThreshold(5, 0);
|
||||
//ArmMotor.configForwardSoftLimitEnable(true, 0);
|
||||
//ArmMotor.configReverseSoftLimitEnable(true, 0);
|
||||
|
||||
//sos
|
||||
//ArmMotor.enableLimitSwitch(true, true);
|
||||
|
||||
|
||||
|
||||
public void setSpeedJoystick(double speed) {
|
||||
motor1.set(ControlMode.PercentOutput, speed);
|
||||
setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
catch(Exception e)
|
||||
{
|
||||
System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor");
|
||||
}
|
||||
}
|
||||
|
||||
private double nonlinearStickCalcPositive(double move, double moveNonLinearity)
|
||||
{
|
||||
return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity);
|
||||
}
|
||||
|
||||
public void setPositionPID(double targetPositionInches) {
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
updatePositionPID(targetPositionInches);
|
||||
setArmControlMode(ArmControlMode.PID);
|
||||
setFinished(false);
|
||||
|
||||
private double nonlinearStickCalcNegative(double move, double moveNonLinearity)
|
||||
{
|
||||
return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity);
|
||||
}
|
||||
|
||||
public void updatePositionPID(double targetPositionInches) {
|
||||
targetPositionInchesPID = limitPosition(targetPositionInches);
|
||||
double startPositionInches = motor1.getPositionWorld()*Y_POSITION_MATH;
|
||||
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
|
||||
}
|
||||
|
||||
|
||||
private double limitPosition(double targetPosition) {
|
||||
if (targetPosition < MIN_POSITION_INCHES) {
|
||||
return MIN_POSITION_INCHES;
|
||||
}
|
||||
else if (targetPosition > MAX_POSITION_INCHES) {
|
||||
return MAX_POSITION_INCHES;
|
||||
|
||||
private boolean inDeadZone(double input)
|
||||
{
|
||||
boolean inDeadZone;
|
||||
if (Math.abs(input) < STICK_DEADBAND)
|
||||
{
|
||||
inDeadZone = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
inDeadZone = false;
|
||||
}
|
||||
|
||||
return targetPosition;
|
||||
return inDeadZone;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onStart(double timestamp) {
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onStop(double timestamp) {
|
||||
// TODO Auto-generated method stub
|
||||
|
||||
}
|
||||
move += trim;
|
||||
move *= scale;
|
||||
move = limitValue(move);
|
||||
|
||||
@Override
|
||||
public void onLoop(double timestamp) {
|
||||
synchronized (Arm.this) {
|
||||
switch( getArmControlMode() ) {
|
||||
case PID:
|
||||
controlPidWithJoystick();
|
||||
break;
|
||||
case JOYSTICK_MANUAL:
|
||||
controlManualWithJoystick();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
||||
|
||||
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 setArmMode()
|
||||
{
|
||||
setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
public void resetArmEncoder()
|
||||
{
|
||||
armMotor.setSelectedSensorPosition(0, 0, 0);
|
||||
}
|
||||
|
||||
public void moveArmXbox()
|
||||
{
|
||||
double moveArmInput;
|
||||
double armSafeZone = 15;
|
||||
|
||||
double armPos = getEncoderArmPosition();
|
||||
|
||||
moveArmInput = Robot.oi.getOperatorController().getLeftYAxis();
|
||||
|
||||
//double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY);
|
||||
|
||||
boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON);
|
||||
boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON);
|
||||
|
||||
if(armTuningPressed == true)
|
||||
{
|
||||
armMotor.set(moveArmInput * 0.5);
|
||||
}
|
||||
else if(armTuningPressed == false)
|
||||
{
|
||||
armMotor.set(moveArmInput);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range
|
||||
|
||||
|
||||
//PID encoder position
|
||||
public double getEncoderArmPosition()
|
||||
{
|
||||
return armMotor.getPositionWorld();
|
||||
}
|
||||
|
||||
public double getArmHeightInchesAboveFloor()
|
||||
{
|
||||
return armMotor.getPositionWorld();
|
||||
}
|
||||
|
||||
public synchronized void setControlMode(DriveControlMode controlMode)
|
||||
{
|
||||
this.controlMode = controlMode;
|
||||
|
||||
isFinished = false;
|
||||
}
|
||||
/*
|
||||
public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError)
|
||||
{
|
||||
double ArmTargetPos = 0;
|
||||
this.targetPPosition = ArmTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE??
|
||||
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
|
||||
}
|
||||
|
||||
public void setArmPIDLowScale(double ArmPosition, double maxError, double minError)
|
||||
{
|
||||
double ArmTargetPos = 0;
|
||||
this.targetPPosition = ArmTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
|
||||
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
|
||||
}
|
||||
|
||||
public void setArmPIDSwitch(double ArmPosition, double maxError, double minError)
|
||||
{
|
||||
double ArmTargetPos = 0;
|
||||
this.targetPPosition = ArmTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
|
||||
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
|
||||
}
|
||||
|
||||
public void setArmPIDLowest(double ArmPosition, double maxError, double minError)
|
||||
{
|
||||
double ArmTargetPos = 0;
|
||||
this.targetPPosition = ArmTargetPos;
|
||||
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
|
||||
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
|
||||
}
|
||||
*/
|
||||
public void rawSetOutput(double output){
|
||||
armMotor.set(/*ControlMode.PercentOutput,*/ output);
|
||||
}
|
||||
|
||||
public void holdInPos()
|
||||
{
|
||||
armMotor.set(-0.43 * 0.2);
|
||||
}
|
||||
|
||||
public void stopMotors()
|
||||
{
|
||||
armMotor.set(0);
|
||||
}
|
||||
|
||||
public void isSwitchPressed()
|
||||
{
|
||||
pressed = false;
|
||||
isPressed = armMotor.getSensorCollection();
|
||||
|
||||
if(isPressed.isFwdLimitSwitchClosed() == true)
|
||||
{
|
||||
if (controlMode == DriveControlMode.JOYSTICK) {
|
||||
Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS);
|
||||
}
|
||||
pressed = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (controlMode == DriveControlMode.STOP_MOTORS){
|
||||
{
|
||||
Robot.arm.setControlMode(DriveControlMode.JOYSTICK);
|
||||
}
|
||||
|
||||
pressed = false;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private void controlPidWithJoystick() {
|
||||
//double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
//double deltaPosition = joystickPosition *.5;
|
||||
targetPositionInchesPID = targetPositionInchesPID;// + deltaPosition;
|
||||
updatePositionPID(targetPositionInchesPID);
|
||||
@Override
|
||||
public void controlLoopUpdate()
|
||||
{
|
||||
if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB)
|
||||
{
|
||||
moveArmXbox();
|
||||
}
|
||||
else if (!isFinished)
|
||||
{
|
||||
//PID control mode
|
||||
if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE)
|
||||
{
|
||||
isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE)
|
||||
{
|
||||
isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH)
|
||||
{
|
||||
isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST)
|
||||
{
|
||||
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
/*
|
||||
else if(controlMode == DriveControlMode.RAW)
|
||||
{
|
||||
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition());
|
||||
}
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
private void controlManualWithJoystick() {
|
||||
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
setSpeedJoystick(joyStickSpeed);
|
||||
@Override
|
||||
public void initDefaultCommand()
|
||||
{
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic()
|
||||
{
|
||||
// isSwitchPressed();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setPeriodMs(long periodMs)
|
||||
{
|
||||
//PID controller
|
||||
pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor);
|
||||
pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor);
|
||||
pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor);
|
||||
pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor);
|
||||
|
||||
this.periodMs = periodMs;
|
||||
}
|
||||
|
||||
|
||||
public double getYPositionInches() {
|
||||
return motor1.getPositionWorld()*Y_POSITION_MATH;
|
||||
|
||||
}
|
||||
public double getXPositionInches() {
|
||||
return motor1.getPositionWorld()*X_POSITION_MATH;
|
||||
}
|
||||
|
||||
// public double getAverageMotorCurrent() {
|
||||
// return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3;
|
||||
// }
|
||||
|
||||
public double getAverageMotorCurrent() {
|
||||
return (motor1.getOutputCurrent() + motor2.getOutputCurrent() / 2);
|
||||
}
|
||||
|
||||
public synchronized boolean isFinished() {
|
||||
public synchronized boolean isFinished()
|
||||
{
|
||||
return isFinished;
|
||||
}
|
||||
|
||||
public synchronized void setFinished(boolean isFinished) {
|
||||
this.isFinished = isFinished;
|
||||
}
|
||||
|
||||
public double getPeriodMs() {
|
||||
public double getPeriodMs()
|
||||
{
|
||||
return periodMs;
|
||||
}
|
||||
|
||||
public void updateStatus(Robot.OperationMode operationMode) {
|
||||
if (operationMode == Robot.OperationMode.TEST) {
|
||||
try {
|
||||
SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent());
|
||||
SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
|
||||
public void updateStatus(Robot.OperationMode operationMode)
|
||||
{
|
||||
if (operationMode == Robot.OperationMode.TEST)
|
||||
{
|
||||
try
|
||||
{
|
||||
SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor());
|
||||
//SmartDashboard.putData(pressed);
|
||||
}
|
||||
catch (Exception e) {
|
||||
catch (Exception e)
|
||||
{
|
||||
System.err.println("Drivetrain update status error");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static Arm getInstance() {
|
||||
if(instance == null) {
|
||||
instance = new Arm();
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -101,8 +101,6 @@ public class Wrist extends Subsystem
|
||||
private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO;
|
||||
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
private boolean pressed;
|
||||
SensorCollection isPressed;
|
||||
|
||||
public Wrist()
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user