// 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
public double getRPM() {
if (Timer.getFPGATimestamp() > RPMUpdatePeriod + encoderMarkTime) {
double encoderCPS =
(enc.get() - encoderMarkCounts) / (Timer.getFPGATimestamp() - encoderMarkTime);
encoderRPM = (encoderCPS * 60) / 16;
encoderMarkCounts = enc.get();
encoderMarkTime = Timer.getFPGATimestamp();
}
return encoderRPM;
}
// lowers elevator partway for last pull-up at end
public void lowerElevatorPartway() {
if (climberEncoder.get() > lowerElevatorPartwayLimit) {
if (!getError(false) || climberEncoder.get() > errorUpperLimit) {
climberMotor.set(-elevatorSpeed);
} else {
if (!getUpperLimit()) {
climberMotor.set(elevatorSpeed);
}
}
} else {
climberMotor.set(0);
}
putData();
}
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 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
@Override
@SuppressWarnings({"nls", "boxing"})
protected void initialize(Class<?> type, Object oldInstance, Object newInstance, Encoder enc) {
super.initialize(type, oldInstance, newInstance, enc);
List list = (List) oldInstance;
Statement setterStm = new Statement(oldInstance, "setSize", new Object[] {list.getSize()});
enc.writeStatement(setterStm);
int count = list.getItemCount();
Expression getterExp = null;
for (int i = 0; i < count; i++) {
getterExp = new Expression(list, "getItem", new Object[] {i});
try {
// Calculate the old value of the property
Object oldVal = getterExp.getValue();
// Write the getter expression to the encoder
enc.writeExpression(getterExp);
// Get the target value that exists in the new environment
Object targetVal = enc.get(oldVal);
// Get the current property value in the new environment
Object newVal = null;
try {
newVal = new Expression(newInstance, "getItem", new Object[] {i}).getValue();
} catch (IndexOutOfBoundsException ex) {
// The newInstance has no elements, so current property
// value remains null
}
/*
* Make the target value and current property value equivalent
* in the new environment
*/
if (null == targetVal) {
if (null != newVal) {
// Set to null
setterStm = new Statement(oldInstance, "add", new Object[] {null});
enc.writeStatement(setterStm);
}
} else {
PersistenceDelegate pd = enc.getPersistenceDelegate(targetVal.getClass());
if (!pd.mutatesTo(targetVal, newVal)) {
setterStm = new Statement(oldInstance, "add", new Object[] {oldVal});
enc.writeStatement(setterStm);
}
}
} catch (Exception ex) {
enc.getExceptionListener().exceptionThrown(ex);
}
}
}
// raises the elevator, tries again if hooks don't catch
public void raiseElevator() {
if (!getUpperLimit()) {
if (!getError(true) || climberEncoder.get() < errorLowerLimit) {
climberMotor.set(elevatorSpeed);
} else {
if (!getLowerLimit()) {
climberMotor.set(-elevatorSpeed);
}
}
} else {
climberMotor.set(0);
}
putData();
}
public void teleopPeriodic() {
watchdog.feed(); // feed the watchdog
// Toggle drive mode
if (!driveToggle && left.getRawButton(2)) {
driveMode = (driveMode + 1) % 3;
driveToggle = true;
} else if (driveToggle && !left.getRawButton(2)) driveToggle = false;
// Print drive mode to DS & send values to Jaguars
switch (driveMode) {
case 0:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Tank ");
red.set(left.getY() + gamePad.getY());
black.set(-right.getY() - gamePad.getThrottle());
break;
case 1:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Arcade");
red.set(right.getX() - right.getY());
black.set(right.getX() + right.getY());
break;
case 2:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Kaj ");
red.set(left.getX() - right.getY());
black.set(left.getX() + right.getY());
break;
} /**/
dsLCD.println(
DriverStationLCD.Line.kUser3,
1,
"1" + photoreceptorL.get() + "2" + photoreceptorM.get() + "3" + photoreceptorR.get());
dsLCD.println(DriverStationLCD.Line.kUser4, 1, "Encoder in 4,5: " + leftEncoder.get() + " ");
dsLCD.println(
DriverStationLCD.Line.kUser5, 1, "Encoder in 6,7: " + rightEncoder.get() + " ");
dsLCD.updateLCD();
}
// puts data to smartdashboard
public void putData() {
SmartDashboard.putNumber("Level", level);
SmartDashboard.putNumber("Encoder", climberEncoder.get());
SmartDashboard.putBoolean("Upper Limit", getUpperLimit());
SmartDashboard.putBoolean("Lower Limit", getLowerLimit());
}
// whether the elevator has reached the top
public boolean getUpperLimit() {
return topClimberSwitch.get() || climberEncoder.get() > encoderMax;
}
// whether the elevator has reached the bottom
public boolean getLowerLimit() {
return bottomClimberSwitch.get() || climberEncoder.get() < encoderMin;
}
public double pidGet() {
return leftEncoder.get();
}
public double get() {
return enc.get();
}
