API tools faq
paste
Advertisement
Guest User

Untitled

a guest
May 26th, 2019
103
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.88 KB | None | 0 0
raw download clone embed print report
  1.  
  2. package org.usfirst.frc.team2199.robot;
  3.  
  4. import edu.wpi.first.wpilibj.IterativeRobot;
  5. import edu.wpi.first.wpilibj.Joystick;
  6. import edu.wpi.first.wpilibj.Talon;
  7. import edu.wpi.first.wpilibj.Timer;
  8. import edu.wpi.first.wpilibj.RobotDrive;
  9. import edu.wpi.first.wpilibj.RobotDrive.MotorType;
  10. import edu.wpi.first.wpilibj.Gyro;
  11. import edu.wpi.first.wpilibj.AnalogInput;
  12. //import edu.wpi.first.wpilibj.DriverStationLCD;
  13. import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  14.  
  15.  
  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 IterativeRobot
  20. * documentation. If you change the name of this class or the package after
  21. * creating this project, you must also update the manifest file in the resource
  22. * directory.
  23. */
  24. public class Robot extends IterativeRobot {
  25. Joystick driveJoystick = new Joystick(0);
  26. Joystick manipulatorJoystick = new Joystick(1); //May change based on driver preference
  27. Talon elevatorTalon = new Talon(5);
  28. Talon labeledElevatorTalon = new Talon(4);
  29. RobotDrive robotDrive;
  30. Gyro gyro = new Gyro(0);
  31. // Channels for the wheels
  32. final int frontLeftChannel = 1;
  33. final int rearLeftChannel = 0;
  34. final int frontRightChannel = 2;
  35. final int rearRightChannel = 3;
  36. int autoMode=0;
  37. //int dStationLed=0;
  38. Timer timer = new Timer();
  39. //DriverStationLCD dstation;
  40.  
  41. /**
  42. * This function is run when the robot is first started up and should be
  43. * used for any initialization code.
  44. */
  45. public void robotInit() {
  46. robotDrive = new RobotDrive(frontLeftChannel, rearLeftChannel, frontRightChannel, rearRightChannel);
  47. robotDrive.setInvertedMotor(MotorType.kFrontLeft, true); // invert the left side motors
  48. robotDrive.setInvertedMotor(MotorType.kRearLeft, true);
  49. // you may need to change or remove this to match your robot
  50. //robotDrive.setExpiration(0.1);
  51. gyro.reset();
  52. //dstation = DriverStationLCD.getInstance();
  53.  
  54. }
  55.  
  56. /**
  57. * This function is run when the robot is in a disabled state and is
  58. * used to initialize autonomous mode.
  59. */
  60. public void disabledPeriodic() {
  61.  
  62. if (driveJoystick.getRawButton(1) && (autoMode>=-1 && autoMode<=2))
  63. {
  64. autoModeIncrease();
  65. Timer.delay(.25);
  66. System.out.println("Auto mode is: " + autoMode);
  67. //dstation.println(DriverStationLCD.Line.kUser1,1,"Auto mode is: " + autoMode);
  68. }
  69. else if(driveJoystick.getRawButton(1) &&(autoMode>=3)) {
  70. autoMode=-1;
  71. //dStationLed=-1;
  72. Timer.delay(.25);
  73. System.out.println("Auto mode is: " + autoMode);
  74. }
  75. dStationLed(autoMode);
  76. }
  77.  
  78. /**
  79. * This function is called periodically during autonomous
  80. */
  81. public void autonomousPeriodic() {
  82.  
  83. switch(autoMode){
  84. case 0: //Pick up tote;drive backward
  85. elevatorUp(1.0);
  86. Timer.delay(.7);
  87. elevatorUp(0);
  88. Timer.delay(0.5);
  89. timer.start();
  90. autoMode=7;
  91. break;
  92. case 1: //Pick up container;drive backward
  93. elevatorUp(1.0);
  94. Timer.delay(1.0);
  95. elevatorUp(0);
  96. timer.start();
  97. autoMode=7;
  98. break;
  99. case 2: //Just drive backward
  100. timer.start();
  101. autoMode=7;
  102. break;
  103. case 3: //Pick up container to place on tote
  104. elevatorUp(1.0);
  105. Timer.delay(2.0);
  106. elevatorUp(0);
  107. timer.start();
  108. autoMode=8;
  109. break;
  110. case 6: //Logic to drive forward to hook tote; case 6 should not be executed by itself
  111. while(!timer.hasPeriodPassed(.6)){
  112. //elevatorDown(.1);
  113. double angle=gyro.getAngle();
  114. System.out.println("Gyro angle is " + angle);
  115. robotDrive.mecanumDrive_Cartesian(0.0,-0.2,0.0,-angle);//.00215 seems to give us the straightest drive so far
  116. Timer.delay(.01);
  117. }
  118. elevatorDown(0);
  119. timer.stop();
  120. Timer.delay(.1);
  121. autoMode=0;
  122. //autoMode=-1;
  123. break;
  124. case 7: //Logic to drive backward; case 7 should not be executed by itself
  125. while(!timer.hasPeriodPassed(2.0)){
  126. double angle=gyro.getAngle();
  127. System.out.println("Gyro angle is " + angle);
  128. robotDrive.mecanumDrive_Cartesian(0.0,0.35,-0.01,-angle*.00215);//.00215 seems to give us the straightest drive so far
  129. Timer.delay(.01);
  130. }
  131. timer.stop();
  132. autoMode=-1;
  133. break;
  134. case 8: //Logic to drive sideways left; case 8 should not be executed by itself
  135. while(!timer.hasPeriodPassed(.655)){
  136. double angle=gyro.getAngle();
  137. System.out.println("Gyro angle is " + angle);
  138. robotDrive.mecanumDrive_Cartesian(-0.6,0.05,-0.15,-angle*.00215);//.00215 seems to give us the straightest drive so far
  139. //robotDrive.mecanumDrive_Polar(-0.4, -270.0, -.02);
  140. Timer.delay(.05);
  141. }
  142. timer.stop();
  143. autoMode=9;
  144. //autoMode=-1;
  145. break;
  146. case 9: //Put down container on tote and prepare to pick up tote and move backward; case 9 should not be executed by itself
  147. elevatorDown(.5);
  148. Timer.delay(1.0);
  149. elevatorDown(.8);
  150. Timer.delay(1.95);
  151. elevatorDown(0);
  152. Timer.delay(1.0);
  153. timer.start();
  154. autoMode=6;
  155. break;
  156. default: //Do nothing
  157. robotDrive.stopMotor();
  158. }
  159.  
  160. }
  161.  
  162. public void autonomousInit() {
  163. gyro.reset();
  164. }
  165.  
  166. public void teleopInit() {
  167. gyro.reset();
  168. }
  169.  
  170. /**
  171. * This function is called periodically during operator control
  172. */
  173. public void teleopPeriodic() {
  174. drive(driveJoystick);
  175. if (manipulatorJoystick.getRawAxis(2) >0){
  176. elevatorDown(manipulatorJoystick.getRawAxis(2));
  177. }
  178. else if (manipulatorJoystick.getRawAxis(3) >0){
  179. elevatorUp(manipulatorJoystick.getRawAxis(3));
  180. } else {
  181. elevatorStop();
  182. }
  183.  
  184.  
  185. }
  186.  
  187. /**
  188. * This function is called periodically during test mode
  189. */
  190. public void testPeriodic() {
  191.  
  192. }
  193.  
  194. private void drive(Joystick driveJoystick) {
  195. // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
  196. // This sample does not use field-oriented drive, so the gyro input is set to zero.
  197. if ((driveJoystick.getX() > 0.1 || driveJoystick.getX() <=-.1) || (driveJoystick.getY() >0.1 || driveJoystick.getY() <=-.1)||(driveJoystick.getZ() >.1 ||driveJoystick.getZ() <=-.1)){
  198. robotDrive.mecanumDrive_Cartesian(driveJoystick.getX(), driveJoystick.getY(), driveJoystick.getZ(), 0);
  199.  
  200. } else{
  201. //robotDrive.mecanumDrive_Cartesian(0,0,0,0);
  202. robotDrive.stopMotor();
  203. }
  204.  
  205. Timer.delay(0.005); // wait 5ms to avoid hogging CPU cycles
  206.  
  207. }
  208. private void autoDrive(double speed) {
  209. robotDrive.mecanumDrive_Polar(speed*-1,0.0,0.0);
  210. //Timer.delay(time);
  211. //robotDrive.stopMotor();
  212. }
  213.  
  214. private void elevatorUp(double speed) {
  215. double realSpeed=speed*-1;
  216. elevatorTalon.set(realSpeed);
  217. labeledElevatorTalon.set(realSpeed);
  218. System.out.println("Executing elevatorUp with " + realSpeed);
  219.  
  220.  
  221.  
  222. }
  223.  
  224. private void elevatorDown(double speed) {
  225. elevatorTalon.set(speed);
  226. labeledElevatorTalon.set(speed);
  227. System.out.println("Executing elevatorDown with " + speed);
  228. }
  229.  
  230. private void elevatorStop(){
  231. elevatorTalon.set(0);
  232. labeledElevatorTalon.set(0);
  233. }
  234. private void autoModeIncrease() {
  235. autoMode++;
  236. //dStationLed++;
  237.  
  238. }
  239. private void dStationLed(int dStationLed){
  240. if( dStationLed==0){
  241. SmartDashboard.putBoolean("DB/LED 0", true);
  242. SmartDashboard.putBoolean("DB/LED 1", false);
  243. SmartDashboard.putBoolean("DB/LED 2", false);
  244. SmartDashboard.putBoolean("DB/LED 3", false);
  245. }
  246. else if(dStationLed==1){
  247. SmartDashboard.putBoolean("DB/LED 0", false);
  248. SmartDashboard.putBoolean("DB/LED 1", true);
  249. SmartDashboard.putBoolean("DB/LED 2", false);
  250. SmartDashboard.putBoolean("DB/LED 3", false);
  251. } else if(dStationLed==2){
  252. SmartDashboard.putBoolean("DB/LED 0", false);
  253. SmartDashboard.putBoolean("DB/LED 1", false);
  254. SmartDashboard.putBoolean("DB/LED 2", true);
  255. SmartDashboard.putBoolean("DB/LED 3", false);
  256. } else if(dStationLed==3){
  257. SmartDashboard.putBoolean("DB/LED 0", false);
  258. SmartDashboard.putBoolean("DB/LED 1", false);
  259. SmartDashboard.putBoolean("DB/LED 2", false);
  260. SmartDashboard.putBoolean("DB/LED 3", true);
  261. } else if(dStationLed>3 ||dStationLed<0){
  262. SmartDashboard.putBoolean("DB/LED 0", false);
  263. SmartDashboard.putBoolean("DB/LED 1", false);
  264. SmartDashboard.putBoolean("DB/LED 2", false);
  265. SmartDashboard.putBoolean("DB/LED 3", false);
  266. }
  267. }
  268. }
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!