public void drive() { // Driving controls /* flips which direction on the joysticks is 'forward' * the variable 'controlFlipClean' needs some explanation: * it forces the driver to release the button before it recognizes * another click. */ if (xboxDriver.getBtnL3()) { // left stick click if (controlFlipClean) { controlFlipClean = false; controlFlip = !controlFlip; } } else { controlFlipClean = true; } // controls right side double yRight = -xboxDriver.getRightY(); // controls left side double yLeft = xboxDriver.getLeftY(); // trigger values will change the maximum speed of the robot double throttle = xboxDriver.getTriggers(); // half speed if (throttle > 0.5) { yRight = yRight * 0.5; yLeft = yLeft * 0.5; } // quarter speed if (throttle < -0.5) { yRight = yRight * 0.25; yLeft = yLeft * 0.25; } // flip controls if we need to. Make them go 'backwards'. // We need to swap and yRight and yLeft values. if (controlFlip) { double tempRight = yRight; yRight = yLeft; yLeft = tempRight; } /* DEADZONE ** * The springs in the joysticks wear out so they don't always come * back to the centre. To fix this we're going to ignore a small region * around the centre. */ if (yRight > -DEADZONE && yRight < DEADZONE) { yRight = 0; } if (yLeft > -DEADZONE && yLeft < DEADZONE) { yLeft = 0; } vicDriveRight.set(yRight); vicDriveLeft.set(yLeft); } // end of drive
public void run() { double power; if (shooting == false && retracting == false) { power = 0.0; } else if (shooting == false && retracting == true) { power = m_retract(); } else if (shooting == true && retracting == false) { power = m_shoot(); } else { // shooting == true && retracting == true power = 0.0; } motor1.set(power); motor2.set(power); motor3.set(power); }
public void autonomousInit() { lightsOutput1.set(true); lightsOutput2.set(true); lightsOutput3.set(false); canShoot = false; vicDriveRight.set(0); vicDriveLeft.set(0); armVictor.set(0); winchRelay.set(Relay.Value.kOff); shooterWheel.set(0); shooterLoft.set(0); bettyRelay.set(Relay.Value.kOff); starterDelay = 0; }
/** Called every time tele-op mode starts */ public void teleopInit() { vicDriveRight.set(0); vicDriveLeft.set(0); armVictor.set(0); winchRelay.set(Relay.Value.kOff); shooterWheel.set(0); shooterLoft.set(0); bettyRelay.set(Relay.Value.kOff); lightsOutput1.set(true); lightsOutput2.set(true); lightsOutput3.set(true); lightsTimer.reset(); lightsTimer.start(); teleopActive = false; controlFlip = true; controlFlipClean = true; shootingSpeed = 0.0; starterDelay = 0; }
public void shooterUse() { // Betty operation if (xboxOperator.getBtnLB()) { // hold right bumper to fire starterDelay++; if (xboxOperator.getTriggers() < -0.5) { // feeder shot if (shootingSpeed > feederShotSpeed) { // accelerating shooter speed until top speed shootingSpeed = shootingSpeed + flyWheelAcceleration; } else { shootingSpeed = feederShotSpeed; // ensures desired value } } else { // normal shot if (shootingSpeed > normalShotSpeed) { // accelerating shooter speed until top speed shootingSpeed = shootingSpeed + flyWheelAcceleration; } else { shootingSpeed = normalShotSpeed; // ensures desired value } } shooterWheel.set(shootingSpeed); if (xboxOperator.getBtnRB() && starterDelay > 60) { // shoot normal shot bettyRelay.setDirection(Relay.Direction.kForward); bettyRelay.set(Relay.Value.kOn); } else if (xboxOperator.getBtnB() && starterDelay > 60) { // shoot normal shot bettyRelay.setDirection(Relay.Direction.kReverse); bettyRelay.set(Relay.Value.kOn); } else { bettyRelay.set(Relay.Value.kOff); } } else if (bettyOpen.get()) { // close betty if not shooting shootingSpeed = 0.0; bettyRelay.setDirection(Relay.Direction.kForward); bettyRelay.set(Relay.Value.kOn); shooterWheel.set(0); starterDelay = 0; } else { shootingSpeed = 0.0; bettyRelay.set(Relay.Value.kOff); shooterWheel.set(0); starterDelay = 0; } } // end of shooter use
/** This function is called periodically during autonomous */ public void autonomousPeriodic() { // int loftPotValue = loftPot.getValue(); // // ajust for far shot // if (!canShoot) { // if (loftPotValue < farShotValue - 8) { // starterDelay = 0; // shooterLoft.set(-0.5); // // } // else if (loftPotValue > farShotValue + 8) { // starterDelay = 0; // shooterLoft.set(0.5); // // } // else { // if ajusted, shoot // canShoot = true; // shooterLoft.set(0); // } // } // if(canShoot) { if (shootingSpeed > normalShotSpeed) { // accelerating shooter speed until top speed(1.0) shootingSpeed = shootingSpeed + flyWheelAcceleration; } else { shootingSpeed = normalShotSpeed; // ensures desired value and no higher } shooterWheel.set(shootingSpeed); if (shootingSpeed == normalShotSpeed) { starterDelay++; if (starterDelay > 60) { bettyRelay.setDirection(Relay.Direction.kForward); bettyRelay.set(Relay.Value.kOn); } else if (starterDelay > 310) { // kill flywheel and betty bettyRelay.set(Relay.Value.kOff); shooterWheel.set(0); } else { bettyRelay.set(Relay.Value.kOff); } } // } }
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()); }
public void manualControl(double power) { motor1.set(power); }
public void setRollers(double i) { rollers.set(i); }
/** This function is called periodically during operator control */ public void teleopPeriodic() { lights.set(Relay.Value.kForward); // tracking.run(); /*try { readString = serial.readString(); readString = readString.substring(readString.indexOf("data{"), readString.indexOf("}")); xTurn = readString.substring(readString.indexOf("x("), readString.indexOf(")")); xTurnVal = Integer.parseInt(xTurn); } catch (VisaException ex) { System.out.println("Error Reading string - " + ex); }*/ y = drive_control.getRawAxis(1); x = drive_control.getRawAxis(2); t = drive_control.getRawAxis(3); if (y > 1) y = 1; else if (y < -1) y = -1; if (x > 1) x = 1; else if (x < -1) x = -1; drive.setDriveJ(x, y, t); /*if(drive_control.getRawButton(12)) //wheelsOn = true; else if(drive_control.getRawButton(11)); wheelsOn = false;*/ /*try{ launcher.setX((drive_control.getRawAxis(4)-1)/2); }catch(Exception e){}*/ // System.out.println((drive_control.getRawAxis(4)-1)*-1350); if (!autoWheels) { csc.setSetpoint((drive_control.getRawAxis(4) - 1) * -1500); try { // launcher.setX((drive_control.getRawAxis(4)+1)*2000); System.out.println( -launcher.getSpeed() + " -- " + csc.tmpResult + " -- " + (drive_control.getRawAxis(4) - 1) * -1500); } catch (Exception e) { System.out.println(e); } if (drive_control.getRawButton(11)) autoWheels = true; } else { csc.setSetpoint(2300); if (drive_control.getRawButton(11)) autoWheels = false; } /*if(wheelsOn) shoot(1); else shoot(0);*/ if (drive_control.getRawButton(5)) { wedge.moveDown(); // servo1.set(1); // servo2.set(0); // wedge.set(Relay.Value.kForward); } else if (drive_control.getRawButton(6)) { wedge.moveUp(); // servo1.set(0); // servo2.set(1); // wedge.set(Relay.Value.kReverse); } else { // servo1.set(.5); // servo2.set(.5); // wedge.set(Relay.Value.kOff); } if (drive_control.getRawButton(3)) turret.set(.5); else if (drive_control.getRawButton(4)) turret.set(-.5); else turret.set(0); if (drive_control.getRawButton(2)) driveMainBelt(true, true); else if (drive_control.getRawButton(7)) driveMainBelt(false, true); else driveMainBelt(false, false); if (drive_control.getTrigger()) { try { belt2.setX(-1); } catch (Exception e) { } } else if (drive_control.getRawButton(8)) { try { belt2.setX(1); } catch (Exception e) { } } else { try { belt2.setX(0); } catch (Exception e) { } } if (control.getRawButton(3)) { turret.set(.25); } else if (control.getRawButton(4)) { turret.set(-.25); } else { turret.set(0); } try { station.println( DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(-launcher.getSpeed())); } catch (CANTimeoutException ex) { } /*try{ if(tracking.foundTarget()){ station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance)); station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8)); if(x>0) station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--"); else if(x<0) station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->"); else station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate"); } else{ station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!"); station.println(DriverStationLCD.Line.kUser3, 1, ""); station.println(DriverStationLCD.Line.kUser4, 1, ""); } }catch(Exception e){}*/ station.updateLCD(); }
public void armUse() { // Arm contols int armPotValue = armPot.getValue(); if (xboxDriver.getBtnB()) { // arm for slot feeding if (armPotValue < armSlotFeedingValue - angleTolerance) { armVictor.set(-.2); } else if (armPotValue < armSlotFeedingValue - focusTolerance) { armVictor.set(-.02); } else if (armPotValue > armSlotFeedingValue + angleTolerance) { armVictor.set(.5); } else if (armPotValue > armSlotFeedingValue + focusTolerance) { armVictor.set(.05); } else { armVictor.set(0); } } else if (xboxDriver.getBtnY()) { // arm for shovel feeding if (armPotValue < armShovelFeedingValue - angleTolerance) { armVictor.set(-.2); } else if (armPotValue < armShovelFeedingValue - focusTolerance) { armVictor.set(-.02); } else if (armPotValue > armShovelFeedingValue + angleTolerance) { armVictor.set(.5); } else if (armPotValue > armShovelFeedingValue + focusTolerance) { armVictor.set(.05); } else { armVictor.set(0); } } else { if (xboxDriver.getBtnLB() && armPotValue > minArmValue) { armVictor.set(-.2); // driver left bumper = down, right bumper = up; } else if (xboxDriver.getBtnRB() && armPotValue < maxArmValue) { armVictor.set(.5); } else { armVictor.set(0); } if (xboxDriver.getBtnA() && armPotValue < climbStopValue) { // when driver controls, hold button A to use winch, release to // stop winchRelay.set(Relay.Value.kOn); winchRelay.setDirection(Relay.Direction.kReverse); } else { winchRelay.set(Relay.Value.kOff); } } } // end of arm use
public void moveRollerReverse() { // Move the roller to suck in the ball roller.set(-1); }
public void turnRollerOff() { roller.set(0); }
private void setPower(double power) { motor1.set(-power); }
public void shooterLoft() { // Ajust loft double operatorLeftY = xboxOperator.getLeftY(); // operator has loft control int loftPotValue = loftPot.getValue(); if (xboxOperator.getBtnX()) { // hold to ajust for close shot if (loftPotValue < closeShotValue - focusTolerance) { if (loftPotValue < closeShotValue - angleTolerance) { shooterLoft.set(-1); } else { shooterLoft.set(-0.001); } } else if (loftPotValue > closeShotValue + focusTolerance) { if (loftPotValue > closeShotValue + angleTolerance) { shooterLoft.set(1); } else { shooterLoft.set(0.001); } } else { shooterLoft.set(0); } } else if (xboxOperator.getBtnY()) { // hold to ajust for far shot if (loftPotValue < farShotValue - focusTolerance) { if (loftPotValue < farShotValue - angleTolerance) { shooterLoft.set(-1); } else { shooterLoft.set(-0.001); } } else if (loftPotValue > farShotValue + focusTolerance) { if (loftPotValue > farShotValue + angleTolerance) { shooterLoft.set(1); } else { shooterLoft.set(0.001); } } else { shooterLoft.set(0); } } else if (xboxOperator.getBtnB()) { // hold to ajust for feeding if (loftPotValue < shooterFeedingValue - focusTolerance) { if (loftPotValue < shooterFeedingValue - angleTolerance) { shooterLoft.set(-1); } else { shooterLoft.set(-0.001); } } else if (loftPotValue > shooterFeedingValue + focusTolerance) { if (loftPotValue > shooterFeedingValue + angleTolerance) { shooterLoft.set(1); } else { shooterLoft.set(0.001); } } else { shooterLoft.set(0); } } else if (loftPotValue > maxHeightValue && loftPotValue < minHeightValue && ((operatorLeftY > .3) || operatorLeftY < -.3)) { // move loft with left operator stick shooterLoft.set(operatorLeftY); } else { shooterLoft.set(0); // don't move loft } } // end of shooter loft
public void stop() { motor1.set(0); }
public void moveRollerForward() { // Move the roller to eject the ball roller.set(1); }