public void disable() {
setSetpoint(0.0);
// pidRight.disable();
// pidLeft.disable();
leftMotor.set(0.0);
rightMotor.set(0.0);
rightEncoder.reset();
leftEncoder.reset();
} // end disable
public void setIndexMotor(int direction) {
wheelDirection = direction;
if (direction == 1) {
indexMotor.set(1);
} else if (direction == -1) {
indexMotor.set(-1);
} else {
indexMotor.disable();
}
}
private void moveRightMotor(double speed) {
// rightMotor.set(speed);
// System.out.println("limit = " + bottomLimit.getSmartDashboardType());
if (speed < 0.0) {
rightMotor.set(speed);
} else if (speed > 0.0 && !bottomLimit.get()) {
rightMotor.set(0.0);
} else {
rightMotor.set(speed);
}
}
/** @param speed */
private void moveLeftMotor(double speed) {
// leftMotor.set(speed*-1.0);
if (speed < 0.0) {
leftMotor.set(speed * -1.0);
} else if (speed > 0.0 && !bottomLimit.get()) {
leftMotor.set(0.0);
} else {
leftMotor.set(speed * -1.0);
}
}
// 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();
}
// lowers the elevator, tries again if hooks don't catch
public void lowerElevator() {
if (!getLowerLimit()) {
if (!getError(false) || climberEncoder.get() > errorUpperLimit) {
climberMotor.set(-elevatorSpeed);
} else {
if (!getUpperLimit()) {
climberMotor.set(elevatorSpeed);
}
}
} else {
climberMotor.set(0);
}
putData();
}
// sets climber speed to given value
public void manualControl(double d) {
if (topClimberSwitch.get() && d > 0 || bottomClimberSwitch.get() && d < 0) {
climberMotor.set(0);
}
if (topClimberSwitch.get() && d < 0) {
climberMotor.set(d);
}
if (bottomClimberSwitch.get() && d > 0) {
climberMotor.set(d);
}
if (!topClimberSwitch.get() && !bottomClimberSwitch.get()) {
climberMotor.set(d);
}
putData();
}
// moves elevator to starting position and sets encoder
public void homingSequence() {
if (homeUp) {
while (!getUpperLimit()) {
climberMotor.set(homingSpeed);
}
while (getUpperLimit()) {
climberMotor.set(-homingReverseSpeed);
}
climberEncoder.reset();
} else {
while (!getLowerLimit()) {
climberMotor.set(-homingSpeed);
}
while (getLowerLimit()) {
climberMotor.set(homingReverseSpeed);
}
climberEncoder.reset();
}
}
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 void set(double d, byte b) {
front.set(d, b);
rear.set(d, b);
}
public void tankDrive(double leftSpeed, double rightSpeed) {
leftMotor.set(leftSpeed);
rightMotor.set(rightSpeed);
SmartDashboard.putNumber("DriveTrainGyro", -gyro.getAngle()); // upside down
} // end tankDrive
/** Shift the gear down to low gear. */
public void shiftToLowGear() {
motor.set(-Constants.GEAR_SHIFTER_MOTOR_POWER);
updateDashboard();
}
public void pidWrite(double output) {
leftMotor.set(output);
}
public void pidWrite(double output) {
rightMotor.set(output);
}
public void stop() {
winchMotor.set(0);
}
public void set(double d) {
front.set(d);
rear.set(d);
}
/** Stop the PID Loop and kill the arm motor */
public void stop() {
disable();
arm.set(0);
}
/** Stop shifting gears. */
public void stop() {
motor.set(0);
updateDashboard();
}
/**
* Reversed tankDrive.
*
* @param left Right motor value
* @param right Left motor value
*/
public void reverseTankDrive(double left, double right) {
leftMotor.set(right * -1.0);
rightMotor.set(left);
}
/**
* Tank drive for main drivetrain.
*
* @param left Left motor value
* @param right Right motor value
*/
public void tankDrive(double left, double right) {
leftMotor.set(left * -1.0);
rightMotor.set(right);
}
public void up() {
winchMotor.set(1);
}
/** Set the talon value (Between -1.0 and 1.0) for the wheel */
public void setSpeed(double speed) {
motor_controller.set(speed);
}
/** Stop the arm motor dead */
public void stop() {
arm.set(0);
}
public void setIntakeSpeed(double speed) {
intake.set(speed);
}
public void spinCIMOutward() {
leftCIM.set(-1);
rightCIM.set(1);
}
public void down() {
winchMotor.set(-1);
}
/**
* Set the speed of the arm motor
*
* @param speed The speed to set the motor to
*/
public void setSpeed(double speed) {
arm.set(speed);
}
public void setFeederIntakeSpeed(double value) {
feederIntake.set(-value);
}
public void teleopPeriodic() {
/*SmartDashboard.getNumber("Accel x: ", accel.getX());
SmartDashboard.getNumber("Accel Y: ", accel.getY());
SmartDashboard.getNumber("Accel Z: ", accel.getZ());*/
if (controller.isEnabled()) {
if (((Timer.getFPGATimestamp() - timerStart) > timeToCancel) || controller.onTarget()) {
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
}
if (j1.getRawButton(2)) {
enc.reset();
controller.setSetpoint(120); // destination 24 inches -> NO!! Trying to figure out this value
timerStart = Timer.getFPGATimestamp();
timeToCancel = 10; // timeout after 10 seconds
controller.enable();
} else if (j1.getRawButton(
1)) { // this button stops the robot if the button 2 command goes crazy
controller.disable();
timerStart = -1;
timeToCancel = -1;
} else { // if time out or distance, end
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
if (!controller.isEnabled()) {
rd.arcadeDrive(-j1.getY(), -j1.getX()); // normal arcade drive
}
if (j2.getRawAxis(2) != 0) { // set shooter values to the left trigger value
shooter.set(-j2.getRawAxis(2));
shooter2.set(-j2.getRawAxis(2));
SmartDashboard.putNumber("Shooter: ", shooter.get());
} else { // stop shooter
shooter.set(0);
shooter2.set(0);
}
/*if(j2.getRawButton(8)){ //runs intake, waits, runs shooter
shooter.set(-1);
intake.set(.5);
Timer.delay(.50);
intake.set(0);
Timer.delay(1.5);
intake.set(-1);
Timer.delay(1);
shooter.set(0);
intake.set(0);
}*/
// Need to mess around with sensitivity of light sensor
if (j2.getRawAxis(3) != 0 && !light.get()) { // run intake at speed of right trigger
intake.set(-1 * j2.getRawAxis(3));
} else if (j2.getRawButton(5) && !light.get()) { // run intake into robot
intake.set(-1);
} else if (j2.getRawButton(6)) { // run intake out of robot
intake.set(1);
} else { // stop intake
intake.set(0);
}
if (j2.getRawButton(2)) { // lift intake
inup.set(DoubleSolenoid.Value.kForward);
inup2.set(DoubleSolenoid.Value.kForward);
} else if (j2.getRawButton(3)) { // drop intake
inup.set(DoubleSolenoid.Value.kReverse);
inup2.set(DoubleSolenoid.Value.kReverse);
} else { // solenoids off
inup.set(DoubleSolenoid.Value.kOff);
inup2.set(DoubleSolenoid.Value.kOff);
}
// reading values
SmartDashboard.putNumber("Encoder Dist: ", enc.getDistance()); // Distance is in inches
SmartDashboard.putNumber("Encoder: ", enc.get());
SmartDashboard.putNumber("Encoder Rate: ", enc.getRate());
SmartDashboard.putNumber("Gyro: ", gyro.getAngle());
SmartDashboard.putNumber("Encoder Raw", enc.getRaw());
SmartDashboard.putNumber("Controller: ", controller.getSetpoint());
SmartDashboard.putBoolean("Light Sensor: ", light.get());
}
public void setBottomFlywheelSpeed(double speed) {
bottomFlywheel.set(speed);
}
