| | 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 com.ctre.phoenix.motorcontrol.can.*; |
| | 14 | import com.ctre.phoenix.motorcontrol.FeedbackDevice; |
| | 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 WPI_TalonSRX leftMotor, rightMotor; |
| | 25 | |
| | 26 | /** |
| | 27 | * This function is run when the robot is first started up and should be |
| | 28 | * used for any initialization code. |
| | 29 | */ |
| | 30 | @Override |
| | 31 | public void robotInit() { |
| | 32 | // CAN motor controllers connected to CAN bus (addrs 3,4) |
| | 33 | leftMotor = new WPI_TalonSRX(3); |
| | 34 | rightMotor = new WPI_TalonSRX(4); |
| | 35 | // clear any previously configured settings |
| | 36 | leftMotor.configFactoryDefault(); |
| | 37 | rightMotor.configFactoryDefault(); |
| | 38 | // Quadrature encoders are connected directly to the motor controllers |
| | 39 | leftMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); |
| | 40 | rightMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); |
| | 41 | } |
| | 42 | |
| | 43 | /** |
| | 44 | * This function is called every robot packet, no matter the mode. Use |
| | 45 | * this for items like diagnostics that you want ran during disabled, |
| | 46 | * autonomous, teleoperated and test. |
| | 47 | * |
| | 48 | * <p>This runs after the mode specific periodic functions, but before |
| | 49 | * LiveWindow and SmartDashboard integrated updating. |
| | 50 | */ |
| | 51 | @Override |
| | 52 | public void robotPeriodic() { |
| | 53 | // Encoders connected to CAN controller return raw counts |
| | 54 | int l_raw = leftMotor.getSelectedSensorPosition(); |
| | 55 | int r_raw = rightMotor.getSelectedSensorPosition(); |
| | 56 | // display distances on smart dashboard |
| | 57 | SmartDashboard.putNumber("Left", raw2inches(l_raw, 360*2, 7.5)); |
| | 58 | SmartDashboard.putNumber("Right", raw2inches(r_raw, 360*2, 7.5)); |
| | 59 | } |
| | 60 | |
| | 61 | double raw2inches(int raw_count, int counts_per_rotation, double wheel_diameter) { |
| | 62 | return (raw_count / counts_per_rotation) * (3.141492 * wheel_diameter); |
| | 63 | } |
| | 64 | |
| | 65 | /** |
| | 66 | * This code runs when you enable autonomous mode |
| | 67 | */ |
| | 68 | @Override |
| | 69 | public void autonomousInit() { |
| | 70 | // Reset encoder counts to 0 |
| | 71 | leftMotor.setSelectedSensorPosition(0); |
| | 72 | rightMotor.setSelectedSensorPosition(0); |
| | 73 | // start motors turning forward at 20% power |
| | 74 | leftMotor.set(0.20); |
| | 75 | // to go forward, left motor turns clockwise, right motor counter-clockwise |
| | 76 | rightMotor.set(-0.20); |
| | 77 | } |
| | 78 | |
| | 79 | /** |
| | 80 | * This function is called periodically during autonomous. |
| | 81 | */ |
| | 82 | @Override |
| | 83 | public void autonomousPeriodic() { |
| | 84 | int l_raw = leftMotor.getSelectedSensorPosition(); |
| | 85 | int r_raw = rightMotor.getSelectedSensorPosition(); |
| | 86 | if ((raw2inches(l_raw, 360*2, 7.5) > 36.0) || |
| | 87 | (raw2inches(r_raw, 360*2, 7.5) > 36.0)) { |
| | 88 | leftMotor.stopMotor(); |
| | 89 | rightMotor.stopMotor(); |
| | 90 | } |
| | 91 | } |
| | 92 | |
| | 93 | /** |
| | 94 | * This function is called periodically during operator control. |
| | 95 | */ |
| | 96 | @Override |
| | 97 | public void teleopPeriodic() { |
| | 98 | } |
| | 99 | |
| | 100 | /** |
| | 101 | * This function is called periodically during test mode. |
| | 102 | */ |
| | 103 | @Override |
| | 104 | public void testPeriodic() { |
| | 105 | } |
| | 106 | } |
| | 107 | |
