| | 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 | import edu.wpi.first.wpilibj.Timer; |
| | 16 | |
| | 17 | /** |
| | 18 | * The VM is configured to automatically run this class, and to call the |
| | 19 | * functions corresponding to each mode, as described in the TimedRobot |
| | 20 | * documentation. If you change the name of this class or the package after |
| | 21 | * creating this project, you must also update the build.gradle file in the |
| | 22 | * project. |
| | 23 | */ |
| | 24 | public class Robot extends TimedRobot { |
| | 25 | private Talon leftMotor, rightMotor; |
| | 26 | private Encoder leftEncoder, rightEncoder; |
| | 27 | private double leftPower, rightPower; |
| | 28 | private Timer autoTimer; |
| | 29 | private boolean autoDone; |
| | 30 | |
| | 31 | /** |
| | 32 | * This function is run when the robot is first started up and should be |
| | 33 | * used for any initialization code. |
| | 34 | */ |
| | 35 | @Override |
| | 36 | public void robotInit() { |
| | 37 | // Quadrature encoder has A and B channels connected to DIO0,1, DIO2,3 |
| | 38 | leftEncoder = new Encoder(0, 1); |
| | 39 | rightEncoder = new Encoder(2, 3); |
| | 40 | // Peanut wheels are 7.5" in diameter |
| | 41 | // Encoders provide 1 count per degree of rotation |
| | 42 | leftEncoder.setDistancePerPulse((3.141592*7.5)/360); |
| | 43 | rightEncoder.setDistancePerPulse((3.141592*7.5)/360); |
| | 44 | // PWM motor controllers connected to PWM0, PWM1 |
| | 45 | leftMotor = new Talon(0); |
| | 46 | rightMotor = new Talon(1); |
| | 47 | // timer to manage autonomous direction updates |
| | 48 | autoTimer = new Timer(); |
| | 49 | } |
| | 50 | |
| | 51 | /** |
| | 52 | * This function is called every robot packet, no matter the mode. Use |
| | 53 | * this for items like diagnostics that you want ran during disabled, |
| | 54 | * autonomous, teleoperated and test. |
| | 55 | * |
| | 56 | * <p>This runs after the mode specific periodic functions, but before |
| | 57 | * LiveWindow and SmartDashboard integrated updating. |
| | 58 | */ |
| | 59 | @Override |
| | 60 | public void robotPeriodic() { |
| | 61 | SmartDashboard.putNumber("Left Distance", leftEncoder.getDistance()); |
| | 62 | SmartDashboard.putNumber("Right Distance", rightEncoder.getDistance()); |
| | 63 | SmartDashboard.putNumber("Left Power", leftPower); |
| | 64 | SmartDashboard.putNumber("Right Power", rightPower); |
| | 65 | } |
| | 66 | |
| | 67 | /** |
| | 68 | */ |
| | 69 | @Override |
| | 70 | public void autonomousInit() { |
| | 71 | // Reset encoder counts to 0 |
| | 72 | leftEncoder.reset(); |
| | 73 | rightEncoder.reset(); |
| | 74 | // start motors turning forward at 20% power |
| | 75 | leftPower = 0.20; |
| | 76 | leftMotor.set(leftPower); |
| | 77 | // to go forward, left motor turns clockwise, right motor counter-clockwise |
| | 78 | rightPower = -0.20; |
| | 79 | rightMotor.set(rightPower); |
| | 80 | |
| | 81 | autoTimer.start(); |
| | 82 | autoDone = false; |
| | 83 | } |
| | 84 | |
| | 85 | /** |
| | 86 | * This function is called periodically during autonomous. |
| | 87 | */ |
| | 88 | @Override |
| | 89 | public void autonomousPeriodic() { |
| | 90 | if (!autoDone) { |
| | 91 | // adjust motor power every 1/4 second to keep us going straight |
| | 92 | if (autoTimer.hasPeriodPassed(0.25)) { |
| | 93 | // if we're going straight, leftDistance == rightDistance |
| | 94 | // otherwise, compute the error and adjust the motor powers to minimize it |
| | 95 | double error = leftEncoder.getDistance() - rightEncoder.getDistance(); |
| | 96 | double kP = 0.01; // proportion of error used to correct motor power |
| | 97 | // adjust motor power to try to keep left/right distances same |
| | 98 | leftPower -= kP*error; // positive error means left is going too fast |
| | 99 | rightPower -= kP*error; // right motor spins opposite direction of left |
| | 100 | leftMotor.set(leftPower); |
| | 101 | rightMotor.set(rightPower); |
| | 102 | } |
| | 103 | |
| | 104 | if ((leftEncoder.getDistance() > 36.0) || |
| | 105 | (rightEncoder.getDistance() > 36.0)) { |
| | 106 | leftMotor.stopMotor(); |
| | 107 | rightMotor.stopMotor(); |
| | 108 | autoDone = true; |
| | 109 | } |
| | 110 | } |
| | 111 | } |
| | 112 | |
| | 113 | /** |
| | 114 | * This function is called periodically during operator control. |
| | 115 | */ |
| | 116 | @Override |
| | 117 | public void teleopPeriodic() { |
| | 118 | } |
| | 119 | |
| | 120 | /** |
| | 121 | * This function is called periodically during test mode. |
| | 122 | */ |
| | 123 | @Override |
| | 124 | public void testPeriodic() { |
| | 125 | } |
| | 126 | } |
| | 127 | }}} |
