@Override
public double getTrigger(Trigger trigger) {
double triggerValue = 0.0;
// The trigger map changes based on the joystick mode
switch (getMode()) {
case D:
switch (trigger) {
// In D mode, the triggers are buttons - return 0 or 1.
case LEFT:
return joystick.getRawButton(7) ? 1.0 : 0.0;
case RIGHT:
return joystick.getRawButton(8) ? 1.0 : 0.0;
}
break;
case X:
switch (trigger) {
case LEFT:
triggerValue = joystick.getRawAxis(2);
break;
case RIGHT:
triggerValue = joystick.getRawAxis(3);
break;
}
break;
}
// Round the trigger value to 2 decimal places
return Math.round(triggerValue * 100.0) / 100.0;
}
public static void teleopPeriodic() {
/*
double newArmPos = gamepad.getRawAxis(1);
if(Math.abs(newArmPos) <= ARM_DEADZONE) {
newArmPos = 0.0;
}
double newMotorPos = (newArmPos * ARM_SPEED_MULTIPLIER) + frontArmMotor.getPosition();
//double newMotorPos = (newArmPos * ARM_SPEED_MULTIPLIER);
frontArmMotor.set(newMotorPos);
System.out.println("input = " + newArmPos + " target pos = " + newMotorPos + " enc pos = " + frontArmMotor.getPosition());
*/
// PercentVbus test ONLY!!
double armSpeed = gamepad.getRawAxis(1);
if (Math.abs(armSpeed) < ARM_DEADZONE) {
armSpeed = 0.0f;
}
armSpeed *= ARM_MULTIPLIER;
double pos = frontArmMotor.getPosition();
if (((pos > SOFT_ENCODER_LIMIT_1) && armSpeed < 0.0)
|| ((pos < SOFT_ENCODER_LIMIT_2) && armSpeed > 0.0)) armSpeed = 0.0;
frontArmMotor.set(armSpeed);
System.out.println("armSpeed = " + armSpeed + " enc pos = " + frontArmMotor.getPosition());
// check for roller motion (right gamepad joystick)
double rollerSpeed = gamepad.getRawAxis(3);
if (Math.abs(rollerSpeed) < ROLLER_DEADZONE) {
rollerSpeed = 0.0f;
}
rollerSpeed *= ARM_ROLLER_MULTIPLIER;
frontArmRollerMotor.set(rollerSpeed);
}
/**
* Returns the axis value of the Right DriverStick
*
* @return the value of the right axis
*/
public double getDriveRightAxis() {
if (drivestick.getRawAxis(3) < 0) { // FALCON CLAW - use electronic braking
return ((((-RobotMap.mod + 1) * drivestick.getRawAxis(3)) + 1) * drivestick.getRawAxis(5));
} else {
return drivestick.getRawAxis(5);
}
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
// getWatchdog().setEnabled(true);
while (isOperatorControl() && isEnabled()) {
mainDrive.mecanumDrive_Cartesian(
driveStick.getRawAxis(1), driveStick.getRawAxis(0), driveStick.getRawAxis(5), 0);
// mainDrive.mecanumDrive_Cartesian(driveStick.getX(), driveStick.getY(), otherstick.getZ(),
// 0);
/**
* public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
* if(!kMecanumCartesian_Reported) {
* UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
* tInstances.kRobotDrive_MecanumCartesian); kMecanumCartesian_Reported = true; } double xIn =
* x; double yIn = y; // Negate y for the joystick. yIn = -yIn; // Compensate for gyro angle.
* double rotated[] = rotateVector(xIn, yIn, gyroAngle); xIn = rotated[0]; yIn = rotated[1];
*
* <p>double wheelSpeeds[] = new double[kMaxNumberOfMotors];
* wheelSpeeds[MotorType.kFrontLeft_val] = xIn + yIn + rotation;
* wheelSpeeds[MotorType.kFrontRight_val] = -xIn + yIn - rotation;
* wheelSpeeds[MotorType.kRearLeft_val] = -xIn + yIn + rotation;
* wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation;
*
* <p>normalize(wheelSpeeds); m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] *
* m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup);
* m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] *
* m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup);
* m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] *
* m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup);
* m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] *
* m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup);
*
* <p>if (m_syncGroup != 0) { CANJaguar.updateSyncGroup(m_syncGroup); }
*
* <p>if (m_safetyHelper != null) m_safetyHelper.feed(); }
*/
if (otherstick.getRawButton(4)) {
mainDrive.mecanumDrive_Polar(0, 0, -.25);
}
if (otherstick.getRawButton(5)) {
mainDrive.mecanumDrive_Polar(0, 0, .25);
}
if (otherstick.getRawButton(6)) {
mainDrive.mecanumDrive_Polar(0, .25, 0);
}
if (otherstick.getRawButton(11)) {
mainDrive.mecanumDrive_Polar(0, -.25, 0);
}
if (otherstick.getRawButton(1)) {
mainDrive.mecanumDrive_Polar(.25, 0, 0);
}
if (driveStick.getRawButton(1)) {
mainDrive.mecanumDrive_Polar(-.25, 0, 0);
}
}
}
// return a delta coordinate to go to
// to calculate the actual coordinate add this methods return
// to a logged coordinate
public synchronized PolarVector calculateCoordinate() {
double leftDistance =
m_distancePerInterval * limit(leftJoystick.getRawAxis(leftAxis), leftThreshold);
double rightDistance =
m_distancePerInterval * limit(rightJoystick.getRawAxis(rightAxis), rightThreshold);
if (m_leftInvert) leftDistance = -leftDistance;
if (m_rightInvert) rightDistance = -rightDistance;
double angle = (rightDistance - leftDistance) / RobotProperties.wheelContactWidth;
double distance = (rightDistance + leftDistance) / 2;
return new PolarVector(distance, angle);
}
@Override
public boolean get() {
switch (joyDir) {
case UP:
if (stick.getRawAxis(axis) <= -.5) {
return true;
}
break;
case DOWN:
if (stick.getRawAxis(axis) >= .5) {
return true;
}
break;
}
return false;
}
public double GetLeftTrigger() {
double y;
y = joyStick.getRawAxis(3);
if (Math.abs(y) < deadZone) y = 0.0;
return y;
}
public double GetRightTrigger() {
double x;
x = joyStick.getRawAxis(2);
if (Math.abs(x) < deadZone) x = 0.0;
return x;
}
public double GetRightY() {
double y;
y = joyStick.getRawAxis(5);
if (Math.abs(y) < deadZone) y = 0.0;
return y;
}
public double GetLeftX() {
double x;
x = joyStick.getRawAxis(0);
if (Math.abs(x) < deadZone) x = 0.0;
return x;
}
public double getThrottle() {
// Inverted Throttle
// if (ps4Controller.getRawAxis(LEFT_TRIGGER) > 0) {
// throttleAverager.addValue(-ps4Controller.getRawAxis(LEFT_STICK_Y));
// } else {
// Regular Throttle
throttleAverager.addValue(ps4Controller.getRawAxis(LEFT_STICK_Y));
// }
return throttleAverager.getAverage();
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
SmartDashboard.putBoolean("squared drive: ", squaredDrive.getState());
if (squaredDrive.getState()) {
// the last parameter is to say "square the joystick values"
// as usual, look inthe wpilibj source if you want to see how that works
drive.tankDrive(leftJoystick, rightJoystick, true);
} else {
drive.tankDrive(leftJoystick.getY(), rightJoystick.getY());
}
if (Math.abs(leftJoystick.getY()) < 0.1 && Math.abs(rightJoystick.getY()) < 0.1) {
drive.tankDrive(gamepad.getRawAxis(2), gamepad.getRawAxis(4));
}
SmartDashboard.putNumber("leftJoy: ", leftJoystick.getY());
SmartDashboard.putNumber("rightJoy: ", rightJoystick.getY());
SmartDashboard.putNumber("gamepad left Y: ", gamepad.getRawAxis(2));
SmartDashboard.putNumber("gamepad right y: ", gamepad.getRawAxis(4));
}
/** This function is called once each time the robot enters operator control. */
public void operatorControl() {
drivetrain.setSafetyEnabled(true);
compressor.start();
while (isOperatorControl() && isEnabled()) {
// The throttle goes from 1 to -1, bottom to top. This
// scales it to go 0 to 1, bottom to top.
double throttle = (-leftStick.getRawAxis(3) + 1) / 2;
double drive = deadband(leftStick.getY()) * throttle;
double steer = deadband(-rightStick.getX()) * throttle;
drivetrain.arcadeDrive(drive, steer);
boolean shouldSpinWheels = leftStick.getRawButton(1);
boolean shouldFire = shouldSpinWheels && rightStick.getRawButton(1);
shooterWheels.set(shouldSpinWheels ? -1 : 0);
shooter.set(shouldFire);
Timer.delay(0.005); // wait for a motor update time
}
}
public double getRightX() {
return stick.getRawAxis(4);
}
public double getRawAxis(int axis) {
double leftAxis = left.getRawAxis(axis);
double rightAxis = right.getRawAxis(axis);
return nonZero(leftAxis, rightAxis);
}
public double getLeftX() {
return stick.getRawAxis(1);
}
public double getRightY() {
return stick.getRawAxis(5);
}
@Override
public double get() {
return TurtleMaths.reverseBool(isReversed) * masterJoystick.getRawAxis(axisNum);
}
public double getLeftY() {
return stick.getRawAxis(2);
}
/**
* Corresponds to HORIZONTAL input on the RIGHT joystick
*
* @return The X coordinate of the RIGHT joystick (-1 is LEFT, 1 is RIGHT)
*/
public double getRightX() {
return deaden(joy.getRawAxis(AXIS_RIGHT_X));
}
/**
* Corresponds to VERTICAL input on the LEFT joystick.
*
* @return The Y coordinate of the LEFT joystick (-1 is UP, 1 is DOWN)
*/
public double getLeftY() {
return deaden(joy.getRawAxis(AXIS_LEFT_Y));
}
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 static double getRightSpeed() {
return xbox.getRawAxis(5);
}
/**
* Returns the axis value of the Right Auxiliary Stick
*
* @return the value of the right stick
*/
public double getAuxRightStick() {
return auxstick.getRawAxis(5); // this is supposed to be the right
}
/**
* Returns the axis value of the Left AuxiliaryStick
*
* @return the value of the left stick
*/
public double getAuxLeftStick() {
return auxstick.getRawAxis(2); // this is supposed to be the left
}
public double getTriggers() {
return deaden(joy.getRawAxis(LEFT_AXIS_TRIGGERS) - joy.getRawAxis(RIGHT_AXIS_TRIGGERS));
}
public static double getLeftSpeed() {
return xbox.getRawAxis(1);
}
public boolean getDPadLeft() {
return joy.getRawAxis(AXIS_DPAD_HORIZONTAL) < -0.5;
}
public static double getTurnVal() {
return xbox.getRawAxis(4);
}
public boolean getDPadRight() {
return joy.getRawAxis(AXIS_DPAD_HORIZONTAL) > 0.5;
}