| 61 | | {{{ |
| 62 | | |
| 63 | | package frc.robot; |
| 64 | | |
| 65 | | // Import the classes of objects you will use |
| 66 | | import edu.wpi.first.wpilibj.TimedRobot; |
| 67 | | import edu.wpi.first.wpilibj.XboxController; |
| 68 | | import edu.wpi.first.wpilibj.GenericHID.Hand; |
| 69 | | import edu.wpi.first.wpilibj.Timer; |
| 70 | | import edu.wpi.first.wpilibj.drive.DifferentialDrive; |
| 71 | | import com.ctre.phoenix.motorcontrol.can.*; |
| 72 | | import edu.wpi.first.wpilibj.Ultrasonic; |
| 73 | | import edu.wpi.first.wpilibj.CameraServer; |
| 74 | | |
| 75 | | import edu.wpi.first.wpilibj.livewindow.LiveWindow; |
| 76 | | |
| 77 | | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; |
| 78 | | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; |
| 79 | | |
| 80 | | /** |
| 81 | | * The VM is configured to automatically run this class, and to call the |
| 82 | | * functions corresponding to each mode, as described in the TimedRobot |
| 83 | | * documentation. If you change the name of this class or the package after |
| 84 | | * creating this project, you must also update the build.gradle file in the |
| 85 | | * project. |
| 86 | | */ |
| 87 | | public class Robot extends TimedRobot { |
| 88 | | // Create instances of each object |
| 89 | | private static final String kDefaultAuto = "Default"; |
| 90 | | private static final String kCustomAuto = "My Auto"; |
| 91 | | private String m_autoSelected; |
| 92 | | private final SendableChooser<String> m_chooser = new SendableChooser<>(); |
| 93 | | private XboxController xbox; |
| 94 | | private Timer timer; |
| 95 | | private WPI_TalonSRX m_rearLeft; |
| 96 | | private WPI_TalonSRX m_rearRight; |
| 97 | | private DifferentialDrive m_drive; |
| 98 | | private boolean f_safetyStop; |
| 99 | | private Ultrasonic f_ultrasonic; |
| 100 | | |
| 101 | | /** |
| 102 | | * This function is run when the robot is first started up and should be |
| 103 | | * used for any initialization code. |
| 104 | | */ |
| 105 | | @Override |
| 106 | | public void robotInit() { |
| 107 | | // initialize the objects and connect them to their underlying hardware |
| 108 | | m_chooser.addDefault("Default Auto", kDefaultAuto); |
| 109 | | m_chooser.addObject("My Auto", kCustomAuto); |
| 110 | | SmartDashboard.putData("Auto choices", m_chooser); |
| 111 | | xbox = new XboxController(0); |
| 112 | | timer = new Timer(); |
| 113 | | m_rearLeft = new WPI_TalonSRX(4); |
| 114 | | m_rearRight = new WPI_TalonSRX(3); |
| 115 | | m_drive = new DifferentialDrive(m_rearLeft, m_rearRight); |
| 116 | | f_ultrasonic = new Ultrasonic(1,0); // ping, echo |
| 117 | | f_ultrasonic.setAutomaticMode(true); |
| 118 | | f_ultrasonic.setEnabled(true); |
| 119 | | CameraServer.getInstance().startAutomaticCapture(); |
| 120 | | } |
| 121 | | |
| 122 | | /** |
| 123 | | * This function is called every robot packet, no matter the mode. Use |
| 124 | | * this for items like diagnostics that you want ran during disabled, |
| 125 | | * autonomous, teleoperated and test. |
| 126 | | * |
| 127 | | * <p>This runs after the mode specific periodic functions, but before |
| 128 | | * LiveWindow and SmartDashboard integrated updating. |
| 129 | | */ |
| 130 | | @Override |
| 131 | | public void robotPeriodic() { |
| 132 | | } |
| 133 | | |
| 134 | | /** |
| 135 | | * This autonomous (along with the chooser code above) shows how to select |
| 136 | | * between different autonomous modes using the dashboard. The sendable |
| 137 | | * chooser code works with the Java SmartDashboard. If you prefer the |
| 138 | | * LabVIEW Dashboard, remove all of the chooser code and uncomment the |
| 139 | | * getString line to get the auto name from the text box below the Gyro |
| 140 | | * |
| 141 | | * <p>You can add additional auto modes by adding additional comparisons to |
| 142 | | * the switch structure below with additional strings. If using the |
| 143 | | * SendableChooser make sure to add them to the chooser code above as well. |
| 144 | | */ |
| 145 | | @Override |
| 146 | | public void autonomousInit() { |
| 147 | | m_autoSelected = m_chooser.getSelected(); |
| 148 | | // autoSelected = SmartDashboard.getString("Auto Selector", |
| 149 | | // defaultAuto); |
| 150 | | System.out.println("Auto selected: " + m_autoSelected); |
| 151 | | timer.reset(); |
| 152 | | timer.start(); |
| 153 | | } |
| 154 | | |
| 155 | | /*public void teleopInit() { |
| 156 | | |
| 157 | | }*/ |
| 158 | | |
| 159 | | /** |
| 160 | | * This function is called periodically during autonomous. |
| 161 | | */ |
| 162 | | @Override |
| 163 | | public void autonomousPeriodic() { |
| 164 | | if (timer.get() < 2.0) { |
| 165 | | m_drive.curvatureDrive(0.1, 0.0, false); |
| 166 | | } |
| 167 | | else |
| 168 | | { |
| 169 | | m_drive.curvatureDrive(0.0, 0.0,false); |
| 170 | | } |
| 171 | | switch (m_autoSelected) { |
| 172 | | case kCustomAuto: |
| 173 | | // Put custom auto code here |
| 174 | | break; |
| 175 | | case kDefaultAuto: |
| 176 | | default: |
| 177 | | // Put default auto code here |
| 178 | | break; |
| 179 | | } |
| 180 | | } |
| 181 | | |
| 182 | | /** |
| 183 | | * This function is called periodically during operator control. |
| 184 | | */ |
| 185 | | @Override |
| 186 | | public void teleopPeriodic() { |
| 187 | | |
| 188 | | // Use front-mounted ultrasonic sensor to stop robot |
| 189 | | // if it gets too close to an obstacle |
| 190 | | double f_range = f_ultrasonic.getRangeInches(); |
| 191 | | if (f_ultrasonic.isRangeValid()) { |
| 192 | | if ((f_range < 15.0) && !f_safetyStop) { |
| 193 | | System.out.println("Safety stopped due to front obstacle"); |
| 194 | | f_safetyStop = true; |
| 195 | | } else if (f_range >= 15.0 && f_safetyStop) { |
| 196 | | System.out.println("Resuming..."); |
| 197 | | f_safetyStop = false; |
| 198 | | } |
| 199 | | } |
| 200 | | |
| 201 | | // Use controller joysticks to set drive speed, but |
| 202 | | // safety stop if too close to an obstacle |
| 203 | | double leftSpeed = -0.5*xbox.getY(Hand.kLeft); |
| 204 | | double rightSpeed = -0.5*xbox.getY(Hand.kRight); |
| 205 | | // If there's an obstacle in front of us, don't |
| 206 | | // allow any more forward motion |
| 207 | | if (f_safetyStop && |
| 208 | | (leftSpeed > 0.0) && (rightSpeed > 0.0)) { |
| 209 | | m_drive.stopMotor(); |
| 210 | | } else { |
| 211 | | // otherwise, set motors according to joysticks |
| 212 | | m_drive.tankDrive(leftSpeed, rightSpeed); |
| 213 | | } |
| 214 | | Timer.delay(0.01); |
| 215 | | } |
| 216 | | |
| 217 | | /** |
| 218 | | * This function is called periodically during test mode. |
| 219 | | */ |
| 220 | | @Override |
| 221 | | public void testPeriodic() { |
| 222 | | // LiveWindow.run(); |
| 223 | | } |
| 224 | | } |
| 225 | | |
| 226 | | }}} |
| 227 | | |
