Merge pull request #81 from FRCteam4909/WIP

WIP: Testing from Saturday

Author: ashwinc12

src/main/java/frc/team4909/robot/LIDAR.java

@@ -4,81 +4,133 @@
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Scheduler;
-import frc.team4909.robot.subsystems.drivetrain.Linefollow;
-import frc.team4909.robot.subsystems.intake.CargoIntakeIn;
-import frc.team4909.robot.subsystems.intake.CargoIntakeOut;
-import frc.team4909.robot.subsystems.intake.HatchPanelIntakeOpen;
-import frc.team4909.robot.subsystems.intake.HatchPanelIntakeClose;
 import frc.team4909.robot.operator.controllers.BionicF310;
-// import frc.team4909.robot.sensors.Stream;
+import frc.team4909.robot.sensors.LidarLitePWM;
+import frc.team4909.robot.sensors.Stream;
+import frc.team4909.robot.subsystems.climber.ClimberSubsystem;
+import frc.team4909.robot.subsystems.climber.commands.DriveStiltsBack;
+import frc.team4909.robot.subsystems.climber.commands.DriveStiltsForward;
+import frc.team4909.robot.subsystems.climber.commands.ExtendStilts;
+import frc.team4909.robot.subsystems.climber.commands.RetractStilts;
 import frc.team4909.robot.subsystems.drivetrain.DriveTrainSubsystem;
+import frc.team4909.robot.subsystems.drivetrain.Linefollow;
 import frc.team4909.robot.subsystems.drivetrain.commands.InvertDriveDirection;
+import frc.team4909.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.team4909.robot.subsystems.elevator.commands.SetElevatorPosition;
+import frc.team4909.robot.subsystems.elevatorarm.ElevatorArmSubsystem;
 import frc.team4909.robot.subsystems.intake.IntakeSubsystem;
