mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 00:28:05 -06:00
Merge branch 'arm-merge' into develop
This commit is contained in:
Keenan D. Buckley
@@ -26,32 +26,39 @@ import jaci.pathfinder.Pathfinder;
|
||||
|
||||
|
||||
|
||||
public class OI
|
||||
public class OI
|
||||
{
|
||||
private static OI instance;
|
||||
|
||||
|
||||
private XboxController m_driverXbox;
|
||||
private XboxController m_operatorXbox;
|
||||
|
||||
private OI()
|
||||
private OI()
|
||||
{
|
||||
try
|
||||
try
|
||||
{
|
||||
// Driver controller
|
||||
m_driverXbox = new XboxController(RobotMap.DRIVER_JOYSTICK_1_USB_ID);
|
||||
m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID);
|
||||
|
||||
|
||||
XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER);
|
||||
CarriageIntake.whenPressed(new SetIntakeSpeed(BallIntake.BALL_INTAKE_SPEED));
|
||||
CarriageIntake.whenReleased(new SetIntakeSpeed(0.0));
|
||||
|
||||
|
||||
XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER);
|
||||
CarriageEject.whenPressed(new SetIntakeSpeed(BallIntake.BALL_EXTAKE_SPEED));
|
||||
CarriageEject.whenReleased(new SetIntakeSpeed(0.0));
|
||||
|
||||
|
||||
JoystickButton Expand = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON);
|
||||
Expand.whenPressed(new WristPlacement(true));
|
||||
|
||||
JoystickButton Contract = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
|
||||
Contract.whenPressed(new WristPlacement(false));
|
||||
|
||||
|
||||
JoystickButton liftBothIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
|
||||
liftBothIntake.whenPressed(new HatchAndBallUp());
|
||||
|
||||
|
||||
JoystickButton liftHatchIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
|
||||
liftHatchIntake.whenPressed(new LiftHatchDropBall());
|
||||
|
||||
@@ -67,34 +74,32 @@ public class OI
|
||||
JoystickButton ratchetFlip = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
|
||||
ratchetFlip.whenPressed(new ratchetFlip(0.5));
|
||||
ratchetFlip.whenReleased(new ratchetFlip(0));
|
||||
|
||||
|
||||
JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
|
||||
shiftUp.whenPressed(new DriveSpeedShift(true));
|
||||
//shiftUp.whenPressed(new LEDIndicators(true));
|
||||
|
||||
|
||||
JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
|
||||
shiftDown.whenPressed(new DriveSpeedShift(false));
|
||||
// shiftDown.whenPressed(new LEDIndicators(false));
|
||||
|
||||
|
||||
//Operator Xbox
|
||||
/*
|
||||
JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
|
||||
openIntake.whenPressed(new IntakePosition(true));
|
||||
|
||||
|
||||
JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
|
||||
CloseIntake.whenPressed(new IntakePosition(false));
|
||||
*/
|
||||
JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON);
|
||||
safteySwitch.whenPressed(new setClimberSafety(true));
|
||||
safteySwitch.whenReleased(new setClimberSafety(false));
|
||||
|
||||
|
||||
//SmartDashboard.putData("PRE GAME!!!!", new PreGame());
|
||||
} catch (Exception e) {
|
||||
System.err.println("An error occurred in the OI constructor");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public static OI getInstance() {
|
||||
if(instance == null) {
|
||||
instance = new OI();
|
||||
@@ -106,7 +111,7 @@ public class OI
|
||||
return m_driverXbox;
|
||||
}
|
||||
|
||||
public IHandController getOperatorController()
|
||||
public IHandController getOperatorController()
|
||||
{
|
||||
return m_operatorXbox;
|
||||
}
|
||||
@@ -116,4 +121,3 @@ public class OI
|
||||
return m_operatorXbox.getJoyStick();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -29,6 +29,8 @@ public class Robot extends IterativeRobot
|
||||
public static OI oi;
|
||||
|
||||
public static final Drive drive = new Drive();
|
||||
public static final Arm arm = new Arm();
|
||||
public static final Wrist wrist = new Wrist();
|
||||
|
||||
|
||||
|
||||
@@ -56,7 +58,9 @@ public class Robot extends IterativeRobot
|
||||
try {
|
||||
oi = OI.getInstance();
|
||||
|
||||
controlLoop.addLoopable(drive);
|
||||
controlLoop.addLoopable(drive);
|
||||
controlLoop.addLoopable(arm);
|
||||
controlLoop.addLoopable(wrist);
|
||||
|
||||
|
||||
operationModeChooser = new SendableChooser<OperationMode>();
|
||||
|
||||
@@ -21,8 +21,9 @@ public class RobotMap {
|
||||
public static final int INTAKE_BELT_DRIVE_CAN_ID = 8;
|
||||
|
||||
//Elevator Motors
|
||||
public static final int ELEVATOR_MOTOR1_ID = 6;
|
||||
public static final int ELEVATOR_MOTOR2_ID = 7;
|
||||
public static final int ARM_MOTOR1_ID= 6;
|
||||
public static final int ARM_MOTOR2_ID = 7;
|
||||
public static final int WRIST_MOTOR_ID = 9;
|
||||
|
||||
|
||||
//Climber Motors
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.subsystems.Arm;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
public class ArmAutoZero extends Command
|
||||
{
|
||||
private double MIN_ELEVATOR_POSITION_CHANGE = 0.05;
|
||||
private double lastArmPosition;
|
||||
private int encoderCount;
|
||||
|
||||
public ArmAutoZero(boolean interrutible) {
|
||||
requires(Robot.arm);
|
||||
setInterruptible(interrutible);
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void initialize() {
|
||||
lastArmPosition = Arm.MAX_POSITION_INCHES;
|
||||
Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED);
|
||||
encoderCount = 0;
|
||||
// System.out.println("Auto zero initialize");
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void execute() {
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
protected boolean isFinished() {
|
||||
Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED);
|
||||
double currentArmPosition = Robot.arm.getPositionInches();
|
||||
double armPositionChange = lastArmPosition - currentArmPosition;
|
||||
lastArmPosition = currentArmPosition;
|
||||
boolean test = encoderCount > 2 && Math.abs(armPositionChange) < MIN_ELEVATOR_POSITION_CHANGE && Robot.arm.getAverageMotorCurrent() > Arm.AUTO_ZERO_MOTOR_CURRENT;
|
||||
System.out.println("encoderCount = " + encoderCount + ", test = " + test + ", arm change = " + armPositionChange + ", current = " + Robot.arm.getAverageMotorCurrent());
|
||||
|
||||
if (Math.abs(armPositionChange) < MIN_ELEVATOR_POSITION_CHANGE) {
|
||||
encoderCount++;
|
||||
}
|
||||
else {
|
||||
encoderCount = 0;
|
||||
}
|
||||
|
||||
return test;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void end() {
|
||||
Robot.arm.setSpeed(0);
|
||||
Robot.arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES);
|
||||
Robot.arm.setPositionPID(Arm.MIN_POSITION_INCHES);
|
||||
// System.out.println("Arm Zeroed");
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void interrupted() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class ArmSetMode extends Command {
|
||||
|
||||
private ArmControlMode controlMode;
|
||||
|
||||
public ArmSetMode(ArmControlMode controlMode) {
|
||||
this.controlMode = controlMode;
|
||||
requires(Robot.arm);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
if (controlMode == ArmControlMode.JOYSTICK_PID) {
|
||||
Robot.arm.setPositionPID(Robot.arm.getPositionInches());
|
||||
}
|
||||
else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) {
|
||||
Robot.arm.setSpeedJoystick(0);
|
||||
}
|
||||
else {
|
||||
Robot.arm.setSpeed(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class ArmSetPositionMP extends Command {
|
||||
|
||||
private double targetPositionInches;
|
||||
private boolean isAtTarget;
|
||||
private static final double MIN_DELTA_TARGET = 0.3;
|
||||
|
||||
public ArmSetPositionMP(double targetPositionInches) {
|
||||
this.targetPositionInches = targetPositionInches;
|
||||
requires(Robot.arm);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
if (Math.abs(targetPositionInches - Robot.arm.getPositionInches()) < MIN_DELTA_TARGET) {
|
||||
isAtTarget = true;
|
||||
}
|
||||
else {
|
||||
isAtTarget = false;
|
||||
Robot.arm.setPositionMP(targetPositionInches);
|
||||
}
|
||||
// System.out.println("Arm set MP initialized, target = " + targetPositionInches);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return isAtTarget || Robot.arm.isFinished();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
Robot.arm.setPositionPID(Robot.arm.getPositionInches());
|
||||
// System.out.println("Arm set MP end");
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
// System.out.println("ArmSetPositionMP interrupted");
|
||||
Robot.arm.setFinished(true);
|
||||
Robot.arm.setPositionPID(Robot.arm.getPositionInches());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.subsystems.Arm;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public class ArmSetPositionPID extends Command {
|
||||
|
||||
private double targetPositionInches;
|
||||
|
||||
public ArmSetPositionPID(double targetPositionInches) {
|
||||
this.targetPositionInches = targetPositionInches;
|
||||
requires(Robot.arm);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
protected void initialize() {
|
||||
Robot.arm.setPositionPID(targetPositionInches);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
protected void execute() {
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return Math.abs(Robot.arm.getPositionInches() - this.targetPositionInches) < Arm.PID_ERROR_INCHES;
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
+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);
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
public class ArmSetZero extends Command
|
||||
{
|
||||
private double position;
|
||||
|
||||
public ArmSetZero(double position) {
|
||||
this.position = position;
|
||||
requires(Robot.arm);
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void initialize() {
|
||||
Robot.arm.resetZeroPosition(position);
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void execute() {
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
protected boolean isFinished() {
|
||||
return true;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void end() {
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void interrupted() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,361 @@
|
||||
package org.usfirst.frc4388.robot.subsystems;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import org.usfirst.frc4388.robot.Constants;
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.RobotMap;
|
||||
import org.usfirst.frc4388.utility.ControlLoopable;
|
||||
import org.usfirst.frc4388.utility.Loop;
|
||||
import org.usfirst.frc4388.utility.MPTalonPIDController;
|
||||
import org.usfirst.frc4388.utility.PIDParams;
|
||||
import org.usfirst.frc4388.utility.TalonSRXEncoder;
|
||||
import org.usfirst.frc4388.utility.TalonSRXFactory;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
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.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.SensorCollection;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
|
||||
import edu.wpi.first.wpilibj.Solenoid;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
|
||||
public class Arm extends Subsystem implements ControlLoopable
|
||||
{
|
||||
private static Arm instance;
|
||||
|
||||
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
|
||||
|
||||
// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks
|
||||
public static final double ENCODER_TICKS_TO_INCHES = ((360/4096)/(3))-60;
|
||||
|
||||
|
||||
private double periodMs;
|
||||
private double lastControlLoopUpdatePeriod = 0.0;
|
||||
private double lastControlLoopUpdateTimestamp = 0.0;
|
||||
// Defined speeds
|
||||
public static final double CLIMB_SPEED = -1.0;
|
||||
public static final double TEST_SPEED_UP = 0.3;
|
||||
public static final double TEST_SPEED_DOWN = -0.3;
|
||||
public static final double AUTO_ZERO_SPEED = -0.3;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
|
||||
|
||||
// Defined positions
|
||||
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
|
||||
public static final double ZERO_POSITION_INCHES = -0.25;
|
||||
public static final double NEAR_ZERO_POSITION_INCHES = 0.0;
|
||||
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;
|
||||
|
||||
public static final double SWITCH_POSITION_INCHES = 24.0;
|
||||
public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR
|
||||
public static final double SCALE_LOW_POSITION_INCHES = 56.0;
|
||||
public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0
|
||||
public static final double SCALE_SECOND_CUBE_POSITION_INCHES = 77.0;
|
||||
public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES;
|
||||
public static final double CLIMB_BAR_POSITION_INCHES = 70.0;
|
||||
public static final double CLIMB_HIGH_POSITION_INCHES = 10.0;
|
||||
public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;
|
||||
|
||||
// Motion profile max velocities and accel times
|
||||
public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60;
|
||||
public static final double MP_T1 = 400; // Fast = 300
|
||||
public static final double MP_T2 = 150; // Fast = 150
|
||||
|
||||
// Motor controllers
|
||||
private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
|
||||
|
||||
private TalonSRXEncoder motor1;
|
||||
private TalonSRX motor2;
|
||||
|
||||
// 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);
|
||||
private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 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;
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
// Pneumatics
|
||||
private Solenoid speedShift;
|
||||
|
||||
private ArmControlMode controlMode = ArmControlMode.JOYSTICK_MANUAL;
|
||||
// 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 double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
|
||||
|
||||
public Arm() {
|
||||
try {
|
||||
motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.ARM_MOTOR1_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);
|
||||
motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID);
|
||||
|
||||
|
||||
motor1.setInverted(true);
|
||||
motor2.setInverted(true);
|
||||
|
||||
// if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
|
||||
// Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
|
||||
// }
|
||||
motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
motor1.setNeutralMode(NeutralMode.Brake);
|
||||
motor2.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
motorControllers.add(motor1);
|
||||
|
||||
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("An error occurred in the Arm 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;
|
||||
}
|
||||
|
||||
public void setSpeed(double speed) {
|
||||
motor1.set(ControlMode.PercentOutput, speed);
|
||||
setArmControlMode(ArmControlMode.MANUAL);
|
||||
}
|
||||
|
||||
public void setSpeedJoystick(double speed) {
|
||||
motor1.set(ControlMode.PercentOutput, speed);
|
||||
setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
|
||||
}
|
||||
|
||||
public void setPositionPID(double targetPositionInches) {
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
updatePositionPID(targetPositionInches);
|
||||
setArmControlMode(ArmControlMode.JOYSTICK_PID);
|
||||
setFinished(false);
|
||||
}
|
||||
|
||||
public void updatePositionPID(double targetPositionInches) {
|
||||
targetPositionInchesPID = limitPosition(targetPositionInches);
|
||||
double startPositionInches = motor1.getPositionWorld();
|
||||
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
|
||||
}
|
||||
|
||||
public void setPositionMP(double targetPositionInches) {
|
||||
double startPositionInches = motor1.getPositionWorld();
|
||||
mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2);
|
||||
setFinished(false);
|
||||
firstMpPoint = true;
|
||||
setArmControlMode(ArmControlMode.MOTION_PROFILE);
|
||||
}
|
||||
|
||||
private double limitPosition(double targetPosition) {
|
||||
if (targetPosition < MIN_POSITION_INCHES) {
|
||||
return MIN_POSITION_INCHES;
|
||||
}
|
||||
else if (targetPosition > MAX_POSITION_INCHES) {
|
||||
return MAX_POSITION_INCHES;
|
||||
}
|
||||
|
||||
return targetPosition;
|
||||
}
|
||||
@Override
|
||||
public void setPeriodMs(long periodMs) {
|
||||
mpController = new MPTalonPIDController(periodMs, motorControllers);
|
||||
mpController.setPID(mpPIDParams, MP_SLOT);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
this.periodMs = periodMs;
|
||||
}
|
||||
/*@Override
|
||||
public void onStart(double timestamp) {
|
||||
mpController = new MPTalonPIDController(periodMs, motorControllers);
|
||||
mpController.setPID(mpPIDParams, MP_SLOT);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onStop(double timestamp) {
|
||||
// TODO Auto-generated method stub
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onLoop(double timestamp) {
|
||||
synchronized (Arm.this) {
|
||||
switch( getElevatorControlMode() ) {
|
||||
case JOYSTICK_PID:
|
||||
controlPidWithJoystick();
|
||||
break;
|
||||
case JOYSTICK_MANUAL:
|
||||
controlManualWithJoystick();
|
||||
break;
|
||||
case MOTION_PROFILE:
|
||||
if (!isFinished()) {
|
||||
if (firstMpPoint) {
|
||||
mpController.setPIDSlot(MP_SLOT);
|
||||
firstMpPoint = false;
|
||||
}
|
||||
setFinished(mpController.controlLoopUpdate());
|
||||
if (isFinished()) {
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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 == ArmControlMode.JOYSTICK_MANUAL) {
|
||||
controlManualWithJoystick();
|
||||
}
|
||||
else if (!isFinished) {
|
||||
if (controlMode == ArmControlMode.MOTION_PROFILE) {
|
||||
isFinished = mpController.controlLoopUpdate(getPositionInches());
|
||||
}
|
||||
|
||||
/*else if (controlMode == ArmControlMode.MP_PATH_VELOCITY) {
|
||||
isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
|
||||
}
|
||||
else if (controlMode == ArmControlMode.ADAPTIVE_PURSUIT) {
|
||||
updatePose();
|
||||
isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
|
||||
}*/
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private void controlPidWithJoystick() {
|
||||
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
double deltaPosition = joystickPosition * joystickInchesPerMs;
|
||||
targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
|
||||
updatePositionPID(targetPositionInchesPID);
|
||||
}
|
||||
|
||||
private void ControlWithJoystickhold(){
|
||||
double holdPosition = motor1.getPositionWorld();
|
||||
updatePositionPID(holdPosition);
|
||||
|
||||
}
|
||||
|
||||
private void controlManualWithJoystick() {
|
||||
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
setSpeedJoystick(joyStickSpeed*.60);
|
||||
}
|
||||
/*
|
||||
public void setShiftState(ElevatorSpeedShiftState state) {
|
||||
|
||||
joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_HI;
|
||||
speedShift.set(true);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
}
|
||||
else if(state == ElevatorSpeedShiftState.LO) {
|
||||
joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
|
||||
speedShift.set(false);
|
||||
mpController.setPID(pidPIDParamsLoGear, PID_SLOT);
|
||||
}
|
||||
}
|
||||
|
||||
public ElevatorSpeedShiftState getShiftState() {
|
||||
return shiftState;
|
||||
}
|
||||
*/
|
||||
public double getPositionInches() {
|
||||
return motor1.getPositionWorld();
|
||||
}
|
||||
|
||||
// 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() {
|
||||
return isFinished;
|
||||
}
|
||||
|
||||
public synchronized void setFinished(boolean isFinished) {
|
||||
this.isFinished = isFinished;
|
||||
}
|
||||
|
||||
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 Motor 1 Amps", motor1.getOutputCurrent());
|
||||
SmartDashboard.putNumber("Elevator Motor 2 Amps", motor2.getOutputCurrent());
|
||||
SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent());
|
||||
// SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID));
|
||||
// SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));
|
||||
// SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
|
||||
SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
|
||||
}
|
||||
catch (Exception e) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static Arm getInstance() {
|
||||
if(instance == null) {
|
||||
instance = new Arm();
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,356 @@
|
||||
package org.usfirst.frc4388.robot.subsystems;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import org.usfirst.frc4388.robot.Constants;
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.RobotMap;
|
||||
import org.usfirst.frc4388.utility.ControlLoopable;
|
||||
import org.usfirst.frc4388.utility.Loop;
|
||||
import org.usfirst.frc4388.utility.MPTalonPIDController;
|
||||
import org.usfirst.frc4388.utility.PIDParams;
|
||||
import org.usfirst.frc4388.utility.TalonSRXEncoder;
|
||||
import org.usfirst.frc4388.utility.TalonSRXFactory;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
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.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.SensorCollection;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
|
||||
import edu.wpi.first.wpilibj.Solenoid;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
|
||||
public class Wrist extends Subsystem implements ControlLoopable
|
||||
{
|
||||
private static Wrist instance;
|
||||
|
||||
public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
|
||||
|
||||
// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks
|
||||
public static final double ENCODER_TICKS_TO_INCHES = ((360/4096)/(3))-60;
|
||||
|
||||
|
||||
private double periodMs;
|
||||
private double lastControlLoopUpdatePeriod = 0.0;
|
||||
private double lastControlLoopUpdateTimestamp = 0.0;
|
||||
// Defined speeds
|
||||
public static final double CLIMB_SPEED = -1.0;
|
||||
public static final double TEST_SPEED_UP = 0.3;
|
||||
public static final double TEST_SPEED_DOWN = -0.3;
|
||||
public static final double AUTO_ZERO_SPEED = -0.3;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
|
||||
|
||||
// Defined positions
|
||||
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
|
||||
public static final double ZERO_POSITION_INCHES = -0.25;
|
||||
public static final double NEAR_ZERO_POSITION_INCHES = 0.0;
|
||||
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;
|
||||
|
||||
public static final double SWITCH_POSITION_INCHES = 24.0;
|
||||
public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR
|
||||
public static final double SCALE_LOW_POSITION_INCHES = 56.0;
|
||||
public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0
|
||||
public static final double SCALE_SECOND_CUBE_POSITION_INCHES = 77.0;
|
||||
public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES;
|
||||
public static final double CLIMB_BAR_POSITION_INCHES = 70.0;
|
||||
public static final double CLIMB_HIGH_POSITION_INCHES = 10.0;
|
||||
public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;
|
||||
|
||||
// Motion profile max velocities and accel times
|
||||
public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60;
|
||||
public static final double MP_T1 = 400; // Fast = 300
|
||||
public static final double MP_T2 = 150; // Fast = 150
|
||||
|
||||
// Motor controllers
|
||||
private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
|
||||
|
||||
private TalonSRXEncoder wristmotor1;
|
||||
|
||||
// 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);
|
||||
private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 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;
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
// Pneumatics
|
||||
private Solenoid speedShift;
|
||||
|
||||
private WristControlMode controlMode = WristControlMode.JOYSTICK_MANUAL;
|
||||
// Misc
|
||||
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
|
||||
private boolean isFinished;
|
||||
private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
|
||||
private double targetPositionInchesPID = 0;
|
||||
private boolean firstMpPoint;
|
||||
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
|
||||
|
||||
public Wrist() {
|
||||
try {
|
||||
wristmotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);
|
||||
System.err.println("the tallon shold be made in wrist");
|
||||
|
||||
wristmotor1.setInverted(true);
|
||||
|
||||
// if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
|
||||
// Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
|
||||
// }
|
||||
wristmotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
wristmotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
wristmotor1.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
motorControllers.add(wristmotor1);
|
||||
|
||||
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("An error occurred in the Wrist constructor");
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initDefaultCommand() {
|
||||
}
|
||||
|
||||
public void resetZeroPosition(double position) {
|
||||
mpController.resetZeroPosition(position);
|
||||
}
|
||||
|
||||
private synchronized void setWristControlMode(WristControlMode controlMode) {
|
||||
this.wristControlMode = controlMode;
|
||||
}
|
||||
|
||||
private synchronized WristControlMode getWristControlMode() {
|
||||
return this.wristControlMode;
|
||||
}
|
||||
|
||||
public void setSpeed(double speed) {
|
||||
wristmotor1.set(ControlMode.PercentOutput, speed);
|
||||
setWristControlMode(WristControlMode.MANUAL);
|
||||
}
|
||||
|
||||
public void setSpeedJoystick(double speed) {
|
||||
wristmotor1.set(ControlMode.PercentOutput, speed);
|
||||
setWristControlMode(WristControlMode.JOYSTICK_MANUAL);
|
||||
}
|
||||
|
||||
public void setPositionPID(double targetPositionInches) {
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
updatePositionPID(targetPositionInches);
|
||||
setWristControlMode(WristControlMode.JOYSTICK_PID);
|
||||
setFinished(false);
|
||||
}
|
||||
|
||||
public void updatePositionPID(double targetPositionInches) {
|
||||
targetPositionInchesPID = limitPosition(targetPositionInches);
|
||||
double startPositionInches = wristmotor1.getPositionWorld();
|
||||
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
|
||||
}
|
||||
|
||||
public void setPositionMP(double targetPositionInches) {
|
||||
double startPositionInches = wristmotor1.getPositionWorld();
|
||||
mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2);
|
||||
setFinished(false);
|
||||
firstMpPoint = true;
|
||||
setWristControlMode(WristControlMode.MOTION_PROFILE);
|
||||
}
|
||||
|
||||
private double limitPosition(double targetPosition) {
|
||||
if (targetPosition < MIN_POSITION_INCHES) {
|
||||
return MIN_POSITION_INCHES;
|
||||
}
|
||||
else if (targetPosition > MAX_POSITION_INCHES) {
|
||||
return MAX_POSITION_INCHES;
|
||||
}
|
||||
|
||||
return targetPosition;
|
||||
}
|
||||
@Override
|
||||
public void setPeriodMs(long periodMs) {
|
||||
mpController = new MPTalonPIDController(periodMs, motorControllers);
|
||||
mpController.setPID(mpPIDParams, MP_SLOT);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
this.periodMs = periodMs;
|
||||
}
|
||||
/*@Override
|
||||
public void onStart(double timestamp) {
|
||||
mpController = new MPTalonPIDController(periodMs, motorControllers);
|
||||
mpController.setPID(mpPIDParams, MP_SLOT);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onStop(double timestamp) {
|
||||
// TODO Auto-generated method stub
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onLoop(double timestamp) {
|
||||
synchronized (Wrist.this) {
|
||||
switch( getElevatorControlMode() ) {
|
||||
case JOYSTICK_PID:
|
||||
controlPidWithJoystick();
|
||||
break;
|
||||
case JOYSTICK_MANUAL:
|
||||
controlManualWithJoystick();
|
||||
break;
|
||||
case MOTION_PROFILE:
|
||||
if (!isFinished()) {
|
||||
if (firstMpPoint) {
|
||||
mpController.setPIDSlot(MP_SLOT);
|
||||
firstMpPoint = false;
|
||||
}
|
||||
setFinished(mpController.controlLoopUpdate());
|
||||
if (isFinished()) {
|
||||
mpController.setPIDSlot(PID_SLOT);
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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 == WristControlMode.JOYSTICK_MANUAL) {
|
||||
controlManualWithJoystick();
|
||||
}
|
||||
else if (!isFinished) {
|
||||
if (controlMode == WristControlMode.MOTION_PROFILE) {
|
||||
isFinished = mpController.controlLoopUpdate(getPositionInches());
|
||||
}
|
||||
|
||||
/*else if (controlMode == WristControlMode.MP_PATH_VELOCITY) {
|
||||
isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
|
||||
}
|
||||
else if (controlMode == WristControlMode.ADAPTIVE_PURSUIT) {
|
||||
updatePose();
|
||||
isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
|
||||
}*/
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private void controlPidWithJoystick() {
|
||||
double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis();
|
||||
double deltaPosition = joystickPosition * joystickInchesPerMs;
|
||||
targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
|
||||
updatePositionPID(targetPositionInchesPID);
|
||||
}
|
||||
|
||||
private void ControlWithJoystickhold(){
|
||||
double holdPosition = wristmotor1.getPositionWorld();
|
||||
updatePositionPID(holdPosition);
|
||||
|
||||
}
|
||||
|
||||
private void controlManualWithJoystick() {
|
||||
double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
|
||||
setSpeedJoystick(joyStickSpeed*.50);
|
||||
}
|
||||
/*
|
||||
public void setShiftState(ElevatorSpeedShiftState state) {
|
||||
|
||||
joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_HI;
|
||||
speedShift.set(true);
|
||||
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
|
||||
}
|
||||
else if(state == ElevatorSpeedShiftState.LO) {
|
||||
joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
|
||||
speedShift.set(false);
|
||||
mpController.setPID(pidPIDParamsLoGear, PID_SLOT);
|
||||
}
|
||||
}
|
||||
|
||||
public ElevatorSpeedShiftState getShiftState() {
|
||||
return shiftState;
|
||||
}
|
||||
*/
|
||||
public double getPositionInches() {
|
||||
return wristmotor1.getPositionWorld();
|
||||
}
|
||||
|
||||
// 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 (wristmotor1.getOutputCurrent());
|
||||
}
|
||||
|
||||
public synchronized boolean isFinished() {
|
||||
return isFinished;
|
||||
}
|
||||
|
||||
public synchronized void setFinished(boolean isFinished) {
|
||||
this.isFinished = isFinished;
|
||||
}
|
||||
|
||||
public double getPeriodMs() {
|
||||
return periodMs;
|
||||
}
|
||||
|
||||
public void updateStatus(Robot.OperationMode operationMode) {
|
||||
if (operationMode == Robot.OperationMode.TEST) {
|
||||
try {
|
||||
SmartDashboard.putNumber("Elevator Position Inches", wristmotor1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Elevator Motor 1 Amps", wristmotor1.getOutputCurrent());
|
||||
SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent());
|
||||
// SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID));
|
||||
// SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));
|
||||
// SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
|
||||
SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
|
||||
}
|
||||
catch (Exception e) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static Wrist getInstance() {
|
||||
if(instance == null) {
|
||||
instance = new Wrist();
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user