| | 1 | {{{ |
| | 2 | /*----------------------------------------------------------------------------*/ |
| | 3 | /* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ |
| | 4 | /* Open Source Software - may be modified and shared by FRC teams. The code */ |
| | 5 | /* must be accompanied by the FIRST BSD license file in the root directory of */ |
| | 6 | /* the project. */ |
| | 7 | /*----------------------------------------------------------------------------*/ |
| | 8 | |
| | 9 | package frc.robot; |
| | 10 | |
| | 11 | import edu.wpi.first.wpilibj.TimedRobot; |
| | 12 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; |
| | 13 | import edu.wpi.first.wpilibj.Encoder; |
| | 14 | import edu.wpi.first.wpilibj.Talon; |
| | 15 | |
| | 16 | /** |
| | 17 | * The VM is configured to automatically run this class, and to call the |
| | 18 | * functions corresponding to each mode, as described in the TimedRobot |
| | 19 | * documentation. If you change the name of this class or the package after |
| | 20 | * creating this project, you must also update the build.gradle file in the |
| | 21 | * project. |
| | 22 | */ |
| | 23 | public class Robot extends TimedRobot { |
| | 24 | private Talon leftMotor, rightMotor; |
| | 25 | private Encoder leftEncoder, rightEncoder; |
| | 26 | |
| | 27 | /** |
| | 28 | * This function is run when the robot is first started up and should be |
| | 29 | * used for any initialization code. |
| | 30 | */ |
| | 31 | @Override |
| | 32 | public void robotInit() { |
| | 33 | // Quadrature encoder has A and B channels connected to DIO0,1, DIO2,3 |
| | 34 | leftEncoder = new Encoder(0, 1); |
| | 35 | rightEncoder = new Encoder(2, 3); |
| | 36 | // Peanut wheels are 7.5" in diameter |
| | 37 | // Encoders provide 1 count per degree of rotation |
| | 38 | leftEncoder.setDistancePerPulse((3.141592*7.5)/360); |
| | 39 | rightEncoder.setDistancePerPulse((3.141592*7.5)/360); |
| | 40 | // PWM motor controllers connected to PWM0, PWM1 |
| | 41 | leftMotor = new Talon(0); |
| | 42 | rightMotor = new Talon(1); |
| | 43 | } |
| | 44 | |
| | 45 | /** |
| | 46 | * This function is called every robot packet, no matter the mode. Use |
| | 47 | * this for items like diagnostics that you want ran during disabled, |
| | 48 | * autonomous, teleoperated and test. |
| | 49 | * |
| | 50 | * <p>This runs after the mode specific periodic functions, but before |
| | 51 | * LiveWindow and SmartDashboard integrated updating. |
| | 52 | */ |
| | 53 | @Override |
| | 54 | public void robotPeriodic() { |
| | 55 | SmartDashboard.putNumber("Left", leftEncoder.getDistance()); |
| | 56 | SmartDashboard.putNumber("Right", rightEncoder.getDistance()); |
| | 57 | } |
| | 58 | |
| | 59 | /** |
| | 60 | */ |
| | 61 | @Override |
| | 62 | public void autonomousInit() { |
| | 63 | // Reset encoder counts to 0 |
| | 64 | leftEncoder.reset(); |
| | 65 | rightEncoder.reset(); |
| | 66 | // start motors turning forward at 20% power |
| | 67 | leftMotor.set(0.20); |
| | 68 | // to go forward, left motor turns clockwise, right motor counter-clockwise |
| | 69 | rightMotor.set(-0.20); |
| | 70 | } |
| | 71 | |
| | 72 | /** |
| | 73 | * This function is called periodically during autonomous. |
| | 74 | */ |
| | 75 | @Override |
| | 76 | public void autonomousPeriodic() { |
| | 77 | if ((leftEncoder.getDistance() > 36.0) || |
| | 78 | (rightEncoder.getDistance() > 36.0)) { |
| | 79 | leftMotor.stopMotor(); |
| | 80 | rightMotor.stopMotor(); |
| | 81 | } |
| | 82 | } |
| | 83 | |
| | 84 | /** |
| | 85 | * This function is called periodically during operator control. |
| | 86 | */ |
| | 87 | @Override |
| | 88 | public void teleopPeriodic() { |
| | 89 | } |
| | 90 | |
| | 91 | /** |
| | 92 | * This function is called periodically during test mode. |
| | 93 | */ |
| | 94 | @Override |
| | 95 | public void testPeriodic() { |
| | 96 | } |
| | 97 | } |
| | 98 | }}} |