-import frc.team4909.robot.subsystems.elevator.ElevatorSubsystem;;
+import frc.team4909.robot.subsystems.intake.commands.CargoIntakeIn;
+import frc.team4909.robot.subsystems.intake.commands.CargoIntakeOut;
+import frc.team4909.robot.subsystems.intake.commands.HatchPanelIntakeOpen;
+
+//  Controls:
+//  
+//  Driver Gamepad (Port 0): 
+//     LY: Drive Speed 
+//     RX: Drive Rotation 
+//     LT: Retract Stilts
+//     RT: Extend Stilts 
+//     LB: Drive Stilt Back
+//     RB: Drive Stilt Forward
+//     A: Invert Drive Direction 
+//     B: Line Follow
+//  
+//  Operator Gamped (Port 1): 
+//     RY: Elevator Arm Pivot 
+//     LY: Elevator Speed 
+//     RT: Cargo Intake In 
+//     RB: Cargo Intake Out 
+//     LT: Hatch Panel Intake In 
+
 public class Robot extends TimedRobot {
 
-  // Stream stream = new Stream();
+  // Camera
+  public static Stream stream;
+  // public static GripPipeline grip;
   // Operator Input
   public static BionicF310 driverGamepad;
   public static BionicF310 manipulatorGamepad;
+  public boolean StiltsStop;
 
   // Subsystems
   public static PowerDistributionPanel powerDistributionPanel;
   public static DriveTrainSubsystem drivetrainSubsystem;
   public static IntakeSubsystem intakeSubsystem;
   public static ElevatorSubsystem elevatorSubsystem;
+  public static ElevatorArmSubsystem elevatorArmSubsystem;
+  public static ClimberSubsystem climberSubsystem;
   public static Compressor c;
 
-
   // Sensors
   public static LidarLitePWM lidar;
 
+  // SmartDashboard Buttons
+  public boolean CargoIntake;
+  public boolean CargoOuttake;
+
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
    */
+
   @Override
   public void robotInit() {
 
-    c = new Compressor(0);        //Initialize Compressor
-    c.setClosedLoopControl(true); // Start Compressor in Closed Loop Control
-
+    // Cameras (subsystem)
+    stream = new Stream();
+    // CameraServer.getInstance().startAutomaticCapture();
+    stream.streamCamera();
+    // grip = new GripPipeline();
 
-    // stream.streamCamera();
-    // GripPipeline grip = new GripPipeline();
+    // Compressor
+    c = new Compressor(0); // Initialize Compressor
+    c.setClosedLoopControl(true); // Start Compressor in Closed Loop Control
 
     // Subsystems
     powerDistributionPanel = new PowerDistributionPanel();
     drivetrainSubsystem = new DriveTrainSubsystem();
     intakeSubsystem = new IntakeSubsystem();
     elevatorSubsystem = new ElevatorSubsystem();
+    elevatorArmSubsystem = new ElevatorArmSubsystem();
+    climberSubsystem = new ClimberSubsystem();
 
     // Sensors
-    lidar = new LidarLitePWM(4);
+    lidar = new LidarLitePWM(RobotMap.lidarPort);
 
     // Operator Input
-
-    driverGamepad = new BionicF310(RobotConstants.driverGamepadPort, // Port
-                                   RobotConstants.driverGamepadDeadzone, // Deadzone
-                                   RobotConstants.driverGamepadSensitivity // Gamepad sensitivity
+    driverGamepad = new BionicF310(RobotMap.driverGamepadPort, // Port
+        RobotConstants.driverGamepadDeadzone, // Deadzone
+        RobotConstants.driverGamepadSensitivity // Gamepad sensitivity
     );
 
-    manipulatorGamepad = new  BionicF310(RobotConstants.manipulatorGamepadPort, // Port
-                                         RobotConstants.manipulatorGamepadDeadzone, // Deadzone 
-                                         RobotConstants.manipulatorGamepadSensitivity // Gamepad sensitivity
+    manipulatorGamepad = new BionicF310(RobotMap.manipulatorGamepadPort, // Port
+        RobotConstants.manipulatorGamepadDeadzone, // Deadzone
+        RobotConstants.manipulatorGamepadSensitivity // Gamepad sensitivity
     );
+    /* Drivetrain */
 
-    driverGamepad.buttonPressed(BionicF310.A, new Linefollow());
-    driverGamepad.buttonPressed(BionicF310.X, new CargoIntakeIn());
-    driverGamepad.buttonPressed(BionicF310.Y, new CargoIntakeOut());
-    driverGamepad.buttonPressed(BionicF310.LB, new HatchPanelIntakeOpen());
-    driverGamepad.buttonPressed(BionicF310.RB, new HatchPanelIntakeClose());
-    driverGamepad.buttonPressed(BionicF310.Start, new InvertDriveDirection());
-  }
+    /* Intake */
+    manipulatorGamepad.buttonHeld(BionicF310.RT, 0.2, new CargoIntakeIn());
+    manipulatorGamepad.buttonHeld(BionicF310.RB, new CargoIntakeOut());
+    manipulatorGamepad.buttonHeld(BionicF310.LT, 0.2, new HatchPanelIntakeOpen());
+
+    /* Climber */
+    driverGamepad.buttonHeld(BionicF310.RT, 0.2, new ExtendStilts());
+    driverGamepad.buttonHeld(BionicF310.LT, 0.2, new RetractStilts());
+    driverGamepad.buttonHeld(BionicF310.LB, new DriveStiltsBack());
+    driverGamepad.buttonHeld(BionicF310.RB, new DriveStiltsForward());
+
+    /* Elevator */
+    manipulatorGamepad.buttonPressed(BionicF310.A, new SetElevatorPosition(-13000, 1));
+
+    /* Sensors/Misc. */
+    driverGamepad.buttonPressed(BionicF310.A, new InvertDriveDirection());
+    driverGamepad.buttonPressed(BionicF310.B, new Linefollow());
+   }
 
   /**
-   * This function is called every robot packet, no matter the mode. Use this for
-   * items like diagnostics that you want ran during disabled, autonomous,
+   * '' This function is called every robot packet, no matter the mode. Use this
+   * for items like diagnostics that you want ran during disabled, autonomous,
    * teleoperated and test.
    *
    * <p>
@@ -108,7 +160,7 @@ public void autonomousPeriodic() {
    */
   @Override
   public void teleopPeriodic() {
-    System.out.println("Distance is  " + lidar.getDistance()); // Remove for competition (necessary only for testing)
+
   }
 
   /**
@@ -117,4 +169,4 @@ public void teleopPeriodic() {
   @Override
   public void testPeriodic() {
   }
-}
+}

src/main/java/frc/team4909/robot/Robot.java

@@ -4,52 +4,52 @@
 
 public class RobotConstants {
   /* Operator Input */
-  public static final int driverGamepadPort = 0;
   public static final double driverGamepadSensitivity = 0.6; // Value selected from 2018 code
-  public static final int driverGamepadDeadzone = 0; // Value selected from 2018 code
+  public static final double driverGamepadDeadzone = 0.05; // Value selected from 2018 code
 
-  public static final int manipulatorGamepadPort = 1;
   public static final double manipulatorGamepadSensitivity = 0.6; // Value selected from 2018 code
-  public static final int manipulatorGamepadDeadzone = 0; // Value selected from 2018 code
+  public static final double manipulatorGamepadDeadzone = 0.05; // Value selected from 2018 code
 
   /* Drivetrain */
-  public static final double speedMultiplier = .7;  
+  public static final double speedMultiplier = .7;
+  public static final double speedTurnMultiplier = .4;
+  public static final double speedTurnPreciseMultiplier = .2;
 
   /* Intake */
   public static final double cargoIntakeInSpeed = 1.0;
-  public static final double cargoIntakeHoldSpeed = 0;
+  public static final double cargoIntakeHoldSpeed = 0.0625;
   public static final double cargoIntakeOutSpeed = -1.0;
 
-  public static final double cargoIntakeCurrentLimit = 20;
+ // public static final double cargoIntakeCurrentLimit = 0;
 
   /* LIDAR */
-  public static final int calibrationOffset = -2;
-  public static final double lidarLimit = 20; // cm
+  public static final int calibrationOffset = -9; // calibration constant used for Lidar
+  public static final double lidarLimit = 30; // cm
 
   /*
    * Sensor Threshold derived by testing the minimum voltage readouts when the
    * ball is placed LEFT, RIGHT, and CENTER. This value should then be compared to
    * when there is no cargo to ensure that the values do not overlap. The
    * distinguishing value is then denoted as the treshold.
    */
+
   /* Sensors */
   public static final double irSensorThreshold = 1.7;
 
-
   /* Elevator */
-  public static final int elevatorSRXID = 7;  //Master SRX ID
-  public static final int elevatorSPX1ID = 4; //Slave 1 ID
-  public static final int elevatorSPX2ID = 5; //Slave 2 ID
-  public static final int elevatorSPX3ID = 6; //Slave 3 ID
-
-  public static final double elevatorSpeedMultiplier = 1.0; //Multiplier for elevator speed
-  public static final double elevatorArmSpeedMultiplier = 1.0; //Multiplier for elevator arm speed
-
- 
+  public static final double elevatorSpeedMultiplier = 0.5; // Multiplier for elevator speed
+  public static final double elevatorArmSpeedMultiplier = 0.75; // Multiplier for elevator arm speed
 
   /* Linefollow */
   public static final double fastVelocity = 0.7;
   public static final double mediumVelocity = 0.5; // constant velocity given to both motors
   public static final double slowVelocity = 0.2; // value given to motor when trying to turn
 
+  /* Climber */
+  public static final double climberStiltSpeed = 0.3;
+  public static final double climberDriveSpeed = 0.3;
+  public static final double elevatorClimbSpeed = 0.3;
+
+  /* Timeout */
+  public static final int timeoutMs = 30; // milliseconds
 }

src/main/java/frc/team4909/robot/RobotConstants.java

@@ -14,28 +14,45 @@
  * floating around.
  */
 public class RobotMap {
-  /* The following variables are PWM slots, with the exception of
-   * CAN: CAN Device IDs, need to be configured through respective tool
-   * PDP: PDP Slot Number, corresponds to hardware
-   * IR: Analog Sensor Input
-   * Channel: PCM Solenoid Output
+  /*
+   * The following variables are PWM slots, with the exception of CAN: CAN Device
+   * IDs, need to be configured through respective tool PDP: PDP Slot Number,
+   * corresponds to hardware IR: Analog Sensor Input Channel: PCM Solenoid Output
    */
 
-  /* Drivetrain  */
+  /* Gamepad */
+  public static final int driverGamepadPort = 0;
+  public static final int manipulatorGamepadPort = 1;
+
+  /* Drivetrain */
   public static final int driveFrontLeftSparkMaxCAN = 3;
   public static final int driveRearLeftSparkMaxCAN = 4;
   public static final int driveFrontRightSparkMaxCAN = 1;
   public static final int driveRearRightSparkMaxCAN = 2;
 
   /* Intake */
-  public static final int intakeForwardChannel = 1;
-  public static final int intakeReverseChannel = 2;
-  public static final int intakeMotorCAN = 1;
-  public static final int intakeMotorPDP = 1;
+  public static final int intakePCMChannelL = 0;
+  public static final int intakePCMChannelR= 1;
+  public static final int intakeMotorCAN = 9;
+  public static final int intakeMotorPDP = 4;
   public static final int leftIRSensor = 0;
   public static final int rightIRSensor = 1;
-  
+
+  /* Elevator */
+  public static final int elevatorSRXID = 7; // Master SRX ID Back left
+  public static final int elevatorSPX1ID = 4; // Slave 1 ID FrontLeft ELevator Left
+  public static final int elevatorSPX2ID = 5; // Slave 2 ID Bottom `Elevator Right
+  public static final int elevatorSPX3ID = 6; // Slave 3 ID Back right Elevator right
+
+  public static final int elevatorArmSRXID = 8; // Front
   // If you are using multiple modules, make sure to define both the port
   // number and the module. For example you with a rangefinder:
-  
+
+  /* Climber */
+  public static final int climberSRXID = 10;
+  public static final int climberSPXID = 11;
+
+  /* Sensors */
+  public static final int lidarPort = 4;
+
 }