Team4388/2019-Hit-or-Miss
1
0
Fork 0
mirror of https://github.com/Team4388/2019-Hit-or-Miss.git synced 2026-06-09 00:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Potential fix for arm motor control mode

Browse Source 
This may be a proper fix for the workaround in updatePositionPID, but needs to be verified on the robot.
This commit is contained in:
Dave Staudacher
2019-03-07 07:33:20 -07:00
parent 1b120ac85c
commit cfcbf2ce4a
1 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+11 -11
View File
@@ -87,7 +87,9 @@ public class Arm extends Subsystem implements ControlLoopable
public static final double KF_UP = 0.06; public static final double KF_UP = 0.06;
public static final double KF_DOWN = 0.01; public static final double KF_DOWN = 0.01;
public static final double P_Value = 0.6; public static final double P_Value = 0.6;
public static final double I_Value = .0005; public static final double I_Value = 0.0005;
public static final double D_Value = 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 = 1.0; public static final double PID_ERROR_INCHES = 1.0;
LimitSwitchSource limitSwitchSource; LimitSwitchSource limitSwitchSource;
// Pneumatics // Pneumatics
@@ -156,8 +158,8 @@ public class Arm extends Subsystem implements ControlLoopable
} }
public void setPositionPID(double targetPositionInches) { public void setPositionPID(double targetPositionInches) {
motor1.set(ControlMode.Position, targetPositionInches);
mpController.setPIDSlot(PID_SLOT); mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set
updatePositionPID(targetPositionInches); updatePositionPID(targetPositionInches);
setArmControlMode(ArmControlMode.JOYSTICK_PID); setArmControlMode(ArmControlMode.JOYSTICK_PID);
setFinished(false); setFinished(false);
@@ -166,11 +168,11 @@ public class Arm extends Subsystem implements ControlLoopable
public void updatePositionPID(double targetPositionInches) { public void updatePositionPID(double targetPositionInches) {
targetPositionInchesPID = limitPosition(targetPositionInches); targetPositionInchesPID = limitPosition(targetPositionInches);
double startPositionInches = motor1.getPositionWorld(); 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.set(ControlMode.Position, targetPositionInches);
motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); //motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS); //motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); //motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS);
System.err.println(motor1.getControlMode()); System.err.println(motor1.getControlMode());
//System.err.print(motor1.getClosedLoopError()); //System.err.print(motor1.getClosedLoopError());
} }
@@ -197,9 +199,7 @@ public class Arm extends Subsystem implements ControlLoopable
public void setPeriodMs(long periodMs) { public void setPeriodMs(long periodMs) {
mpController = new MPTalonPIDController(periodMs, motorControllers); mpController = new MPTalonPIDController(periodMs, motorControllers);
mpController.setPID(mpPIDParams, MP_SLOT); mpController.setPID(mpPIDParams, MP_SLOT);
mpController.setPID(pidPIDParamsHiGear, PID_SLOT); mpController.setPID(armPIDParams, PID_SLOT);
mpController.setPIDSlot(PID_SLOT);
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
mpController.setPIDSlot(PID_SLOT); mpController.setPIDSlot(PID_SLOT);
this.periodMs = periodMs; this.periodMs = periodMs;
} }