API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 15th, 2018
75
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 21.45 KB | None | 0 0
raw download clone embed print report
  1. /*----------------------------------------------------------------------------*/
  2. /* Copyright (c) 2017 FIRST. All Rights Reserved. */
  3. /* Open Source Software - may be modified and shared by FRC teams. The code */
  4. /* must be accompanied by the FIRST BSD license file in the root directory of */
  5. /* the project. */
  6. /*----------------------------------------------------------------------------*/
  7.  
  8. package frc.team696;
  9.  
  10. import com.kauailabs.nav6.frc.IMU;
  11. import com.kauailabs.nav6.frc.IMUAdvanced;
  12. import edu.wpi.first.wpilibj.*;
  13. import edu.wpi.first.wpilibj.command.Command;
  14. import edu.wpi.first.wpilibj.command.Scheduler;
  15. import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
  16. import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  17. import frc.team696.autonomousCommands.*;
  18. import frc.team696.subsystems.*;
  19. import frc.team696.subsystems.DriveTrainSubsystem;
  20. import frc.team696.subsystems.IntakeSubsystem;
  21. import frc.team696.subsystems.RGBSensorSubsystem;
  22.  
  23. /**
  24. * @Author: Ismail Hasan && Justin Gonzales
  25. * <p>
  26. * Semi-fully custom-made code, this is where I'm doing things I've never done before,
  27. * while also making the most of my current abilities. I'm going next level.
  28. * </p>
  29. * Time and date of writing this comment: 4:48 AM, 2/5/2018
  30. */
  31.  
  32. public class Robot extends TimedRobot {
  33.  
  34. /*
  35. Creation of Class Objects
  36. */
  37.  
  38. // TODO Make class object(s) for the LEDSubsystem, finish DriveStraight code, and tune PID.
  39.  
  40. public static DriveTrainSubsystem driveTrainSubsystem = new DriveTrainSubsystem(RobotMap.leftRear, RobotMap.leftMid, RobotMap.leftFront,
  41. RobotMap.rightRear, RobotMap.rightMid, RobotMap.rightFront);
  42. public static Constants constants = new Constants();
  43. public static RGBSensorSubsystem rgbSensorSubsystem = new RGBSensorSubsystem(RobotMap.deviceAddress);
  44. public static IntakeSubsystem intakeSubsystem = new IntakeSubsystem(RobotMap.intakeA, RobotMap.intakeB, RobotMap.intakeSol);
  45. public static ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(RobotMap.elevator, RobotMap.elevatorSol, RobotMap.discBrake);
  46. public static ClimberSubsystem climberSubsystem = new ClimberSubsystem(RobotMap.climberA, RobotMap.climberB, RobotMap.climberSol);
  47. public static AntiTiltSubsystem antiTiltSubsystem = new AntiTiltSubsystem();
  48.  
  49.  
  50. /**
  51. * Object and Variable Declaration
  52. **/
  53.  
  54. public static OI oi;
  55. private String autonomousCommand;
  56. private SendableChooser<String> chooser = new SendableChooser<>();
  57. public static IMU navX;
  58. SerialPort port;
  59. public Timer time = new Timer();
  60. public static final PowerDistributionPanel PDP = new PowerDistributionPanel();
  61.  
  62. /**
  63. * Drive Variables
  64. **/
  65.  
  66. // Drive Straight and Deadzone variables
  67.  
  68. private final double wheelDeadZoneMin = -0.1;
  69. private final double wheelDeadZoneMax = 0.1;
  70. private final double stickDeadZoneMin = -0.25;
  71. private final double stickDeadZoneMax = 0.3;
  72.  
  73. int loopNumber = 0;
  74. double directionError;
  75. double currentDirection;
  76. double targetDirection;
  77.  
  78. public double speed;
  79. public double wheel;
  80. double leftDrive;
  81. double rightDrive;
  82.  
  83. // Speed Turn Scale Variables
  84.  
  85. final double a = 0.170641; // 0.286095 0.170641
  86. final double h = -0.258475; // -0.243151 -0.258475
  87. double k = 0.364407; // -0.130137 0.364407
  88.  
  89. // Nora's currently preferred values for turn scale
  90.  
  91. double speedTurnScale;
  92.  
  93. // Ramping Variables
  94.  
  95. double lowElevatorRampRate = 0.05;
  96. double highElevatorRampRate = 0.02;
  97. double centerElevatorRampRate = 0.05;
  98. double forwardRampingRate = 0.03;
  99. double commandedSpeed;
  100. double elevatorMaxHeight = 70;
  101. double minimumSpeed = -0.3;
  102. double elevatorActuatorLoopNumber;
  103.  
  104. /* Elevator Positioning Booleans */
  105.  
  106. public boolean lowButton;
  107. public boolean lowButtonOld;
  108. public boolean midButton;
  109. public boolean midButtonOld;
  110. public boolean highButton;
  111. public boolean highButtonOld;
  112. public boolean switchButton;
  113. public boolean switchButtonOld;
  114. public boolean intakeButton;
  115. public boolean intakeButtonOld;
  116. public boolean climbButton;
  117. public boolean climbButtonOld;
  118. public boolean moveLow;
  119. public boolean moveMid;
  120. public boolean moveHigh;
  121. public boolean moveSwitch;
  122. public boolean moveIntake;
  123. public boolean moveClimb;
  124.  
  125. /*
  126. Compressor
  127. */
  128.  
  129. public Compressor compressor = new Compressor();
  130.  
  131. /*
  132. Elevator Variables
  133. */
  134.  
  135. int elevatorLoopNumber = 0;
  136. double elevatorPositionInches;
  137. boolean elevatorSolState;
  138. double maxForwardSpeed = 0.5;
  139. double maxActuatingSpeed = 0.8;
  140. public boolean isHomed = true;
  141. String controlMode;
  142. int homeLoopNumber;
  143.  
  144. public boolean runElevator;
  145. public boolean currentElevatorState;
  146. public boolean oldElevatorState;
  147.  
  148. public boolean homeState;
  149. public boolean oldHomeState;
  150.  
  151.  
  152.  
  153. /*
  154. Intake Variables
  155. */
  156.  
  157. boolean intakeSolButton;
  158. boolean oldIntakeSolButton;
  159. boolean toggleIntake;
  160. double intakeSpeed = 0.7;
  161. double intakeOutputValue;
  162.  
  163. /*
  164. SPI Testing
  165. */
  166.  
  167. public SPI spiTest = new SPI(SPI.Port.kOnboardCS0);
  168.  
  169. /*
  170. Anti-Tilt Variables
  171. */
  172.  
  173. public boolean antiTilt = false;
  174.  
  175. @Override
  176. public void robotInit() {
  177. oi = new OI();
  178.  
  179. chooser.addObject("Left Position", "LeftPos");
  180. chooser.addObject("Center Position", "CenterPos");
  181. chooser.addObject("Right Position", "RightPos");
  182. chooser.addDefault("Center Position", "CenterPos");
  183. SmartDashboard.putData("Auto mode", chooser);
  184.  
  185. /*
  186. Initialization of the NavX Gyro
  187. */
  188.  
  189. try {
  190. byte UpdateRateHz = 50;
  191. port = new SerialPort(57600, SerialPort.Port.kMXP);
  192. navX = new IMUAdvanced(port, UpdateRateHz);
  193. } catch (Exception ex) {
  194. System.out.println("NavX not working");
  195. }
  196.  
  197. /*
  198. Zero Elevator
  199. */
  200.  
  201. /*
  202. Camera
  203. */
  204.  
  205. CameraServer.getInstance().addAxisCamera("10.6.96.11");
  206.  
  207. /*
  208. Turn on LEDS
  209. */
  210.  
  211. for (int i = 8; i <= 13; i++) {
  212. OI.ControlPanel.setOutput(i, true);
  213. }
  214.  
  215.  
  216. }
  217.  
  218. @Override
  219. public void disabledInit() {
  220.  
  221. }
  222.  
  223. @Override
  224. public void disabledPeriodic() {
  225.  
  226. Scheduler.getInstance().run();
  227. SmartDashboard.putString("Selected Auto", chooser.getSelected());
  228. }
  229.  
  230. @Override
  231. public void autonomousInit() {
  232. autonomousCommand = chooser.getSelected();
  233. if (autonomousCommand.equals("LeftPos")) {
  234. new LeftPos().start();
  235. }
  236.  
  237. if (autonomousCommand.equals("CenterPos")) {
  238. new CenterPos().start();
  239. }
  240.  
  241. if (autonomousCommand.equals("RightPos")) {
  242. new RightPos().start();
  243. }
  244.  
  245. // if (autonomousCommand != null) {
  246. // autonomousCommand.start();
  247. // }
  248. }
  249.  
  250. @Override
  251. public void autonomousPeriodic() {
  252. Scheduler.getInstance().run();
  253. }
  254.  
  255. @Override
  256. public void teleopInit() {
  257.  
  258. // if (autonomousCommand != null) {
  259. // autonomousCommand.cancel();
  260. // }
  261.  
  262. rgbSensorSubsystem.rgbSensor.write(0xC0, 1); // set Integration Time
  263. rgbSensorSubsystem.rgbSensor.write(0x02, 1); // set Gain
  264.  
  265. /*
  266. Start Compressor
  267. */
  268.  
  269. compressor.start();
  270. time.start();
  271. navX.zeroYaw();
  272.  
  273. }
  274.  
  275. @Override
  276. public void teleopPeriodic() {
  277. Scheduler.getInstance().run();
  278.  
  279. /*
  280. LED Outputs
  281. */
  282.  
  283. if (elevatorSubsystem.elevator.getSensorCollection().isRevLimitSwitchClosed()) {
  284. // elevatorSubsystem.elevator.setSelectedSensorPosition(0, 0, 20);
  285. OI.ControlPanel.setOutput(3, true);
  286. } else {
  287. OI.ControlPanel.setOutput(3, false);
  288. }
  289.  
  290. if (elevatorSubsystem.elevator.getSensorCollection().isFwdLimitSwitchClosed()) {
  291. OI.ControlPanel.setOutput(5, true);
  292. } else {
  293. OI.ControlPanel.setOutput(5, false);
  294. }
  295.  
  296. if (OI.ControlPanel.getRawButton(2)) {
  297. OI.ControlPanel.setOutput(6, true);
  298. } else {
  299. OI.ControlPanel.setOutput(7, true);
  300. }
  301.  
  302. OI.ControlPanel.setOutput(15, true);
  303.  
  304.  
  305. /** Drive Functionality **/
  306.  
  307.  
  308. if (wheel < 0) {
  309. wheel = wheel - wheelDeadZoneMax;
  310. } else {
  311. wheel = wheel + wheelDeadZoneMax;
  312. }
  313.  
  314. if (OI.ControlPanel.getRawButton(14)) {
  315. antiTilt = true;
  316. } else {
  317. antiTilt = false;
  318. }
  319.  
  320. if (antiTilt) {
  321. antiTiltSubsystem.antiTilt();
  322. speed = antiTiltSubsystem.speed;
  323. wheel = (antiTiltSubsystem.wheel * speedTurnScale) - wheelDeadZoneMax;
  324. } else {
  325. speed = -OI.Stick.getRawAxis(1);
  326. wheel = (OI.wheel.getRawAxis(constants.wheelDriveAxis) * speedTurnScale) - wheelDeadZoneMax;
  327. }
  328.  
  329. speedTurnScale = a * (1 / ((speed * speed) - h)) + k;
  330.  
  331.  
  332. /**
  333. Climber Functions
  334. */
  335.  
  336. // if(OI.Psoc.getRawButton(16)) {
  337. // climberSubsystem.setClimberSpeed(1);
  338. // }
  339. // else if(OI.wheel.getRawButton(2)){
  340. // climberSubsystem.setClimberSpeed(-0.25);
  341. // }
  342. // else{
  343. //
  344. //// climberSubsystem.deployHook(true);
  345. // }
  346.  
  347.  
  348. /** RGB Sensor **/
  349.  
  350. // rgbSensorSubsystem.rgbGetLux();
  351.  
  352.  
  353. /** Climber Functions **/
  354. // if(OI.Psoc.getRawButton(16)){
  355. // climberSubsystem.setClimberSpeed(1);
  356. // }else{
  357. // climberSubsystem.setClimberSpeed(0);
  358. // }
  359.  
  360.  
  361. /** Elevator Functions **/
  362.  
  363. // Toggle Elevator Solenoid
  364.  
  365. /**
  366. * Checks to see the current boolean state for the elevator button, and compares it to another boolean, oldElevatorState.
  367. * The if statement checks to see if the two boolean states are opposite of each other, and if they are, flip
  368. * the state of the runElevator boolean, which controls the pneumatic solenoid of the elevator.
  369. * After the check, oldElevatorState is set equal to currentElevatorState to keep it consistent with the robot,
  370. * and to be able to check again in the future.
  371. */
  372.  
  373. // Homing of the elevator
  374.  
  375. homeState = OI.ControlPanel.getRawButton(constants.elevatorHome);
  376. if (homeState && !oldHomeState) {
  377. isHomed = !isHomed;
  378. }
  379. oldHomeState = homeState;
  380.  
  381. currentElevatorState = OI.ControlPanel.getRawButton(6);
  382. if (antiTiltSubsystem.preventBack && antiTilt) {
  383. runElevator = true;
  384. } else if (antiTiltSubsystem.preventForward && antiTilt) {
  385. runElevator = false;
  386. } else if (currentElevatorState && !oldElevatorState) {
  387. runElevator = !runElevator;
  388. }
  389. oldElevatorState = currentElevatorState;
  390.  
  391.  
  392. if (runElevator) {
  393. elevatorSubsystem.toggleElevatorPos(true);
  394. } else {
  395. elevatorSubsystem.toggleElevatorPos(false);
  396. }
  397.  
  398. /**
  399. * PID Move Elevator
  400. */
  401.  
  402. lowButton = OI.ControlPanel.getRawButton(constants.scaleLow);
  403. if (lowButton && !lowButtonOld) {
  404. moveLow = !moveLow;
  405. moveMid = false;
  406. moveHigh = false;
  407. moveIntake = false;
  408. moveSwitch = false;
  409. moveClimb = false;
  410. }
  411. lowButtonOld = lowButton;
  412.  
  413. switchButton = OI.ControlPanel.getRawButton(constants.switchPos);
  414. if (switchButton && !switchButtonOld) {
  415. moveLow = false;
  416. moveMid = false;
  417. moveHigh = false;
  418. moveIntake = false;
  419. moveSwitch = !moveSwitch;
  420. moveClimb = false;
  421. }
  422. switchButtonOld = switchButton;
  423.  
  424.  
  425. climbButton = OI.ControlPanel.getRawButton(constants.climbPos);
  426. if (climbButton && !climbButtonOld) {
  427. moveLow = false;
  428. moveMid = false;
  429. moveHigh = false;
  430. moveIntake = false;
  431. moveSwitch = false;
  432. moveClimb = !moveClimb;
  433. }
  434. climbButtonOld = climbButton;
  435. //
  436. intakeButton = OI.ControlPanel.getRawButton(constants.intakePos);
  437. if (intakeButton && !intakeButtonOld) {
  438. moveLow = false;
  439. moveMid = false;
  440. moveHigh = false;
  441. moveIntake = !moveIntake;
  442. moveSwitch = false;
  443. moveClimb = false;
  444. }
  445. intakeButtonOld = intakeButton;
  446.  
  447. highButton = OI.ControlPanel.getRawButton(constants.scaleHigh);
  448. if (highButton && !highButtonOld) {
  449. moveLow = false;
  450. moveMid = false;
  451. moveHigh = !moveHigh;
  452. moveIntake = false;
  453. moveSwitch = false;
  454. moveClimb = false;
  455. }
  456. highButtonOld = highButton;
  457.  
  458. midButton = OI.ControlPanel.getRawButton(constants.scaleMid);
  459. if (midButton && !midButtonOld) {
  460. moveLow = false;
  461. moveMid = !moveMid;
  462. moveHigh = false;
  463. moveIntake = false;
  464. moveSwitch = false;
  465. moveClimb = false;
  466. }
  467. midButtonOld = midButton;
  468.  
  469. intakeSolButton = OI.ControlPanel.getRawButton(constants.intakeSol);
  470. if (intakeSolButton && !oldIntakeSolButton) {
  471. toggleIntake = !toggleIntake;
  472. }
  473. oldIntakeSolButton = intakeSolButton;
  474.  
  475.  
  476. // if(moveLow){
  477. // elevatorSubsystem.moveToPos("low");
  478. // }
  479. // if(moveSwitch){
  480. // elevatorSubsystem.moveToPos("switch");
  481. // }
  482. // if(moveClimb) {
  483. // elevatorSubsystem.moveToPos("climb");
  484. // }
  485.  
  486. // lo = OI.ControlPanel.getRawButton(constants.lowButton);
  487. // if(lowButton && !lowButtonOld){
  488. // moveLow = !moveLow;
  489. // }
  490. // lowButtonOld = lowButton;
  491. //
  492. // if(moveLow){
  493. // movePos = "low";
  494. // }
  495. //
  496. //
  497. //
  498.  
  499.  
  500. /**
  501. * Manual-Move Elevator
  502. */
  503.  
  504. controlMode = elevatorSubsystem.elevator.getControlMode().toString();
  505.  
  506. if (!isHomed) {
  507. elevatorSubsystem.discBrake.set(true);
  508. elevatorSubsystem.manualMoveElevator(-0.25);
  509. System.out.println("Homing...");
  510. // elevatorSubsystem.elevator.setSelectedSensorPosition(0, 0, 20);
  511. elevatorSubsystem.zeroElevator();
  512. if (elevatorSubsystem.elevator.getSensorCollection().isRevLimitSwitchClosed()) {
  513. elevatorSubsystem.discBrake.set(false);
  514. elevatorSubsystem.manualMoveElevator(0);
  515. System.out.println("Homed");
  516. isHomed = true;
  517. elevatorSubsystem.zeroElevator();
  518. }
  519. } else if (!elevatorSubsystem.elevatorSol.get() && elevatorSubsystem.elevator.getSelectedSensorPosition(0) >= 26000 && OI.ControlPanel.getRawAxis(0) < -0.1) {
  520. elevatorSubsystem.discBrake.set(false);
  521. elevatorSubsystem.manualMoveElevator(0);
  522. System.out.println("hi");
  523. } else if (OI.ControlPanel.getRawAxis(0) < -0.1 && isHomed) {
  524. elevatorLoopNumber++;
  525. elevatorSubsystem.discBrake.set(true);
  526. moveLow = false;
  527. moveMid = false;
  528. moveHigh = false;
  529. moveIntake = false;
  530. moveSwitch = false;
  531. moveClimb = false;
  532. if (elevatorLoopNumber > 2) {
  533. elevatorSubsystem.manualMoveElevator(-OI.ControlPanel.getRawAxis(0));
  534. }
  535. } else if (OI.ControlPanel.getRawAxis(0) > 0.1 && isHomed) {
  536. elevatorLoopNumber++;
  537. elevatorSubsystem.discBrake.set(true);
  538. moveLow = false;
  539. moveMid = false;
  540. moveHigh = false;
  541. moveIntake = false;
  542. moveSwitch = false;
  543. moveClimb = false;
  544. if (elevatorLoopNumber > 2) {
  545. elevatorSubsystem.manualMoveElevator(-OI.ControlPanel.getRawAxis(0) * 0.5);
  546. }
  547. } else if (moveSwitch) {
  548. elevatorSubsystem.moveToPos("switch");
  549. } else if (moveClimb) {
  550. elevatorSubsystem.moveToPos("climb");
  551. } else if (moveIntake) {
  552. elevatorSubsystem.moveToPos("intake");
  553. } else if (moveHigh) {
  554. elevatorSubsystem.moveToPos("high");
  555. } else if (moveLow) {
  556. elevatorSubsystem.moveToPos("low");
  557. } else if (moveMid) {
  558. elevatorSubsystem.moveToPos("mid");
  559. } else if (OI.ControlPanel.getRawButton(1)) {
  560. /** Auto Climb and Unwind - Once pressed: Hook will deploy, Elevator will retract, and hold to keep climbing **/
  561. moveMid = false;
  562. moveHigh = false;
  563. moveIntake = false;
  564. moveSwitch = false;
  565. moveClimb = false;
  566. moveLow = false;
  567. climberSubsystem.autoClimb(1);
  568. } else if (OI.ControlPanel.getRawButton(15)) {
  569. climberSubsystem.setClimberSpeed(-0.25);
  570. } else {
  571. elevatorLoopNumber = 0;
  572. climberSubsystem.loopNumber = 0;
  573. climberSubsystem.setClimberSpeed(0);
  574. // elevatorSubsystem.discBrake.set(false); already set in auto-climb, would cause overlaps.
  575. elevatorSubsystem.manualMoveElevator(0);
  576. elevatorSubsystem.discBrake.set(false);
  577. homeLoopNumber = 0;
  578. climberSubsystem.deployHook(false);
  579. }
  580.  
  581. /** Intake Functions **/
  582.  
  583. if (OI.ControlPanel.getRawButton(8)) {
  584. intakeSubsystem.runIntake(-0.35);
  585. } else if (OI.ControlPanel.getRawButton(7)) {
  586. intakeSubsystem.runIntake(0.6);
  587. } else {
  588. intakeSubsystem.runIntake(0);
  589. }
  590.  
  591.  
  592. /**
  593. * Intake Solenoid Toggling
  594. */
  595.  
  596. intakeSubsystem.toggleIntake(toggleIntake);
  597.  
  598.  
  599. /** Any Testing/Temporary Code will Go here **/
  600.  
  601.  
  602. /** VERY WIP, DOESN'T FULLY FUNCTION CURRENTLY
  603. Drive-straight and Testing code.
  604. **/
  605.  
  606. if (wheel >= wheelDeadZoneMin && wheel <= wheelDeadZoneMax) {
  607.  
  608. // loopNumber++;
  609. // currentDirection = navX.getYaw();
  610. // if(loopNumber == 1){
  611. // targetDirection = navX.getYaw();
  612. // }
  613. // directionError = targetDirection - currentDirection;
  614. // driveTrainSubsystem.driveStraightPID.setError(directionError);
  615. // wheel = driveTrainSubsystem.driveStraightPID.getValue();
  616. wheel = 0;
  617.  
  618. } else {
  619. loopNumber = 0;
  620. }
  621.  
  622. // /**
  623. // * Speed Deadzone
  624. // */
  625. //
  626. // if(speed >= stickDeadZoneMin && speed <= stickDeadZoneMax){
  627. // speed = 0;
  628. // }
  629.  
  630.  
  631. // if(elevatorSubsystem.elevator.getSensorCollection().isRevLimitSwitchClosed()){
  632. // elevatorSubsystem.elevator.setSelectedSensorPosition(0, 0, 20);
  633. // }
  634.  
  635. leftDrive = speed + wheel;
  636. rightDrive = speed - wheel;
  637.  
  638. driveTrainSubsystem.tankDrive(leftDrive, rightDrive);
  639.  
  640. /** Outputs to Console **/
  641.  
  642. // System.out.println("speed " + speed);
  643. // System.out.println("loopNumber = " + (loopNumber) + " time.get: " + time.get());
  644. // System.out.println(elevatorSubsystem.elevatorSol.get() + " " + speed);
  645. // System.out.println(elevatorLoopNumber);
  646. // System.out.println(runElevator);
  647. // System.out.println("intakeOutputValue = " + intakeOutputValue);
  648. // System.out.println(elevatorSubsystem.elevator.getSelectedSensorPosition(0));
  649. // System.out.println(isHomed + " " + elevatorSubsystem.elevator.getSelectedSensorPosition(0) + " " + elevatorLoopNumber);
  650.  
  651. // System.out.println(PDP.getCurrent(7) + " " + PDP.getCurrent(8));
  652. // System.out.println(elevatorSubsystem.elevator.getSelectedSensorPosition(0));
  653. // System.out.println(elevatorSubsystem.elevator.getSelectedSensorPosition(0) + " " + elevatorSubsystem.elevatorTarget + " " + isHomed);
  654. // System.out.println("TARGET" + elevatorSubsystem.elevator.getClosedLoopTarget(0));
  655.  
  656.  
  657. // System.out.println("TARGET" + elevatorSubsystem.elevatorTarget);
  658.  
  659. // System.out.println(movePos + " " + moveClimb + " " + climbButton + " " + elevatorSubsystem.elevatorSol.get() + " " + elevatorSubsystem.elevator.getSelectedSensorPosition(0) +
  660. // " " + elevatorSubsystem.elevator.getClosedLoopError(0));
  661. // System.out.println(elevatorSubsystem.elevator.getSelectedSensorPosition(0));
  662. // System.out.println(controlMode + " " + moveSwitch + " " + moveClimb + " " + elevatorSubsystem.elevator.getClosedLoopError(0) + " " + elevatorSubsystem.elevatorTarget);
  663.  
  664. System.out.println(navX.getYaw());
  665.  
  666.  
  667. }
  668.  
  669.  
  670. @Override
  671. public void testPeriodic() {
  672.  
  673.  
  674. }
  675.  
  676. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!