/** @param armPos the armPos to set */
public void setArmPos(int armPos) {
this.armPos = armPos;
try {
double p = 0;
switch (armPos) {
case 0:
// target = PIDConstants.armIntake;
target = preferences.getDouble("intake", 0);
p = 2;
break;
case 1:
// target = PIDConstants.armFeed;
target = preferences.getDouble("feed", 0);
p = 2.8;
break;
case 2:
// target = PIDConstants.armStow;
target = preferences.getDouble("stow", 0);
// SmartDashboard.putNumber("armCurrent", arm.getOutputCurrent());
p = 4;
break;
}
double change = arm.getPosition() - target;
// SmartDashboard.putNumber("armPosition", arm.getPosition());
// System.out.println(arm.getPosition());
// arm.setX(change * p*0.9);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
public void storeLoaderAngle(String sPosition) {
float volts = (float) leftAngleCh.getVoltage();
float voltsR = (float) rightAngleCh.getVoltage();
m_preferences.putFloat(sPosition, volts);
m_preferences.putFloat(sPosition + "R", voltsR);
System.out.println(sPosition + " = " + volts);
}
// Called just before this Command runs the first time
protected void initialize() {
pushTime = Preferences.getInstance().getDouble("PushDiskTime", 0.5);
pushSpeed = Preferences.getInstance().getDouble("PushDiskSpeed", 0.7);
returnTime = Preferences.getInstance().getDouble("ReturnTime", 0.7);
returnSpeed = Preferences.getInstance().getDouble("ReturnSpeed", 0.3);
step = 0;
}
public void goToPosition(String sPosition) {
m_sPosition = sPosition;
pidLeft.setSetpoint(m_preferences.getFloat(sPosition, 0));
pidRight.setSetpoint(m_preferences.getFloat(sPosition + "R", 0));
System.out.println(sPosition + " Setpoint = " + m_preferences.getFloat(sPosition, 0));
}
// basic arcadeDrive: y=forward/backward speed, x=left/right speed
public void arcadeDrive(double y, double x) {
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
robotDrive.arcadeDrive(y, x + 0.05);
SmartDashboard.putNumber("DriveTrainGyro", -gyro.getAngle()); // upside down
SmartDashboard.putNumber("RangefinderVoltage", ultraDist.getVoltage());
} // end arcadeDrive
// basic arcadeDrive: y=forward/backward speed, x=left/right speed
public void arcadeDrive(double y, double x) {
SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
robotDrive.arcadeDrive(y, x);
} // end arcadeDrive
@Override
public String getValue() {
if (Preferences.getInstance().containsKey(mKey)) {
return Preferences.getInstance().getString(mKey, mDefault);
}
sPropertyAdded = true;
Preferences.getInstance().putString(mKey, mDefault);
return mDefault;
}
public boolean alignStraight() {
double angleOut = gyro.getAngle() * Preferences.getInstance().getDouble("Gyro_kP", 0.0);
SmartDashboard.putNumber("gyroErr", gyro.getAngle());
SmartDashboard.putNumber("angleOut", angleOut);
robotDrive.arcadeDrive(0, angleOut);
if (Math.abs(gyro.getAngle()) < Preferences.getInstance().getDouble("AngleBuffer", 100)) {
arcadeDrive(0, 0);
return true;
}
return false;
}
public void tankDrive(double leftSpeed, double rightSpeed) {
Preferences p = Preferences.getInstance();
final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
// if (kReverseDirection) {
// robotDrive.tankDrive(rightSpeed, leftSpeed);
// return;
// }
robotDrive.tankDrive(leftSpeed, rightSpeed);
SmartDashboard.putNumber("gyroErr", gyro.getAngle());
SmartDashboard.putNumber("encoder", rightEncoder.get());
} // end tankDrive
public Intake() {
preferences = Preferences.getInstance();
leftGate =
new DoubleSolenoid(
ElectricalConstants.rightGateForward, ElectricalConstants.rightGateReverse);
rightGate = new Solenoid(ElectricalConstants.leftGate);
try {
arm = new CANJaguar(ElectricalConstants.armJag);
arm.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
arm.configNeutralMode(CANJaguar.NeutralMode.kBrake);
arm.configFaultTime(0.5);
arm.configPotentiometerTurns(1);
arm.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
arm.setPID(0, 0, 0);
arm.enableControl(0);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
rollers = new Victor(ElectricalConstants.rollerVic);
}
// starts encoders
public DriveTrain() {
leftEncoder.start();
rightEncoder.start();
gyro.reset();
Preferences p = Preferences.getInstance();
if (!p.containsKey("DriveTrainShootLimitVoltage")) {
p.putDouble("DriveTrainShootLimitVoltage", kDefaultShootLimitVoltage);
}
if (!p.containsKey("DriveTrainQuickTurnP")) {
p.putDouble("DriveTrainQuickTurnP", kDefaultQuickTurnProportion);
}
if (!p.containsKey("DriveTrainQuickTurnDeadband")) {
p.putDouble("DriveTrainQuickTurnDeadband", kDefaultQuickTurnDeadband);
}
if (!p.containsKey("DriveTrainReverseDirection")) {
p.putBoolean("DriveTrainReverseDirection", kDefaultReverseDirection);
}
} // end constructor
public void enable() {
rightEncoder.reset();
leftEncoder.reset();
rightEncoder.start();
leftEncoder.start();
gyro.reset();
final boolean kReverseDirection =
Preferences.getInstance().getBoolean("DriveTrainReverseDirection", false);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
} // end enable
// Initialize your subsystem here
public LoaderClimberOld2PID() {
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// super("LoaderClimberOld2PID", 1.5, 0.1, 0.0);
// setAbsoluteTolerance(0.02);
// getPIDController().setContinuous(false);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
m_preferences = Preferences.getInstance();
if (leftJag != null) { // for practice robot
// Fix for motors going in opposite directios
PIDOutput rightOutput =
new PIDOutput() {
public void pidWrite(double output) {
rightJag.pidWrite(-1 * output);
}
};
pidLeft = new PIDController(P, I, D, leftAngleCh, leftJag); // PID Output Interface
pidRight = new PIDController(P, I, D, rightAngleCh, rightOutput); // PID Output Interface*/
LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidLeft);
LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidRight);
pidLeft.setSetpoint(m_preferences.getFloat("Retract", 2.4f));
pidRight.setSetpoint(m_preferences.getFloat("RetractR", 2.4f));
pidLeft.setInputRange(0, 4);
pidRight.setInputRange(0, 4);
pidLeft.setPercentTolerance(PID_TOL);
pidRight.setPercentTolerance(PID_TOL);
pidLeft.enable(); // - Enables the PID controller.
pidRight.enable(); // - Enables the PID controller.
// disable();
}
}
// aligns robot for shooting
public void alignToShoot(double left, double right) {
Preferences p = Preferences.getInstance();
final double kShootLimitVoltage = p.getDouble("DriveTrainShootLimitVoltage", 0.0);
double maxVoltage = ultraDist.getVoltage();
for (int i = 0; i < 10; i++) {
double voltage = ultraDist.getVoltage();
if (voltage > maxVoltage) {
maxVoltage = voltage;
}
}
SmartDashboard.putNumber(
"RangefinderVoltage", maxVoltage); // want to see on competition dashboard!!
System.out.println(
"RangefinderVoltage: " + maxVoltage + " kShootLimitVoltage: " + kShootLimitVoltage);
if (maxVoltage <= kShootLimitVoltage) {
hasBeenAchieved = true;
}
if (left <= 0.0 && hasBeenAchieved) {
left = 0.0;
}
arcadeDrive(left, right);
} // end alignToShoot
public boolean quickTurn(double setpoint) {
// Proportional quickturn algorithm
Preferences p = Preferences.getInstance();
final double kP = p.getDouble("DriveTrainQuickTurnP", 0.0);
final double kDB = p.getDouble("DriveTrainQuickTurnDeadband", 0.0);
double angle = -getGyro(); // -driveTrain.getGyro() b/c the gyro is upsidedown
double error = setpoint - angle;
double twist = error * kP;
double magnitude = Math.abs(twist);
if (magnitude < kDB) {
twist = kDB * (twist > 0.0 ? 1.0 : -1.0);
} else if (magnitude > 1.0) {
twist = (twist > 0.0 ? 1.0 : -1.0);
}
SmartDashboard.putNumber("DriveTrainGyro", angle);
leftMotor.set(-twist);
rightMotor.set(twist);
// Done once we pass the setpoint
return Math.abs(angle) >= Math.abs(setpoint);
} // end quickTurn
public boolean driveStraight(double dist) {
SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
Preferences p = Preferences.getInstance();
L_encoderVal =
leftEncoder.get()
/ p.getDouble(
"LeftEncoderRatio",
0.0); // converts encoder value to inches; encoder ratio should be about 58.76
R_encoderVal =
rightEncoder.get()
/ p.getDouble("RightEncoderRatio", 0.0); // converts encoder value to inches
encoderVal = (R_encoderVal); // only one encoder
encoderErr = dist - encoderVal;
distOut =
encoderErr * p.getDouble("Encoder_kP", 0.0)
+ (Math.abs(encoderErr) / encoderErr)
* p.getDouble(
"OutputMin",
0.0); // Encoder kP = 0.5; abs(x)/x returns sign of x; 0.2 is the min. magnitude
gyroErr = gyro.getAngle(); // Setpoint is always 0
double angleOut = gyroErr * p.getDouble("Gyro_kP", 0.0) + .3; // * distOut / Math.abs(distOut);
SmartDashboard.putNumber("encoderErr", encoderErr);
SmartDashboard.putNumber("distOut", distOut);
SmartDashboard.putNumber("gyroErr", gyro.getAngle());
SmartDashboard.putNumber("angleOut", angleOut);
if (Math.abs(encoderErr) < p.getDouble("DistBuffer", 0.0)
&& Math.abs(gyroErr) < p.getDouble("AngleBuffer", 0.0)) {
// leftEncoder.reset();
// rightEncoder.reset();
// gyro.reset();
// arcadeDrive(0, 0);
return true; // returns true when robot gets to its goal
} else {
arcadeDrive(
-distOut,
angleOut); // Gyro kP = 1/18.0; Arcade Drive uses reversed rotate values (neg. goes Left /
// pos. goes Right)
return false; // returns false if robot still hasn't reached its goal yet
}
} // end driveStraight
public static double getAutoRotateDefaultTaretDegrees() {
return Preferences.getInstance().getDouble("AutoRotateDefaultTargetDegrees", 0.0);
}
public static double getAutoRotateOnTargetToleranceDegrees() {
return Preferences.getInstance().getDouble("AutoRotateOnTargetToleranceDegrees", 2.0);
}
// starts encoders
public DriveTrain() {
leftEncoder.start();
rightEncoder.start();
gyro.reset();
Preferences p = Preferences.getInstance();
if (!p.containsKey("DriveTrainShootLimitVoltage")) {
p.putDouble("DriveTrainShootLimitVoltage", kDefaultShootLimitVoltage);
}
if (!p.containsKey("DriveTrainQuickTurnP")) {
p.putDouble("DriveTrainQuickTurnP", kDefaultQuickTurnProportion);
}
if (!p.containsKey("DriveTrainQuickTurnDeadband")) {
p.putDouble("DriveTrainQuickTurnDeadband", kDefaultQuickTurnDeadband);
}
if (!p.containsKey("DriveTrainReverseDirection")) {
p.putBoolean("DriveTrainReverseDirection", kDefaultReverseDirection);
}
if (!p.containsKey("LeftEncoderRatio")) {
p.putDouble("LeftEncoderRatio", 4.875);
}
if (!p.containsKey("RightEncoderRatio")) {
p.putDouble("RightEncoderRatio", 4.875);
}
if (!p.containsKey("Encoder_kP")) {
p.putDouble("Encoder_kP", 0.05);
}
if (!p.containsKey("OutputMin")) {
p.putDouble("OutputMin", .2);
}
if (!p.containsKey("DistBuffer")) {
p.putDouble("DistBuffer", 1);
}
if (!p.containsKey("Gyro_kP")) {
p.putDouble("Gyro_kP", 1 / 18);
}
if (!p.containsKey("AngleBuffer")) {
p.putDouble("AngleBuffer", 4.5);
}
if (!p.containsKey("AutoDist_0")) {
p.putDouble("AutoDist_0", 0.0);
}
if (!p.containsKey("AutoDist_1")) {
p.putDouble("AutoDist_1", 0.0);
}
if (!p.containsKey("AutoDist_2")) {
p.putDouble("AutoDist_2", 0.0);
}
if (!p.containsKey("AutoDist_3")) {
p.putDouble("AutoDist_3", 0.0);
}
if (!p.containsKey("ShiftWhenTurningThreshold")) {
p.putDouble("ShiftWhenTurningThreshold", 0.5);
}
if (!p.containsKey("ShiftWhenTurningDelay")) {
p.putDouble("ShiftWhenTurningDelay", 1.5);
}
} // end constructor
// int errors = 0;
// protected void usePIDOutput(double output) {
//
// double outputNeg = output * -1;
// double slow = output * .815;
//// System.out.println("LoaderClimberOld2PID: usePIDOutput, outputNeg=" + outputNeg + "
// slow=" + slow + " errors:" + errors);
// try {
// leftJag.setX(slow);
// rightJag.setX(outputNeg);
// } catch (CANTimeoutException ex) {
//// ex.printStackTrace();
// errors = errors + 1;
// }
// }
public void savePreferences() {
m_preferences.save();
System.out.println("Preferences saved");
}
