From 6fdaa28e3c582e0e7219d610f85493d69e587f3b Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Sat, 16 Feb 2019 20:50:21 -0700 Subject: [PATCH 1/4] added arm functionality arm works on manuel, pid needs work --- .../java/org/usfirst/frc4388/robot/Robot.java | 4 +- .../org/usfirst/frc4388/robot/RobotMap.java | 4 +- .../frc4388/robot/commands/ArmAutoZero.java | 64 ++++ .../frc4388/robot/commands/ArmSetMode.java | 50 +++ .../robot/commands/ArmSetPositionMP.java | 55 +++ .../robot/commands/ArmSetPositionPID.java | 42 +++ ...ElevatorSetSpeed.java => ArmSetSpeed.java} | 0 .../frc4388/robot/commands/ArmSetZero.java | 40 ++ .../usfirst/frc4388/robot/subsystems/Arm.java | 355 ++++++++++++++++++ 9 files changed, 611 insertions(+), 3 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java rename 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/{ElevatorSetSpeed.java => ArmSetSpeed.java} (100%) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetZero.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index 1f3669c..c2c6a02 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -29,6 +29,7 @@ public class Robot extends IterativeRobot public static OI oi; public static final Drive drive = new Drive(); + public static final Arm arm = new Arm(); @@ -56,7 +57,8 @@ public class Robot extends IterativeRobot try { oi = OI.getInstance(); - controlLoop.addLoopable(drive); + controlLoop.addLoopable(drive); + controlLoop.addLoopable(arm); operationModeChooser = new SendableChooser(); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index 3995e0b..8b8dff4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -21,8 +21,8 @@ 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; //Climber Motors diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java new file mode 100644 index 0000000..7ba43a8 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java @@ -0,0 +1,64 @@ +package org.usfirst.frc.team3310.robot.commands; + +import org.usfirst.frc.team3310.robot.Robot; +import org.usfirst.frc.team3310.robot.subsystems.Elevator; + +import edu.wpi.first.wpilibj.command.Command; + +public class ElevatorAutoZero extends Command +{ + private double MIN_ELEVATOR_POSITION_CHANGE = 0.05; + private double lastElevatorPosition; + private int encoderCount; + + public ElevatorAutoZero(boolean interrutible) { + requires(Robot.intake); + requires(Robot.elevator); + setInterruptible(interrutible); + } + + @Override + protected void initialize() { + lastElevatorPosition = Elevator.MAX_POSITION_INCHES; + Robot.elevator.setSpeed(Elevator.AUTO_ZERO_SPEED); + encoderCount = 0; +// System.out.println("Auto zero initialize"); + } + + @Override + protected void execute() { + + } + + @Override + protected boolean isFinished() { + Robot.elevator.setSpeed(Elevator.AUTO_ZERO_SPEED); + double currentElevatorPosition = Robot.elevator.getPositionInches(); + double elevatorPositionChange = lastElevatorPosition - currentElevatorPosition; + lastElevatorPosition = currentElevatorPosition; + boolean test = encoderCount > 2 && Math.abs(elevatorPositionChange) < MIN_ELEVATOR_POSITION_CHANGE && Robot.elevator.getAverageMotorCurrent() > Elevator.AUTO_ZERO_MOTOR_CURRENT; + System.out.println("encoderCount = " + encoderCount + ", test = " + test + ", elevator change = " + elevatorPositionChange + ", current = " + Robot.elevator.getAverageMotorCurrent()); + + if (Math.abs(elevatorPositionChange) < MIN_ELEVATOR_POSITION_CHANGE) { + encoderCount++; + } + else { + encoderCount = 0; + } + + return test; + } + + @Override + protected void end() { + Robot.elevator.setSpeed(0); + Robot.elevator.resetZeroPosition(Elevator.ZERO_POSITION_INCHES); + Robot.elevator.setPositionPID(Elevator.MIN_POSITION_INCHES); +// System.out.println("Elevator Zeroed"); + } + + @Override + protected void interrupted() { + + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java new file mode 100644 index 0000000..33b0c91 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java @@ -0,0 +1,50 @@ +package org.usfirst.frc.team3310.robot.commands; + +import org.usfirst.frc.team3310.robot.Robot; +import org.usfirst.frc.team3310.robot.subsystems.Elevator.ElevatorControlMode; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ElevatorSetMode extends Command { + + private ElevatorControlMode controlMode; + + public ElevatorSetMode(ElevatorControlMode controlMode) { + this.controlMode = controlMode; + requires(Robot.elevator); + } + + // Called just before this Command runs the first time + protected void initialize() { + if (controlMode == ElevatorControlMode.JOYSTICK_PID) { + Robot.elevator.setPositionPID(Robot.elevator.getPositionInches()); + } + else if (controlMode == ElevatorControlMode.JOYSTICK_MANUAL) { + Robot.elevator.setSpeedJoystick(0); + } + else { + Robot.elevator.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() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java new file mode 100644 index 0000000..baad693 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java @@ -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()); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java new file mode 100644 index 0000000..0156e34 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java @@ -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() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java similarity index 100% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetZero.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetZero.java new file mode 100644 index 0000000..8702c3c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetZero.java @@ -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() { + + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java new file mode 100644 index 0000000..ea270a7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -0,0 +1,355 @@ +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 motorControllers = new ArrayList(); + + 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_PID; + // Misc + public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; + private boolean isFinished; + private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_PID; + 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_PID) { + controlPidWithJoystick(); + } + 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 controlManualWithJoystick() { + double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); + setSpeedJoystick(joyStickSpeed*.3); + } + /* + 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; + } +} \ No newline at end of file From a0bbd8205157180114c5c264971486e87d4c74f0 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Sun, 17 Feb 2019 13:34:16 -0700 Subject: [PATCH 2/4] added in wrist and last pnumatics pid dosent work yet, talk with maya --- .../java/org/usfirst/frc4388/robot/OI.java | 10 +- .../org/usfirst/frc4388/robot/RobotMap.java | 1 + .../frc4388/robot/commands/ArmAutoZero.java | 41 +- .../frc4388/robot/commands/ArmSetMode.java | 24 +- .../frc4388/robot/commands/ArmSetSpeed.java | 4 +- .../usfirst/frc4388/robot/subsystems/Arm.java | 16 +- .../frc4388/robot/subsystems/Wrist.java | 358 ++++++++++++++++++ 7 files changed, 412 insertions(+), 42 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index 865059f..4605dfd 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -48,6 +48,13 @@ public class OI 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(true)); + //JoystickButton endEfector = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); //endEfector.toggleWhenActive(new WristPlacement(true)); @@ -78,8 +85,7 @@ public class OI 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)); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index 8b8dff4..e7a1a21 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -23,6 +23,7 @@ public class RobotMap { //Elevator Motors 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 diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java index 7ba43a8..57ccb5f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmAutoZero.java @@ -1,26 +1,25 @@ -package org.usfirst.frc.team3310.robot.commands; +package org.usfirst.frc4388.robot.commands; -import org.usfirst.frc.team3310.robot.Robot; -import org.usfirst.frc.team3310.robot.subsystems.Elevator; +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Arm; import edu.wpi.first.wpilibj.command.Command; -public class ElevatorAutoZero extends Command +public class ArmAutoZero extends Command { private double MIN_ELEVATOR_POSITION_CHANGE = 0.05; - private double lastElevatorPosition; + private double lastArmPosition; private int encoderCount; - public ElevatorAutoZero(boolean interrutible) { - requires(Robot.intake); - requires(Robot.elevator); + public ArmAutoZero(boolean interrutible) { + requires(Robot.arm); setInterruptible(interrutible); } @Override protected void initialize() { - lastElevatorPosition = Elevator.MAX_POSITION_INCHES; - Robot.elevator.setSpeed(Elevator.AUTO_ZERO_SPEED); + lastArmPosition = Arm.MAX_POSITION_INCHES; + Robot.arm.setSpeed(Arm.AUTO_ZERO_SPEED); encoderCount = 0; // System.out.println("Auto zero initialize"); } @@ -32,14 +31,14 @@ public class ElevatorAutoZero extends Command @Override protected boolean isFinished() { - Robot.elevator.setSpeed(Elevator.AUTO_ZERO_SPEED); - double currentElevatorPosition = Robot.elevator.getPositionInches(); - double elevatorPositionChange = lastElevatorPosition - currentElevatorPosition; - lastElevatorPosition = currentElevatorPosition; - boolean test = encoderCount > 2 && Math.abs(elevatorPositionChange) < MIN_ELEVATOR_POSITION_CHANGE && Robot.elevator.getAverageMotorCurrent() > Elevator.AUTO_ZERO_MOTOR_CURRENT; - System.out.println("encoderCount = " + encoderCount + ", test = " + test + ", elevator change = " + elevatorPositionChange + ", current = " + Robot.elevator.getAverageMotorCurrent()); + 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(elevatorPositionChange) < MIN_ELEVATOR_POSITION_CHANGE) { + if (Math.abs(armPositionChange) < MIN_ELEVATOR_POSITION_CHANGE) { encoderCount++; } else { @@ -51,10 +50,10 @@ public class ElevatorAutoZero extends Command @Override protected void end() { - Robot.elevator.setSpeed(0); - Robot.elevator.resetZeroPosition(Elevator.ZERO_POSITION_INCHES); - Robot.elevator.setPositionPID(Elevator.MIN_POSITION_INCHES); -// System.out.println("Elevator Zeroed"); + Robot.arm.setSpeed(0); + Robot.arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES); + Robot.arm.setPositionPID(Arm.MIN_POSITION_INCHES); +// System.out.println("Arm Zeroed"); } @Override diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java index 33b0c91..25ccaf4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java @@ -1,32 +1,32 @@ -package org.usfirst.frc.team3310.robot.commands; +package org.usfirst.frc4388.robot.commands; -import org.usfirst.frc.team3310.robot.Robot; -import org.usfirst.frc.team3310.robot.subsystems.Elevator.ElevatorControlMode; +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; import edu.wpi.first.wpilibj.command.Command; /** * */ -public class ElevatorSetMode extends Command { +public class ArmSetMode extends Command { - private ElevatorControlMode controlMode; + private ArmControlMode controlMode; - public ElevatorSetMode(ElevatorControlMode controlMode) { + public ArmSetMode(ArmControlMode controlMode) { this.controlMode = controlMode; - requires(Robot.elevator); + requires(Robot.arm); } // Called just before this Command runs the first time protected void initialize() { - if (controlMode == ElevatorControlMode.JOYSTICK_PID) { - Robot.elevator.setPositionPID(Robot.elevator.getPositionInches()); + if (controlMode == ArmControlMode.JOYSTICK_PID) { + Robot.arm.setPositionPID(Robot.arm.getPositionInches()); } - else if (controlMode == ElevatorControlMode.JOYSTICK_MANUAL) { - Robot.elevator.setSpeedJoystick(0); + else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) { + Robot.arm.setSpeedJoystick(0); } else { - Robot.elevator.setSpeed(0.0); + Robot.arm.setSpeed(0.0); } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java index 7553d48..21e328a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java @@ -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); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index ea270a7..ee2be32 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -91,11 +91,11 @@ public class Arm extends Subsystem implements ControlLoopable // Pneumatics private Solenoid speedShift; - private ArmControlMode controlMode = ArmControlMode.JOYSTICK_PID; + 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_PID; + private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; private double targetPositionInchesPID = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; @@ -252,8 +252,8 @@ public class Arm extends Subsystem implements ControlLoopable lastControlLoopUpdateTimestamp = currentTimestamp; // Do the update - if (controlMode == ArmControlMode.JOYSTICK_PID) { - controlPidWithJoystick(); + if (controlMode == ArmControlMode.JOYSTICK_MANUAL) { + controlManualWithJoystick(); } else if (!isFinished) { if (controlMode == ArmControlMode.MOTION_PROFILE) { @@ -282,10 +282,16 @@ public class Arm extends Subsystem implements ControlLoopable 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*.3); + setSpeedJoystick(joyStickSpeed*.60); } /* public void setShiftState(ElevatorSpeedShiftState state) { diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java new file mode 100644 index 0000000..3b3f427 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -0,0 +1,358 @@ +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 motorControllers = new ArrayList(); + + 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 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 { + motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); + + + motor1.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); + + motorControllers.add(motor1); + + + } + 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) { + motor1.set(ControlMode.PercentOutput, speed*0.3); + setWristControlMode(WristControlMode.MANUAL); + } + + public void setSpeedJoystick(double speed) { + motor1.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 = 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; + 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 = motor1.getPositionWorld(); + updatePositionPID(holdPosition); + + } + + private void controlManualWithJoystick() { + double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); + setSpeedJoystick(joyStickSpeed*.10); + } + /* + 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 Wrist getInstance() { + if(instance == null) { + instance = new Wrist(); + } + return instance; + } +} \ No newline at end of file From 5bf04716bb0c325177d42fe2b663938096484827 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Sun, 17 Feb 2019 14:40:17 -0700 Subject: [PATCH 3/4] chanmged varables --- .../frc4388/robot/subsystems/Wrist.java | 34 +++++++++---------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 3b3f427..bfb7817 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -73,8 +73,7 @@ public class Wrist extends Subsystem implements ControlLoopable // Motor controllers private ArrayList motorControllers = new ArrayList(); - private TalonSRXEncoder motor1; - private TalonSRX motor2; + private TalonSRXEncoder wristmotor1; // PID controller and params private MPTalonPIDController mpController; @@ -102,19 +101,19 @@ public class Wrist extends Subsystem implements ControlLoopable public Wrist() { try { - motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); + wristmotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); - motor1.setInverted(true); + wristmotor1.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); + wristmotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + wristmotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + wristmotor1.setNeutralMode(NeutralMode.Brake); - motorControllers.add(motor1); + motorControllers.add(wristmotor1); } @@ -140,12 +139,12 @@ public class Wrist extends Subsystem implements ControlLoopable } public void setSpeed(double speed) { - motor1.set(ControlMode.PercentOutput, speed*0.3); + wristmotor1.set(ControlMode.PercentOutput, speed*0.3); setWristControlMode(WristControlMode.MANUAL); } public void setSpeedJoystick(double speed) { - motor1.set(ControlMode.PercentOutput, speed); + wristmotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.JOYSTICK_MANUAL); } @@ -158,12 +157,12 @@ public class Wrist extends Subsystem implements ControlLoopable public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = motor1.getPositionWorld(); + double startPositionInches = wristmotor1.getPositionWorld(); mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); } public void setPositionMP(double targetPositionInches) { - double startPositionInches = motor1.getPositionWorld(); + double startPositionInches = wristmotor1.getPositionWorld(); mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); setFinished(false); firstMpPoint = true; @@ -281,7 +280,7 @@ public class Wrist extends Subsystem implements ControlLoopable } private void ControlWithJoystickhold(){ - double holdPosition = motor1.getPositionWorld(); + double holdPosition = wristmotor1.getPositionWorld(); updatePositionPID(holdPosition); } @@ -309,7 +308,7 @@ public class Wrist extends Subsystem implements ControlLoopable } */ public double getPositionInches() { - return motor1.getPositionWorld(); + return wristmotor1.getPositionWorld(); } // public double getAverageMotorCurrent() { @@ -317,7 +316,7 @@ public class Wrist extends Subsystem implements ControlLoopable // } public double getAverageMotorCurrent() { - return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2; + return (wristmotor1.getOutputCurrent()); } public synchronized boolean isFinished() { @@ -335,9 +334,8 @@ public class Wrist extends Subsystem implements ControlLoopable 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 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)); From 2f8480b56ef5f11644087d89340996fe125ebe78 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Sun, 17 Feb 2019 16:21:58 -0700 Subject: [PATCH 4/4] added in robot init stuff --- 2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java | 2 +- .../src/main/java/org/usfirst/frc4388/robot/Robot.java | 2 ++ .../java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 6 +++--- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index 4605dfd..14203a2 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -53,7 +53,7 @@ public class OI Expand.whenPressed(new WristPlacement(true)); JoystickButton Contract = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON); - Contract.whenPressed(new WristPlacement(true)); + Contract.whenPressed(new WristPlacement(false)); //JoystickButton endEfector = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index c2c6a02..a693693 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -30,6 +30,7 @@ public class Robot extends IterativeRobot public static final Drive drive = new Drive(); public static final Arm arm = new Arm(); + public static final Wrist wrist = new Wrist(); @@ -59,6 +60,7 @@ public class Robot extends IterativeRobot controlLoop.addLoopable(drive); controlLoop.addLoopable(arm); + controlLoop.addLoopable(wrist); operationModeChooser = new SendableChooser(); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index bfb7817..250193f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -102,7 +102,7 @@ public class Wrist extends Subsystem implements ControlLoopable 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); @@ -139,7 +139,7 @@ public class Wrist extends Subsystem implements ControlLoopable } public void setSpeed(double speed) { - wristmotor1.set(ControlMode.PercentOutput, speed*0.3); + wristmotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.MANUAL); } @@ -287,7 +287,7 @@ public class Wrist extends Subsystem implements ControlLoopable private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); - setSpeedJoystick(joyStickSpeed*.10); + setSpeedJoystick(joyStickSpeed*.50); } /* public void setShiftState(ElevatorSpeedShiftState state) {