/**
* ***************************
*
* <p>Completes everything required for PID to work: - Sets encoder/potentiometer type - Sets the
* P, I, D, and F terms - configures min and max voltages
*
* <p>***************************
*/
private void setupPID() {
leftMotor1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
rightMotor1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
leftMotor1.configNominalOutputVoltage(+0f, -0f);
rightMotor1.configNominalOutputVoltage(+0f, -0f);
leftMotor1.configPeakOutputVoltage(+12.0f, -12.0f);
rightMotor1.configPeakOutputVoltage(+12.0f, -12.0f);
// leftMotor1.
setPID(0.62, 0.006, 0.0, 0.36341);
}
public DriveTrainSubsystem() {
RIGHT_MOTOR1 = new CANTalon(RobotMap.DRIVERIGHT_MOTOR1);
RIGHT_MOTOR2 = new CANTalon(RobotMap.DRIVERIGHT_MOTOR2);
LEFT_MOTOR1 = new CANTalon(RobotMap.DRIVELEFT_MOTOR1);
LEFT_MOTOR2 = new CANTalon(RobotMap.DRIVELEFT_MOTOR2);
LEFT_MOTOR1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
LEFT_MOTOR1.setPosition(0);
RIGHT_MOTOR1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
RIGHT_MOTOR1.setPosition(0);
LiveWindow.addActuator("Drive", "R1", RIGHT_MOTOR1);
LiveWindow.addActuator("Drive", "R2", RIGHT_MOTOR2);
LiveWindow.addActuator("Drive", "L1", LEFT_MOTOR1);
LiveWindow.addActuator("Drive", "L2", LEFT_MOTOR2);
}
private static CANTalon initCANTalon(
CANTalon talon,
FeedbackDevice device,
boolean reverseSensor,
int codesPerRev,
int acceptableErr,
double rightDriveKp,
double rightDriveKi,
double rightDriveKd) {
talon.reset();
talon.enableZeroSensorPositionOnIndex(true, true);
talon.setPosition(0);
talon.setFeedbackDevice(device);
talon.reverseSensor(reverseSensor);
if (FeedbackDevice.QuadEncoder.equals(device)) talon.configEncoderCodesPerRev(codesPerRev);
else if (FeedbackDevice.AnalogPot.equals(device)) talon.configPotentiometerTurns(codesPerRev);
talon.configNominalOutputVoltage(NOMINAL_FORWARD_VOLTAGE, NOMINAL_REVERSE_VOLTAGE);
talon.configPeakOutputVoltage(PEAK_FORWARD_VOLTAGE, PEAK_REVERSE_VOLTAGE);
talon.setAllowableClosedLoopErr(acceptableErr);
talon.setPID(rightDriveKp, rightDriveKi, rightDriveKd);
return talon;
}
public static void init() {
if (inited) {
talon_arm = new CANTalon(talon_arm_id);
talon_arm.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Absolute);
JoyDrive.init();
}
}
public SpeedController() {
speedController.setFeedbackDevice(FeedbackDevice.QuadEncoder);
speedController.configEncoderCodesPerRev(20);
speedController.setP(0.75);
speedController.setI(0.01);
speedController.setD(0.0);
speedController.setCloseLoopRampRate(.01);
}
public MasterCANTalon(int port) {
talon = new CANTalon(port);
talon.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
talon.reverseSensor(false);
talon.setProfile(0);
talon.setF(0.1722);
talon.setP(0.1);
talon.setI(0);
talon.setD(0);
talon.configNominalOutputVoltage(+0.0f, -0.0f);
talon.configPeakOutputVoltage(+12.0f, -12.0f);
talon.changeControlMode(TalonControlMode.Speed);
}
// static initializer
public static void initialize() {
if (!initialized) {
gamepad = new Joystick(GAMEPAD_ID);
initialized = true;
teleopMode = false;
// create and initialize arm motor
frontArmMotor = new CANTalon(FRONT_ARM_MOTOR_ID);
if (frontArmMotor != null) {
System.out.println("Initializing front arm motor (position control)...");
// VERY IMPORTANT - resets talon faults to render them usable again!!
frontArmMotor.clearStickyFaults();
frontArmMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
frontArmMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
// set brake mode
frontArmMotor.enableBrakeMode(true);
// initializes encoder to zero
frontArmMotor.setPosition(0);
} else System.out.println("ERROR: Front Arm motor not initialized!");
// create and initialize roller motor
frontArmRollerMotor = new CANTalon(FRONT_ARM_ROLLER_ID);
if (frontArmRollerMotor != null) {
System.out.println("Initializing front arm roller motor (PercentVbus control)...");
frontArmRollerMotor.clearStickyFaults();
// set up roller motor for percent Vbus control mode
frontArmRollerMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
frontArmRollerMotor.enableBrakeMode(false);
// initializes speed of rollers to zero
frontArmRollerMotor.set(0);
} else System.out.println("ERROR: Front Arm roller motor not initialized!");
}
}
@Override
public TalonSRX setFeedbackDevice(FeedbackDevice device) {
talon.setFeedbackDevice(edu.wpi.first.wpilibj.CANTalon.FeedbackDevice.valueOf(device.value()));
switch (device) {
case ANALOG_POTENTIOMETER:
case ANALOG_ENCODER:
if (selectedAnalogInput != null) {
selectedInput = selectedAnalogInput;
} else {
Strongback.logger(getClass())
.error(
"Unable to use the analog input for feedback, since the Talon SRX (device "
+ getDeviceID()
+ ") was not instantiated with an analog input. Check how this device was created using Strongback's Hardware class.");
selectedInput = NO_OP_SENSOR;
}
break;
case QUADRATURE_ENCODER:
case ENCODER_RISING:
if (selectedEncoderInput != null) {
selectedInput = selectedEncoderInput;
} else {
Strongback.logger(getClass())
.error(
"Unable to use the quadrature encoder input for feedback, since the Talon SRX (device "
+ getDeviceID()
+ ") was not instantiated with an encoder input. Check how this device was created using Strongback's Hardware class.");
selectedInput = NO_OP_SENSOR;
}
break;
case ENCODER_FALLING:
// for 2015 the Talon SRX firmware did not support the falling or rising mode ...
selectedInput = NO_OP_SENSOR;
break;
}
return this;
}
@Override
public void setFeedbackDevice(FeedbackDevice devType) {
super.setFeedbackDevice(devType);
feedbackDeviceIsPot = devType == FeedbackDevice.AnalogPot;
}
/** Initializes sensors and controllers */
public static void init() {
compressorSolenoid = new Solenoid(0, 1);
// RightShooterTalon = new CANTalon(6);
// LeftShooterTalon = new CANTalon(7);
// PneumaticCanRight = new Solenoid(0);
// PneumaticCanLeft = new Solenoid(1);
// DRIVE
driveCurrentManager = new NewCurrentManager(12);
DriveFrontRightTalon = new CANTalon(RobotConstants.frontRightMotorPort);
DriveMidRightTalon = new CANTalon(RobotConstants.midRightMotorPort);
DriveRearRightTalon = new CANTalon(RobotConstants.rearRightMotorPort);
DriveFrontLeftTalon = new CANTalon(RobotConstants.frontLeftMotorPort);
DriveMidLeftTalon = new CANTalon(RobotConstants.midLeftMotorPort);
DriveRearLeftTalon = new CANTalon(RobotConstants.rearLeftMotorPort);
DriveSolenoid =
new DoubleSolenoid(
1, RobotConstants.driveRightSolenoidPortA, RobotConstants.driveRightSolenoidPortB);
// DriveLeftSolenoid = new
// DoubleSolenoid(1,RobotConstants.driveRightSolenoidPortA,RobotConstants.driveRightSolenoidPortB);
DriveFrontRightMotor =
new Motor(
DriveFrontRightTalon, null, null, 0.01, 0.0, 0.0, null); // Robot.driveCurrentManager);
DriveMidRightMotor =
new Motor(
DriveMidRightTalon, null, null, 0.01, 0.0, 0.0, null); // Robot.driveCurrentManager);
DriveFrontLeftMotor =
new Motor(DriveFrontLeftTalon, null, null, 0.01, 0, 0, null); // Robot.driveCurrentManager);
DriveRearRightMotor =
new Motor(DriveRearRightTalon, null, null, 0.01, 0, 0, null); // Robot.driveCurrentManager);
DriveMidLeftMotor =
new Motor(
DriveMidLeftTalon, null, null, 0.01, 0.0, 0.0, null); // Robot.driveCurrentManager);
DriveRearLeftMotor =
new Motor(DriveRearLeftTalon, null, null, 0.01, 0, 0, null); // Robot.driveCurrentManager);
DriveEncoder = new Encoder(RobotConstants.DriveEncoderPortA, RobotConstants.DriveEncoderPortB);
DriveEncoder.setDistancePerPulse(RobotConstants.encoderDistancePerPulse);
// ROLLER
ArmTalon = new CANTalon(RobotConstants.ArmTalonPort);
RollerTalon = new CANTalon(RobotConstants.RollerTalonPort);
ArmSolenoid = new Solenoid(1, RobotConstants.ArmSolenoidPort);
ArmPotentiometer = new AnalogPotentiometer(RobotConstants.ArmPotentiometerPort);
ArmMotor = new Motor(ArmTalon, null, ArmPotentiometer, 30, 0.0, 0.0, 0.001);
RollerMotor = new Motor(RollerTalon, null, null, 0.01, 0.0, 0.0);
// SCALE
ScaleRightTalon = new CANTalon(RobotConstants.ScaleRightTalonPort);
ScaleLeftTalon = new CANTalon(RobotConstants.ScaleLeftTalonPort);
// ScaleRightEncoder = new
// Encoder(RobotConstants.ScaleRightEncoderPortA,RobotConstants.ScaleRightEncoderPortB);
// ScaleLeftEncoder = new
// Encoder(RobotConstants.ScaleLeftEncoderPortA,RobotConstants.ScaleLeftEncoderPortB);
// ScaleSolenoid = new Solenoid(1,RobotConstants.ScaleSolenoidPort);
ScaleRightMotor = new Motor(ScaleRightTalon, null, null, 0.01, 0.0, 0.0);
ScaleLeftMotor = new Motor(ScaleLeftTalon, null, null, 0.01, 0.0, 0.0);
// SHOOTER
ShooterLeftTalon = new CANTalon(RobotConstants.ShooterLeftTalonPort);
ShooterRightTalon = new CANTalon(RobotConstants.ShooterRightTalonPort);
ShooterLiftTalon = new CANTalon(RobotConstants.ShooterLiftTalonPort);
ShooterLiftTalon.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
// ShooterLeftTalon.set(0.5);
// ShooterRightTalon.set(0.5);
ShooterBrakeSolenoid = new Solenoid(1, RobotConstants.ShooterBrakeSolenoidPortB);
// ShooterBrakeSolenoid = new DoubleSolenoid(1,5,1);
// ShooterSolenoidA = new Solenoid(1,3);
// ShooterSolenoidB = new Solenoid(0,0);
ShooterSolenoid = new Solenoid(0, RobotConstants.ShooterSolenoidPort);
// ShooterEncoder = new
// Encoder(RobotConstants.ShooterLiftEncoderPort,RobotConstants.ShooterBrakeSolenoidPort);
ShooterLeftMotor = new Motor(ShooterLeftTalon, null, null, 0.01, 0.0, 0.0);
ShooterRightMotor = new Motor(ShooterRightTalon, null, null, 0.01, 0.0, 0.0);
// last working pid for shooter with ball and surgical tubing
// ShooterLiftMotor = new Motor (ShooterLiftTalon, null, null, -1.2, -1,-0.14, 0.001);
// working better. hitting target faster
// ShooterLiftMotor = new Motor (ShooterLiftTalon, null, null, -4.8, -1,-0.14, 0.001);
ShooterLiftMotor = new Motor(ShooterLiftTalon, null, null, -4, -1.2, -0.2, 0.001);
// EXTENDER
ExtenderTalon = new CANTalon(RobotConstants.ExtenderTalonPort);
ExtenderPotentiometer = new AnalogPotentiometer(RobotConstants.ExtenderPotentiometerPort);
// ExtenderSolenoid = new Solenoid(1,RobotConstants.ExtenderSolenoidPortA);
// ExtenderSolenoid = new Solenoid(1,3);
ExtenderMotor = new Motor(ExtenderTalon, null, ExtenderPotentiometer, 15, 0, 0);
// ExtenderSolenoid = new Solenoid(1,1);
// CONTROL
LeftJoy = new Joystick(RobotConstants.rightJoyPort);
RightJoy = new Joystick(RobotConstants.leftJoyPort);
ScaleRightMotor = new Motor(ScaleRightTalon, ScaleRightEncoder, null, 0, 0, 0);
ScaleLeftMotor = new Motor(ScaleRightTalon, ScaleRightEncoder, null, 0, 0, 0);
}
