mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 00:28:05 -06:00
fix
fixed code so it builds, added toggle to opperator controlor for hatch and ball
This commit is contained in:
lukesta182
@@ -7,7 +7,7 @@ import buttons.XBoxTriggerButton;
|
||||
import org.usfirst.frc4388.controller.IHandController;
|
||||
import org.usfirst.frc4388.controller.XboxController;
|
||||
import org.usfirst.frc4388.robot.commands.*;
|
||||
import org.usfirst.frc4388.robot.constants.LEDPatterns;
|
||||
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
@@ -50,11 +50,10 @@ public class OI
|
||||
|
||||
JoystickButton Expand = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON);
|
||||
Expand.whenPressed(new WristPlacement(true));
|
||||
Expand.whenPressed(new setLEDPattern(LEDPatterns.SOLID_RED));
|
||||
|
||||
JoystickButton Contract = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
|
||||
Contract.whenPressed(new WristPlacement(false));
|
||||
Contract.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN));
|
||||
Contract.whenPressed(new WristPlacement(false));
|
||||
|
||||
|
||||
JoystickButton liftBothIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
|
||||
liftBothIntake.whenPressed(new HatchAndBallUp());
|
||||
@@ -69,9 +68,7 @@ public class OI
|
||||
|
||||
JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON);
|
||||
safteySwitch.whenPressed(new setClimberSafety(true));
|
||||
safteySwitch.whenPressed(new setLEDPattern(LEDPatterns.SOLID_YELLOW));
|
||||
safteySwitch.whenReleased(new setClimberSafety(false));
|
||||
safteySwitch.whenReleased(new setLEDPattern(LEDPatterns.C1_HEARTBEAT_FAST));
|
||||
|
||||
JoystickButton climbUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
|
||||
JoystickButton climbDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_TRIGGER_AXIS);
|
||||
@@ -83,10 +80,11 @@ public class OI
|
||||
|
||||
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 DriveSpeedShift(false));
|
||||
// shiftDown.whenPressed(new LEDIndicators(false));
|
||||
//Operator Xbox
|
||||
/*
|
||||
JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
|
||||
@@ -96,12 +94,8 @@ public class OI
|
||||
JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
|
||||
CloseIntake.whenPressed(new IntakePosition(false));
|
||||
*/
|
||||
|
||||
JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
|
||||
stow.whenPressed(new ArmStow());
|
||||
|
||||
JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON);
|
||||
lowHeight.whenPressed(new GrabFromLoadingSatation());
|
||||
JoystickButton lowheight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
|
||||
lowheight.whenPressed(new GrabFromLoadingSatation());
|
||||
|
||||
SmartDashboard.putData("switch to manuel", new SetManual());
|
||||
SmartDashboard.putData("run arm test", new ArmTest());
|
||||
|
||||
+3
-3
@@ -9,16 +9,16 @@ package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.CommandGroup;
|
||||
|
||||
public class ArmStow extends CommandGroup {
|
||||
public class StowArm extends CommandGroup {
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public ArmStow() {
|
||||
public StowArm() {
|
||||
addSequential(new WristSetPositionPID(0), 1);
|
||||
addParallel(new HatchFlip(false));
|
||||
addParallel(new WristPlacement(true));
|
||||
addSequential(new ArmSetPositionMM(10));
|
||||
|
||||
// Add Commands here:
|
||||
// e.g. addSequential(new Command1());
|
||||
// addSequential(new Command2());
|
||||
// these will run in order.
|
||||
@@ -1,38 +1,9 @@
|
||||
package org.usfirst.frc4388.robot.constants;
|
||||
|
||||
public enum LEDPatterns {
|
||||
// PALLETTE PATTERNS
|
||||
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
|
||||
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
|
||||
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
|
||||
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
|
||||
RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
|
||||
RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
|
||||
RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
|
||||
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
|
||||
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
|
||||
RED(0.61f), BLACK(0.99f);
|
||||
|
||||
// COLOR 1 PATTERNS
|
||||
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
|
||||
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
|
||||
|
||||
// COLOR 2 PATTERNS
|
||||
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
|
||||
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
|
||||
|
||||
// COLOR 1 AND 2 PATTERNS
|
||||
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
|
||||
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
|
||||
|
||||
// SOLID COLORS
|
||||
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
|
||||
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
|
||||
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
|
||||
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
|
||||
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
|
||||
|
||||
// GETTERS/SETTERS
|
||||
private final float id;
|
||||
LEDPatterns(float id) { this.id = id; }
|
||||
public float getValue() { return id; }
|
||||
}
|
||||
private final float id;
|
||||
LEDPatterns(float id) { this.id = id; }
|
||||
public float getValue() { return id; }
|
||||
}
|
||||
@@ -12,6 +12,7 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder;
|
||||
import org.usfirst.frc4388.utility.TalonSRXFactory;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.DemandType;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
||||
@@ -31,12 +32,11 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
{
|
||||
private static Arm instance;
|
||||
|
||||
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
|
||||
public static enum ArmPositionMode { CARGO, HATCH };
|
||||
|
||||
// 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 = (1);
|
||||
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC};
|
||||
|
||||
// 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 = (1);
|
||||
|
||||
|
||||
private double periodMs;
|
||||
private double lastControlLoopUpdatePeriod = 0.0;
|
||||
@@ -48,7 +48,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
public static final double AUTO_ZERO_SPEED = -0.3;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_HI = 20;
|
||||
public static final double JOYSTICK_INCHES_PER_MS_LO = 20;
|
||||
|
||||
|
||||
// Defined positions
|
||||
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
|
||||
public static final double ZERO_POSITION_INCHES = -0.25;
|
||||
@@ -57,16 +57,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
public static final double MAX_POSITION_INCHES = 3900;
|
||||
public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
|
||||
|
||||
public static final double HATCH_LOW_POSITION_WORLD = 135;
|
||||
public static final double HATCH_MID_POSITION_WORLD = 2400;
|
||||
public static final double HATCH_HIGH_POSITION_WORLD = 3800;
|
||||
public static final double CARGO_LOW_POSITION_WORLD = 1500;
|
||||
public static final double CARGO_MID_POSITION_WORLD = 2900;
|
||||
public static final double CARGO_HIGH_POSITION_WORLD = 4300;
|
||||
public static final double CARGO_PICKUP_POSITION_WORLD = 0;
|
||||
public static final double HATCH_PICKUP_POSITION_WORLD = 0;
|
||||
|
||||
/*public static final double SWITCH_POSITION_INCHES = 24.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
|
||||
@@ -74,70 +65,83 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
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;*/
|
||||
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_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 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;
|
||||
public static int MM_SLOT = 1;
|
||||
public static int MP_SLOT = 2;
|
||||
|
||||
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 = 1;//0.01;
|
||||
public static final double KF_DOWN = 0;// 0.0;
|
||||
public static final double P_Value = 0.5;// 2;
|
||||
public static final double I_Value = 0.0005;// 0.00030;
|
||||
public static final double D_Value = 100;// 200;
|
||||
public static final double F_Value = 0.75; // 1023 / 1360 max speed (ticks/100ms)
|
||||
public static final double maxGravityComp = 0.08;
|
||||
public static final double RampRate = 0;// 0.0;
|
||||
public static final int A_value = 450;
|
||||
public static final int CV_value = 740;
|
||||
|
||||
|
||||
|
||||
|
||||
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.01;
|
||||
public static final double KF_DOWN = 0.0;
|
||||
public static final double P_Value = 2;
|
||||
public static final double I_Value = 0.00030;
|
||||
public static final double D_Value = 200;
|
||||
public static final double RampRate = 0.0;
|
||||
private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later
|
||||
public static final double PID_ERROR_INCHES = 5;
|
||||
LimitSwitchSource limitSwitchSource;
|
||||
|
||||
|
||||
// Pneumatics
|
||||
private Solenoid speedShift;
|
||||
|
||||
//DPAD
|
||||
public static final double DPAD_UP = 0;
|
||||
public static final double DPAD_RIGHT = 90;
|
||||
public static final double DPAD_DOWN = 180;
|
||||
public static final double DPAD_LEFT = 270;
|
||||
public static final double DPAD_RELEASED = -1;
|
||||
|
||||
// Misc
|
||||
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
|
||||
public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
|
||||
private boolean isFinished;
|
||||
private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL;
|
||||
public ArmPositionMode armPositionMode = ArmPositionMode.HATCH;
|
||||
private double targetPositionInchesPID = 0;
|
||||
private double targetPositionInchesMM = 0;
|
||||
private boolean firstMpPoint;
|
||||
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
|
||||
private double p = 0;
|
||||
|
||||
|
||||
|
||||
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.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS);
|
||||
motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS);
|
||||
motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS);
|
||||
motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS);
|
||||
|
||||
motor1.selectProfileSlot(MM_SLOT, 0);
|
||||
motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
motor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
motor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
motor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
|
||||
motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
|
||||
@@ -145,8 +149,9 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
motor2.setNeutralMode(NeutralMode.Brake);
|
||||
motor1.enableCurrentLimit(true);
|
||||
motorControllers.add(motor1);
|
||||
|
||||
|
||||
//motor1.setSelectedSensorPosition(0, , );
|
||||
|
||||
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("An error occurred in the Arm constructor");
|
||||
@@ -156,55 +161,79 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
@Override
|
||||
public void initDefaultCommand() {
|
||||
}
|
||||
|
||||
|
||||
public void resetZeroPosition(double position) {
|
||||
mpController.resetZeroPosition(position);
|
||||
}
|
||||
}
|
||||
public void resetEncoder(){
|
||||
motor1.setPosition(0);
|
||||
}
|
||||
|
||||
|
||||
private synchronized void setArmControlMode(ArmControlMode controlMode) {
|
||||
this.armControlMode = controlMode;
|
||||
}
|
||||
|
||||
|
||||
private synchronized ArmControlMode getArmControlMode() {
|
||||
return this.armControlMode;
|
||||
}
|
||||
|
||||
public synchronized void setArmPositionMode(ArmPositionMode controlMode) {
|
||||
this.armPositionMode = controlMode;
|
||||
}
|
||||
|
||||
private synchronized ArmPositionMode getArmPositionMode() {
|
||||
return this.armPositionMode;
|
||||
}
|
||||
|
||||
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 setPositionMM(double targetPositionInches){
|
||||
motor1.set(ControlMode.MotionMagic, targetPositionInches);
|
||||
System.err.println(motor1.getControlMode());
|
||||
motor1.selectProfileSlot(MM_SLOT, 0);
|
||||
setArmControlMode(ArmControlMode.MOTION_MAGIC);
|
||||
updatePositionMM(targetPositionInches);
|
||||
setFinished(false);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public double calcGravityCompensationAtCurrentPosition() {
|
||||
int ticks = motor1.getSelectedSensorPosition();
|
||||
double degreesFromDown = (ticks+920)*(360.0/(4096*3));
|
||||
double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180);
|
||||
System.err.println("comp(" + degreesFromDown + "^) = " + compensation);
|
||||
return compensation;
|
||||
}
|
||||
public void updatePositionMM(double targetPositionInches){
|
||||
targetPositionInchesMM = limitPosition(targetPositionInches);
|
||||
//double startPositionInches = motor1.getPositionWorld();
|
||||
double compensation = calcGravityCompensationAtCurrentPosition();
|
||||
//System.err.println("compensation = " + compensation);
|
||||
// motor1.set(ControlMode.MotionMagic, targetPositionInches);
|
||||
motor1.set(ControlMode.MotionMagic, targetPositionInches, DemandType.ArbitraryFeedForward, compensation);
|
||||
//System.err.println(motor1.getControlMode());
|
||||
motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS);
|
||||
|
||||
|
||||
}
|
||||
|
||||
public void setPositionPID(double targetPositionInches) {
|
||||
motor1.set(ControlMode.Position, targetPositionInches);
|
||||
mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set
|
||||
updatePositionPID(targetPositionInches);
|
||||
setArmControlMode(ArmControlMode.JOYSTICK_PID);
|
||||
setArmControlMode(ArmControlMode.JOYSTICK_PID);
|
||||
setFinished(false);
|
||||
}
|
||||
|
||||
|
||||
public void updatePositionPID(double targetPositionInches) {
|
||||
targetPositionInchesPID = limitPosition(targetPositionInches);
|
||||
if (limitPosition(motor1.getPositionWorld()) == MIN_POSITION_INCHES){
|
||||
resetEncoder();
|
||||
}
|
||||
double startPositionInches = motor1.getPositionWorld();
|
||||
//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
|
||||
//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
|
||||
motor1.set(ControlMode.Position, targetPositionInches);
|
||||
motor1.configClosedloopRamp(0);
|
||||
//motor1.configPeakCurrentLimit(5);
|
||||
@@ -216,15 +245,15 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
//System.err.println(motor1.getControlMode());
|
||||
//System.err.print(motor1.getClosedLoopError());
|
||||
}
|
||||
|
||||
|
||||
public void setPositionMP(double targetPositionInches) {
|
||||
double startPositionInches = motor1.getPositionWorld();
|
||||
mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2);
|
||||
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;
|
||||
@@ -232,7 +261,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
else if (targetPosition > MAX_POSITION_INCHES) {
|
||||
return MAX_POSITION_INCHES;
|
||||
}
|
||||
|
||||
|
||||
return targetPosition;
|
||||
}
|
||||
@Override
|
||||
@@ -243,51 +272,6 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
@@ -300,7 +284,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
|
||||
}
|
||||
lastControlLoopUpdateTimestamp = currentTimestamp;
|
||||
|
||||
|
||||
if (motor1.getSensorCollection().isRevLimitSwitchClosed()){
|
||||
resetEncoder();
|
||||
}
|
||||
@@ -311,69 +295,58 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
}
|
||||
else if (!isFinished) {
|
||||
if (armControlMode == ArmControlMode.MOTION_PROFILE) {
|
||||
isFinished = mpController.controlLoopUpdate(getPositionInches());
|
||||
|
||||
isFinished = mpController.controlLoopUpdate(getPositionInches());
|
||||
|
||||
}
|
||||
if (armControlMode == ArmControlMode.JOYSTICK_PID){
|
||||
//System.err.println(motor1.getControlMode());
|
||||
int dPadAngle = Robot.oi.getOperatorController().getDpadAngle();
|
||||
if (dPadAngle == DPAD_RELEASED){
|
||||
controlPidWithJoystick();
|
||||
} else {
|
||||
controlPidWithDPad(dPadAngle);
|
||||
}
|
||||
System.err.println(motor1.getControlMode());
|
||||
}
|
||||
|
||||
if (armControlMode == ArmControlMode.MOTION_MAGIC){
|
||||
controlMMWithJoystick();
|
||||
//System.err.println(motor1.getControlMode());
|
||||
}
|
||||
|
||||
/*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) {
|
||||
isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
|
||||
isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
|
||||
}
|
||||
else if (armControlMode == ArmControlMode.ADAPTIVE_PURSUIT) {
|
||||
updatePose();
|
||||
isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
|
||||
isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
|
||||
}*/
|
||||
}
|
||||
}
|
||||
|
||||
private void controlPidWithDPad(int dPadAngle){
|
||||
if (armPositionMode == ArmPositionMode.HATCH){
|
||||
if (dPadAngle == DPAD_UP){
|
||||
updatePositionPID(HATCH_HIGH_POSITION_WORLD);
|
||||
} else if (dPadAngle == DPAD_RIGHT){
|
||||
updatePositionPID(HATCH_MID_POSITION_WORLD);
|
||||
} else if (dPadAngle == DPAD_DOWN){
|
||||
setPositionPID(HATCH_LOW_POSITION_WORLD);
|
||||
} else if (dPadAngle == DPAD_LEFT){
|
||||
//updatePositionPID(HATCH_PICKUP_POSITION_WORLD);
|
||||
}
|
||||
} else if (armPositionMode == ArmPositionMode.CARGO){
|
||||
if (dPadAngle == DPAD_UP){
|
||||
updatePositionPID(CARGO_HIGH_POSITION_WORLD);
|
||||
} else if (dPadAngle == DPAD_RIGHT){
|
||||
updatePositionPID(CARGO_MID_POSITION_WORLD);
|
||||
} else if (dPadAngle == DPAD_DOWN){
|
||||
updatePositionPID(CARGO_LOW_POSITION_WORLD);
|
||||
} else if (dPadAngle == DPAD_LEFT){
|
||||
//updatePositionPID(CARGO_PICKUP_POSITION_WORLD);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private void controlPidWithJoystick() {
|
||||
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
double deltaPosition = joystickPosition * joystickInchesPerMs;
|
||||
targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
|
||||
updatePositionPID(targetPositionInchesPID);
|
||||
}
|
||||
private void controlMMWithJoystick() {
|
||||
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
double deltaPosition = joystickPosition * joystickInchesPerMs;
|
||||
targetPositionInchesMM = targetPositionInchesMM + deltaPosition;
|
||||
updatePositionMM(targetPositionInchesMM);
|
||||
|
||||
|
||||
}
|
||||
|
||||
private void ControlWithJoystickhold(){
|
||||
double holdPosition = motor1.getPositionWorld();
|
||||
updatePositionPID(holdPosition);
|
||||
|
||||
}
|
||||
|
||||
|
||||
private void controlManualWithJoystick() {
|
||||
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
|
||||
setSpeedJoystick((joyStickSpeed*.30)+.1);
|
||||
setSpeedJoystick((joyStickSpeed*.3)+.08);
|
||||
}
|
||||
/*
|
||||
public void setShiftState(ElevatorSpeedShiftState state) {
|
||||
@@ -388,7 +361,7 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
mpController.setPID(pidPIDParamsLoGear, PID_SLOT);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public ElevatorSpeedShiftState getShiftState() {
|
||||
return shiftState;
|
||||
}
|
||||
@@ -396,49 +369,51 @@ public class Arm extends Subsystem implements ControlLoopable
|
||||
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) {
|
||||
//System.err.println("the encoder is right after this");
|
||||
try {
|
||||
|
||||
SmartDashboard.putNumber("Arm Position", motor1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent());
|
||||
SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent());
|
||||
|
||||
SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld());
|
||||
//SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent());
|
||||
//SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent());
|
||||
SmartDashboard.putNumber("sensor vel", motor1.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent());
|
||||
SmartDashboard.putNumber("arm pid error", motor1.getClosedLoopError());
|
||||
SmartDashboard.putNumber("arm motor output", motor1.getMotorOutputPercent());
|
||||
|
||||
SmartDashboard.putNumber("Arm Target MM Position", targetPositionInchesMM);
|
||||
//SmartDashboard.putNumber("arm output", motor1.getMotorOutputPercent());
|
||||
// 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("Arm Target PID Position", targetPositionInchesPID);
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("Drivetrain update status error" +e.getMessage());
|
||||
System.err.println("Arm update status error" +e.getMessage());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
public static Arm getInstance() {
|
||||
if(instance == null) {
|
||||
instance = new Arm();
|
||||
|
||||
@@ -9,12 +9,10 @@ package org.usfirst.frc4388.robot.subsystems;
|
||||
|
||||
import java.util.HashMap;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.constants.LEDPatterns;
|
||||
|
||||
import edu.wpi.first.wpilibj.Spark;
|
||||
import edu.wpi.first.wpilibj.command.Subsystem;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
@@ -26,17 +24,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
public static Spark LEDController = new Spark(LED_SPARK_ID);
|
||||
|
||||
public LED(){
|
||||
setPattern(LEDPatterns.C1_HEARTBEAT_FAST);
|
||||
setPattern(LEDPatterns.RED);
|
||||
}
|
||||
|
||||
public void periodic() {
|
||||
LEDController.set(currentLED);
|
||||
SmartDashboard.putNumber("LED", currentLED);
|
||||
}
|
||||
|
||||
public void setPattern(LEDPatterns pattern){
|
||||
currentLED = pattern.getValue();
|
||||
LEDController.set(currentLED);
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
Reference in New Issue
Block a user