SHARE
TWEET

Untitled

a guest May 26th, 2019 72 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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. }
RAW Paste Data
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
 
Top