Resolving some issues related to the use of the CANcoder, and added some missing constants and instantiations for PIDControllers related to the Arm
This commit is contained in:
parent
ecf4916b93
commit
c1dddcace5
@ -1,15 +1,27 @@
|
|||||||
package frc.robot.constants;
|
package frc.robot.constants;
|
||||||
|
|
||||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||||
|
import com.ctre.phoenix6.signals.SensorDirectionValue;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.util.Units;
|
||||||
|
|
||||||
public class ArmConstants {
|
public class ArmConstants {
|
||||||
public static final int kArmMotorID = 0;
|
public static final int kArmMotorID = 0;
|
||||||
public static final int kCANcoderID = 0;
|
public static final int kCANcoderID = 0;
|
||||||
|
|
||||||
public static final double kEncoderConversionFactor = 0;
|
|
||||||
|
|
||||||
public static final double kArmMaxVelocity = 0;
|
public static final double kArmMaxVelocity = 0;
|
||||||
|
|
||||||
|
public static final double kPositionalP = 0;
|
||||||
|
public static final double kPositionalI = 0;
|
||||||
|
public static final double kPositionalD = 0;
|
||||||
|
public static final double kPositionalTolerance = Units.degreesToRadians(1);
|
||||||
|
|
||||||
|
public static final double kVelocityP = 0;
|
||||||
|
public static final double kVelocityI = 0;
|
||||||
|
public static final double kVelocityD = 0;
|
||||||
|
// TODO Is this reasonable?
|
||||||
|
public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;
|
||||||
|
|
||||||
public static final double kArmCoralIntakePosition = 0;
|
public static final double kArmCoralIntakePosition = 0;
|
||||||
public static final double kArmL1Position = 0;
|
public static final double kArmL1Position = 0;
|
||||||
public static final double kArmL2Position = 0;
|
public static final double kArmL2Position = 0;
|
||||||
@ -21,5 +33,9 @@ public class ArmConstants {
|
|||||||
|
|
||||||
public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
|
public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
|
||||||
|
|
||||||
|
static {
|
||||||
|
canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
|
||||||
|
canCoderConfig.MagnetSensor.MagnetOffset = 0.0;
|
||||||
|
canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
@ -29,7 +29,26 @@ public class Arm extends SubsystemBase {
|
|||||||
MotorType.kBrushless
|
MotorType.kBrushless
|
||||||
);
|
);
|
||||||
|
|
||||||
|
positionController = new PIDController(
|
||||||
|
ArmConstants.kPositionalP,
|
||||||
|
ArmConstants.kPositionalI,
|
||||||
|
ArmConstants.kPositionalD
|
||||||
|
);
|
||||||
|
|
||||||
|
// TODO: Generate constants for continuous input range based on CANcoder configuration?
|
||||||
|
positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
|
||||||
|
positionController.setTolerance(ArmConstants.kPositionalTolerance);
|
||||||
|
|
||||||
|
velocityController = new PIDController(
|
||||||
|
ArmConstants.kVelocityP,
|
||||||
|
ArmConstants.kVelocityI,
|
||||||
|
ArmConstants.kVelocityD
|
||||||
|
);
|
||||||
|
|
||||||
|
velocityController.setTolerance(ArmConstants.kVelocityTolerance);
|
||||||
|
|
||||||
canCoder = new CANcoder(ArmConstants.kCANcoderID);
|
canCoder = new CANcoder(ArmConstants.kCANcoderID);
|
||||||
|
canCoder.getConfigurator().apply(ArmConstants.canCoderConfig);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -85,7 +104,7 @@ public class Arm extends SubsystemBase {
|
|||||||
}
|
}
|
||||||
|
|
||||||
public double getEncoderPosition() {
|
public double getEncoderPosition() {
|
||||||
return Units.rotationsToRadians(canCoder.getPosition().getValueAsDouble());
|
return Units.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble());
|
||||||
}
|
}
|
||||||
|
|
||||||
public double getEncoderVelocity() {
|
public double getEncoderVelocity() {
|
||||||
|
Loading…
Reference in New Issue
Block a user