/** @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 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
// 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 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
