@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);
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
Joystick joystickDrive = Robot.oi.getJoystickDrive();
this.joystickX = joystickDrive.getAxis(Joystick.AxisType.kX) * Robot.driveTrain.turnMultiplier;
this.joystickY = joystickDrive.getAxis(Joystick.AxisType.kY) * -1;
VisionState vs = VisionState.getInstance();
if (vs == null || !vs.wantsControl()) {
// endAutoTurn is harmless when not needed but required
// if the driver changes her mind after initiating auto-targeting..
Robot.driveTrain.endAutoTurn();
this.scaledThrottle = scaleThrottle(joystickDrive.getAxis(Joystick.AxisType.kThrottle));
if ((Math.abs(this.joystickX) < 0.075) && (Math.abs(this.joystickY) < 0.075)) {
Robot.driveTrain.stop();
} else {
Robot.driveTrain.arcadeDrive(joystickY * scaledThrottle, joystickX * scaledThrottle);
}
} else {
if (vs.DriveLockedOnTarget) {
// wait for launcher to shoot and exit auto mode or toggle AutoAim
Robot.driveTrain.stop(); // needed to keep driveTrain alive
} else {
if (!Robot.driveTrain.isAutoTurning()) {
double h = Robot.driveTrain.getCurrentHeading();
double target = vs.getTargetHeading(h);
Robot.driveTrain.startAutoTurn(target);
} else if (Robot.driveTrain.isAutoTurnFinished()) {
Robot.driveTrain.endAutoTurn();
vs.DriveLockedOnTarget = true;
} // else allow auto-turn to continue
}
}
}
/**
* 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);
}
}
public double getRightY() {
// set threshold value to 0.09
if (Math.abs(rightJoy.getY()) <= 0.09) {
return 0;
}
SmartDashboard.putNumber("SPEED RIGHT TRAIN", (-1) * rightJoy.getY());
return (-1) * rightJoy.getY();
}
public void takeJoystickInputs(Joystick leftJoystick, Joystick rightJoystick) {
// robotDrive.tankDrive(leftJoystick, rightJoystick);
SmartDashboard.putNumber("Sensitivity", 1 - leftJoystick.getAxis(AxisType.kZ));
sensitivity = 1 - leftJoystick.getAxis(AxisType.kZ);
robotDrive.tankDrive(
leftJoystick.getAxis(AxisType.kY) * sensitivity,
rightJoystick.getAxis(AxisType.kY) * sensitivity);
}
/** Gyro sensitivity is set and mecanum drive is used with the gyro angle as an input. */
public void operatorControl() {
gyro.setSensitivity(
voltsPerDegreePerSecond); // calibrate gyro to have the value equal to degrees
while (isOperatorControl() && isEnabled()) {
myRobot.mecanumDrive_Cartesian(
joystick.getX(), joystick.getY(), joystick.getZ(), gyro.getAngle());
Timer.delay(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
public double getLeftY() {
// set threshold value to 0.09
if (Math.abs(leftJoy.getY()) <= 0.09) {
return 0;
} else {
SmartDashboard.putNumber("SPEED LEFT TRAIN", (-1) * leftJoy.getY());
return (-1) * leftJoy.getY();
}
}
public void logData(
DriverStation ds,
Joystick left,
Joystick right,
Joystick co,
DriveTrain dt,
SuperStructure ss) {
System.out.println(
"Data: B:" + ds.getBatteryVoltage() + " L:" + left.getY() + " R:" + right.getY());
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
double x = joy1.getX();
double y = -joy1.getY();
double r = joy1.getZ();
if (Math.abs(x) < .05) x = 0;
if (Math.abs(y) < .05) y = 0;
if (Math.abs(r) < .05) r = 0;
drive(x / 2, y / 2, r / 2);
shooter1.set(joy2.getY());
shooter2.set(joy2.getY());
}
/** 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);
}
public void updateSmartDashboard() {
if (joystick_left.getRawButton(2) && dev_ < 100) {
dev_++;
} else if (joystick_left.getRawButton(3) && dev_ > 0) {
dev_--;
}
SmartDashboard.putDouble("X", joystick_left.getX());
SmartDashboard.putDouble("Y", joystick_left.getY());
// SmartDashboard.putDouble("Optical Sensor", Optical_Sensor.getVoltage());
SmartDashboard.putDouble("Battery Voltage", DriverStation.getInstance().getBatteryVoltage());
SmartDashboard.putInt("dev_", dev_);
SmartDashboard.putDouble(
"Elevator Sensor Value (.getAverageVoltage())",
this.elevator.ana_chana.getAverageVoltage());
String state = "ROBOT STATE INFORMATION:";
if (this.elevator.state == 0) {
state = state + "\nElevator Stopped.";
}
if (this.elevator.state == 1) {
state = state + "\nElevator Moving.";
}
if (this.elevator.state == 2) {
state = state + "\nElevator E-Stopped.";
}
// state = state+"\nShooter Power Level (Raw):
// "+this.shooter.calculateShooterSpeed();
// state = state+"\nShooter Power Level (Setting): "+this.shooter.shooterSpeedState;
// if (this.integ.globalState==1){
// state = state+"\nMoving Shooter Up To Shoot.";
// }if (this.integ.globalState==2){
// state = state+"\nShooting!";
// }if (this.integ.globalState2LOCK){
// state = state+"\nREALLY SHOOTING!.";
// }
state = state + "\nNumber Of Discs: " + this.elevator.numDiscs;
state =
state
+ "\n Louis + Bobni Are Amazing (Hiding This Message Is ILLEGAL! IT WILL STOP THE ROBOT!)";
SmartDashboard.putString("State", state);
SmartDashboard.putBoolean("Intake", this.intake.toggle.get());
}
/**
* Determines which height button is pressed. All (7 logically because side buttons are wired
* together) buttons are wired by means of resistors to one analog input. Depending on the button
* that is pressed, a different voltage is read by the analog input. Each resistor reduces the
* voltage by about 1/7 the maximum voltage.
*
* @return An integer value representing the distance button that was pressed. If a Joystick
* button is being used, that will returned. Otherwise, the button will be returned from the
* voltage (if it returns 0, no button is pressed).
*/
public int getDistanceButton() {
if (shooterStick.getRawButton(9)) {
distanceButton = DISTANCE_BUTTON_STOP;
} else if (shooterStick.getRawButton(10)) {
distanceButton = DISTANCE_BUTTON_FENDER;
} else if (shooterStick.getRawButton(11)) {
distanceButton = DISTANCE_BUTTON_FAR;
}
int preValue = (int) ((getRawAnalogVoltage() / (getMaxVoltage() / 8)) + 0.5);
// If no buttons are pressed, it does not update the distance.
if (preValue != 0) {
distanceButton = preValue;
}
return distanceButton;
}
@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 GetLeftX() {
double x;
x = joyStick.getRawAxis(0);
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;
}
private JoystickMode getMode() {
if (joystick.getButtonCount() > 10) {
return JoystickMode.D;
}
return JoystickMode.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));
}
public void tankDrive(Joystick left, Joystick right) {
if (left.getRawButton(1)) {
if (turbo == false) {
startTime = Timer.getFPGATimestamp();
turbo = true;
} else if (Timer.getFPGATimestamp() - startTime < 0.25) {
drive.tankDrive(
left.getY() / (2 - 4 * (Timer.getFPGATimestamp() - startTime)),
right.getY() / (2 - 4 * (Timer.getFPGATimestamp() - startTime)));
// System.out.println("start: " + startTime + " time: " + Timer.getFPGATimestamp() + "
// scale: " + (2 - 4*(startTime - Timer.getFPGATimestamp())));
} else {
drive.tankDrive(left, right);
}
} else {
turbo = false;
drive.tankDrive(left.getY() / 2, right.getY() / 2);
}
}
/** 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[] processDriveJoystick() {
double xValue;
double yValue;
if (((driveJoy.getX() < 0.10) && (driveJoy.getX() > 0))
|| ((driveJoy.getX() > -0.10) && (driveJoy.getX() < 0))) { // Check for dead zones
xValue = 0;
} else {
xValue = driveJoy.getX(); // No dead zone proceded normally
}
if (((driveJoy.getY() < 0.10) && (driveJoy.getY() > 0))
|| ((driveJoy.getY() > -0.10) && (driveJoy.getY() < 0))) { // Check for dead zones
yValue = 0;
} else {
yValue = driveJoy.getY(); // No dead zone proceded normally
}
double values[] = new double[1];
values[0] = yValue;
values[1] = xValue;
return values;
}
public void teleopPeriodic() {
armpid.setPID(
SmartDashboard.getNumber("kP"),
SmartDashboard.getNumber("kI"),
SmartDashboard.getNumber("kD"));
if (joy1.getRawButton(1)) {
armpid.enable();
} else {
armpid.disable();
arm.set(0);
}
SmartDashboard.putBoolean("Checkbox 2", limit.get());
System.out.println("limit " + limit.get());
}
/** @author Alex */
public class testInput {
Joystick ds1 = new Joystick(1);
Joystick ds2 = new Joystick(2);
Jaguar leftMotor = new Jaguar(1);
Jaguar rightMotor = new Jaguar(2);
boolean x = ds1.getRawButton(3);
public void main(String[] args) {
for (int i = 0; i < 50000; i++) {
if (x == true) {
System.out.println("You are pressing X");
}
}
}
}
public void teleopPeriodic() {
if (rightStick.getRawButton(1)) {
switch (state) {
case CLAMPED:
state = roboState.LIFTED;
updateState();
break;
case LIFTED:
state = roboState.OPEN;
updateState();
break;
case OPEN:
state = roboState.CLAMPED;
updateState();
break;
default:
state = roboState.OPEN;
updateState();
break;
}
}
}
public static boolean getRightButton() {
return flight.getRawButton(3);
}
public static boolean getLeftButton() {
return flight.getRawButton(2);
}
