API tools faq
paste
Advertisement
Guest User

The pro scuff code

a guest
Jun 23rd, 2017
68
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.70 KB | None | 0 0
raw download clone embed print report
  1.  
  2. package org.usfirst.frc.team6725.robot;
  3. import edu.wpi.first.wpilibj.ADXRS450_Gyro;
  4. import edu.wpi.first.wpilibj.AnalogGyro;
  5. import edu.wpi.first.wpilibj.AnalogTrigger;
  6. import edu.wpi.first.wpilibj.CameraServer;
  7. import edu.wpi.first.wpilibj.Compressor;
  8. import edu.wpi.first.wpilibj.DoubleSolenoid;
  9. import edu.wpi.first.wpilibj.IterativeRobot;
  10. import edu.wpi.first.wpilibj.Joystick;
  11. import edu.wpi.first.wpilibj.RobotDrive;
  12. import edu.wpi.first.wpilibj.Timer;
  13. import edu.wpi.first.wpilibj.buttons.Button;
  14. import edu.wpi.first.wpilibj.buttons.JoystickButton;
  15. import edu.wpi.first.wpilibj.command.Command;
  16. import edu.wpi.first.wpilibj.command.Scheduler;
  17. import edu.wpi.first.wpilibj.interfaces.Gyro;
  18. import edu.wpi.first.wpilibj.livewindow.LiveWindow;
  19. import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
  20. import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  21.  
  22. //import java.util.Timer;
  23. //import org.usfirst.frc.team6725.robot.commands.ExampleCommand;
  24.  
  25. //import org.usfirst.frc.team6725.robot.commands.Test;
  26. //import org.usfirst.frc.team6725.robot.commands.climbfalse;
  27. //import org.usfirst.frc.team6725.robot.commands.getXandY;
  28. //import org.usfirst.frc.team6725.robot.commands.stgfjgf;
  29. //import org.usfirst.frc.team6725.robot.commands.stopclimbing;
  30. import org.usfirst.frc.team6725.robot.subsystems.ExampleSubsystem;
  31.  
  32. /**
  33. * The VM is configured to automatically run this class, and to call the
  34. * functions corresponding to each mode, as described in the IterativeRobot
  35. * documentation. If you change the name of this class or the package after
  36. * creating this project, you must also update the manifest file in the resource
  37. * directory.
  38. */
  39. public class Robot extends IterativeRobot {
  40.  
  41. public static final ExampleSubsystem exampleSubsystem = new ExampleSubsystem();
  42.  
  43. public static Boolean RBPressed;
  44. public static OI oi;
  45. public static RobotDrive myRobot = new RobotDrive(0, 1, 2, 3);
  46. public static RobotDrive ropeClimb = new RobotDrive(4,5);
  47. public static Joystick stick = new Joystick(0);
  48. //Joystick leftJoy = new Joystick(1);
  49. //Buttons
  50. public static Button buttonA = new JoystickButton(stick, 1);
  51. public static Button buttonX = new JoystickButton(stick, 3);
  52. public static Button buttonB = new JoystickButton(stick, 2);
  53. public static Button buttonRB = new JoystickButton(stick, 6);
  54. public static Button buttonY = new JoystickButton(stick, 4);
  55. public static Button buttonLB = new JoystickButton(stick, 5);
  56. public static Double yVal;
  57. public static Double ValX;
  58. //public static ADXRS450_Gyro Gyro = new ADXRS450_Gyro();
  59. //public static Double Angle = 0.0;
  60. public static boolean isclimbing=false;
  61. //public static double[] recordx = new double[9000];
  62. ////public static double[] recordy = new double[9000];
  63. public static int i=0;
  64. public static boolean isautodone = false;
  65. public static long t;
  66. public static long endforward;
  67. public static long endback;
  68. public static long topeg;
  69. public static double Kp = 0.03;
  70. private Gyro gyro;
  71. //Pneumatics
  72. public static Compressor comp = new Compressor(0);
  73. public static DoubleSolenoid dSol1 = new DoubleSolenoid(0,1);
  74. //public static AnalogTrigger trigger0 = new AnalogTrigger(0);
  75.  
  76. Command autonomousCommand;
  77. SendableChooser<Command> chooser = new SendableChooser<>();
  78.  
  79. /**
  80. * This function is run when the robot is first started up and should be
  81. * used for any initialization code.
  82. */
  83. @Override
  84. public void robotInit() {
  85. // oi = new OI();
  86. // chooser.addDefault("Default Auto", new ExampleCommand());
  87. // chooser.addObject("My Auto", new MyAutoCommand());
  88. // SmartDashboard.putData("Auto mode", chooser);
  89. CameraServer server = CameraServer.getInstance();
  90. CameraServer server1 = CameraServer.getInstance();
  91. server.startAutomaticCapture("cam" ,0);
  92. server1.startAutomaticCapture("cam1",1);
  93. // myRobot.setSafetyEnabled(false);
  94. //Pneumatics
  95. comp.setClosedLoopControl(true);
  96.  
  97.  
  98. }
  99.  
  100. /**
  101. * This function is called once each time the robot enters Disabled mode.
  102. * You can use it to reset any subsystem information you want to clear when
  103. * the robot is disabled.
  104. */
  105. @Override
  106. public void disabledInit() {
  107.  
  108. }
  109.  
  110.  
  111. // public Robot() {
  112. // gyro = new AnalogGyro(1);
  113. // }
  114.  
  115. @Override
  116. public void disabledPeriodic() {
  117. Scheduler.getInstance().run();
  118. }
  119.  
  120. /**
  121. * This autonomous (along with the chooser code above) shows how to select
  122. * between different autonomous modes using the dashboard. The sendable
  123. * chooser code works with the Java SmartDashboard. If you prefer the
  124. * LabVIEW Dashboard, remove all of the chooser code and uncomment the
  125. * getString code to get the auto name from the text box below the Gyro
  126. *
  127. * You can add additional auto modes by adding additional commands to the
  128. * chooser code above (like the commented example) or additional comparisons
  129. * to the switch structure below with additional strings & commands.
  130. */
  131. @Override
  132. public void autonomousInit() {
  133. t = System.currentTimeMillis();
  134. ////
  135. endforward = t + 4000; // original : 3400;
  136. endback = t + 5200;
  137. topeg = t + 10000;
  138. //autonomousCommand = chooser.getSelected();
  139. //seconds = 0;
  140. /*
  141. * String autoSelected = SmartDashboard.getString("Auto Selector",
  142. * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
  143. * = new MyAutoCommand(); break; case "Default Auto": default:
  144. * autonomousCommand = new ExampleCommand(); break; }
  145. */
  146.  
  147. // schedule the autonomous command (example)
  148. //if (autonomousCommand != null)
  149. //autonomousCommand.start();
  150. gyro = new AnalogGyro(1);
  151. gyro.calibrate();
  152. gyro.reset();
  153.  
  154. }
  155.  
  156. /**
  157. * This function is called periodically during autonomous
  158. */
  159. @Override
  160. public void autonomousPeriodic() {
  161. //if (System.currentTimeMillis() <= endforward) {
  162. //double angle = gyro.getAngle();
  163.  
  164. //myRobot.drive(0.3, 0.0000001);
  165. //}
  166. /*
  167. while (System.currentTimeMillis() <= endforward){
  168.  
  169. myRobot.drive(0.3, 0.0000001);
  170.  
  171. }*/
  172. // if (System.currentTimeMillis() <= endback && System.currentTimeMillis() > endforward){
  173. // myRobot.drive(-0.4, -0.39); // original (-.4 , -0.39)
  174. // }
  175. //
  176. // if (System.currentTimeMillis() >= endback && System.currentTimeMillis() < topeg){
  177. // myRobot.drive(0.3, 0.0000001);
  178. // //gyro.reset();
  179. // //double angle = gyro.getAngle();
  180. // // System.out.println(angle*Kp);
  181. // }
  182. /*
  183. isautodone=true;
  184. if (isautodone==true)
  185. myRobot.drive(0, 0);*/
  186. /*while (System.currentTimeMillis() <= endback & System.currentTimeMillis() > endforward){
  187. myRobot.drive(-0.3, -0.0000001);
  188. }
  189.  
  190. System.exit(0);*/
  191.  
  192. // for(i=0;i<=5000;i++){
  193. // myRobot.drive(0.3, 0.0000001);
  194.  
  195. //myRobot.drive(0.3, 0.0000001);
  196. /*while (seconds < 8){
  197. if(seconds <= 4){
  198. myRobot.drive(0.3, 0.0000001);
  199. seconds ++;
  200. //edu.wpi.first.wpilibj.Timer.delay(1);
  201. } else if (seconds > 4 & seconds < 8){
  202. myRobot.drive(-0.3, 0);
  203. seconds ++;
  204. // edu.wpi.first.wpilibj.Timer.delay(1);
  205. } else if (seconds >= 8){
  206. myRobot.drive(0, 0);
  207. seconds ++;
  208. }
  209. System.gc();
  210. Timer.delay(1);
  211. System.out.println(seconds);
  212. }*/
  213. /*for(int i = 0; i >= 8000; i++){
  214. if (i <= 4000){
  215. myRobot.drive(0.3, 0.0000001);
  216. } else if (i > 4000 & i < 8000){
  217. myRobot.drive(-0.3, 0.0);
  218. } else if (i == 8000){
  219. myRobot.drive(0, 0);
  220. }
  221. edu.wpi.first.wpilibj.Timer.delay(0.001);
  222. }*/
  223.  
  224. /* Timer timeAuto = new Timer();
  225. TimerTask AutoMovement = new TimerTask(){
  226.  
  227. @Override
  228. public void run() {
  229. // TODO Auto-generated method stub
  230. //seconds ++;
  231. if(seconds == 0){
  232. myRobot.drive(0.3, 0.0000001);
  233. seconds ++;
  234. edu.wpi.first.wpilibj.Timer.delay(1000);
  235. } else if (seconds == 1){
  236. myRobot.drive(-0.3, 0);
  237. seconds ++;
  238. edu.wpi.first.wpilibj.Timer.delay(1000);
  239. } else if (seconds > 2){
  240. timeAuto.cancel();
  241. timeAuto.purge();
  242. }
  243. System.gc();
  244. System.out.println(seconds);
  245. //edu.wpi.first.wpilibj.Timer.delay(1);
  246. }
  247. /*
  248. };
  249.  
  250. timeAuto.scheduleAtFixedRate(AutoMovement, 0, 4000);*/
  251.  
  252. gyro.reset();
  253. while (isAutonomous()){
  254. double angle = gyro.getAngle();
  255. myRobot.drive(-1, -angle*Kp);
  256. Timer.delay(0.004);
  257.  
  258.  
  259.  
  260.  
  261. }
  262. myRobot.drive(0.0, 0.0);
  263.  
  264.  
  265.  
  266. }
  267.  
  268. @Override
  269. public void teleopInit() {
  270. myRobot.drive(0, 0);
  271. // This makes sure that the autonomous stops running when
  272. // teleop starts running. If you want the autonomous to
  273. // continue until interrupted by another command, remove
  274. // this line or comment it out.
  275. //if (autonomousCommand != null)
  276. //autonomousCommand.cancel();
  277.  
  278. }
  279.  
  280. /**
  281. * This function is called periodically during operator control
  282. */
  283. @Override
  284. public void teleopPeriodic() {
  285. // Scheduler.getInstance().run();
  286. //buttonY.whileHeld(new Test());
  287. // System.out.println("X: " + stick.getX());
  288. // System.out.println("Y: " + stick.getY());
  289. // buttonLB.whileHeld(new getXandY());
  290. // buttonB.whileHeld(new climbfalse());
  291. //buttonB.whenReleased(new stopclimbing());
  292. // if(Robot.stick.getRawAxis(5)>0){
  293. // Robot.ropeClimb.arcadeDrive(-Robot.stick.getRawAxis(5), 0);
  294. // }
  295.  
  296.  
  297. //Pneumatics - Button A to extend, Button B to retract
  298. if(Robot.stick.getRawButton(1)){
  299. dSol1.set(DoubleSolenoid.Value.kForward);
  300. } else if (Robot.stick.getRawButton(2)){
  301. dSol1.set(DoubleSolenoid.Value.kReverse);
  302. }
  303.  
  304. if(Robot.stick.getRawAxis(3)>0.5){
  305. Robot.ropeClimb.arcadeDrive(-Robot.stick.getRawAxis(3)*1.1, 0);
  306. }
  307. myRobot.arcadeDrive(-stick.getY(), -stick.getX());}
  308.  
  309. /**
  310. * This function is called periodically during test mode
  311. */
  312. @Override
  313. public void testPeriodic() {
  314. LiveWindow.run();
  315.  
  316. }
  317.  
  318.  
  319.  
  320.  
  321.  
  322. }
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!