Update Robot.java

WPILib-License.md

@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/main/java/frc/robot/Constants.java

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+    /** Organizes all the non-changing values we use in the code */
+    public static final int leftFrontPort = 10;
+    public static final int leftBackPort = 11;
+    public static final int rightFrontPort = 12;
+    public static final int rightBackPort = 13;
+
+}

src/main/java/frc/robot/Main.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

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

@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.DriveTrain;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  /** You can leave this in if you want, it only changes anything if you are using the autonomous part of the code, 
+   * if you do remove it, just delete any errors it creates */
+  private Command m_autonomousCommand;
+
+  /**Creates an object for our DriveTrain class so we can call things from the class
+   * make sure to include any variables needed for the parameters
+   */
+  public static DriveTrain m_driveTrain = new DriveTrain(Constants.leftFrontPort, Constants.leftBackPort, Constants.rightFrontPort, Constants.rightBackPort);
+  /** Similar to the previous line, creates a RobotContainer object so we can call things from the robotcontainer class, 
+   * what you name this is up to you  */
+  private RobotContainer m_robotContainer;
+
+
+  /** The values that we will use from the joystick */
+  double xAxis;
+  double yAxis;
+  /** Values that will go in the parameters that are needed for the RobotDrive method */
+  double leftSpeed;
+  double rightSpeed;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * 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>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {
+    /** Defining the XAxis and yAxis objects as the x and y values from our joysticks on the controller */
+    xAxis =   -m_robotContainer.xboxController.getRawAxis(1);
+    yAxis = m_robotContainer.xboxController.getRawAxis(4);
+    /** The math required to make the robot turn left and right properly according to the joystick configuration. */
+    leftSpeed =   yAxis + xAxis;
+    rightSpeed = yAxis - xAxis;
+    /** Calling back the robotDrive method from the DriveTrain class so that it runs fifty times a second
+     * uses the parameters leftSpeed and rightSpeed that we just defined
+     */
+    m_driveTrain.robotDrive(leftSpeed, rightSpeed);
+
+  }
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

src/main/java/frc/robot/RobotContainer.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems and commands are defined here...
+  /** Declaring and defining our joystick object */
+  public static Joystick xboxController = new Joystick(1);
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {}
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+
+}

src/main/java/frc/robot/subsystems/DriveTrain.java

@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+/** YOU DO NOT HAVE TO WORRY ABOUT THIS SECTION, USING TAB WILL ADD ANYTHING NEEDED HERE AUTOMATICALLY */
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveTrain extends SubsystemBase {
+  /** Creates a new DriveTrain. */
+  /** Creates a differential drive which allows the two speed controller groups to work together */
+  DifferentialDrive diffDrive;
+  /**Creates a speed controller group which allows you to run multiple motors with one object */
+  SpeedControllerGroup leftSide;
+  SpeedControllerGroup rightSide;
+  /**Creates the objects for each motor controller */
+  CANSparkMax leftFront;
+  CANSparkMax leftBack;
+  CANSparkMax rightFront;
+  CANSparkMax rightBack;
+
+
+  public DriveTrain(int leftFrontPort, int leftBackPort, int rightFrontPort, int rightBackPort) {
+    /** Defining all the motor controllers with the CANSparkMax(port, motor type) method */
+    leftFront = new CANSparkMax(leftFrontPort, MotorType.kBrushless);
+    leftBack = new CANSparkMax(leftBackPort, MotorType.kBrushless);
+    rightBack = new CANSparkMax(rightBackPort, MotorType.kBrushless);
+    rightFront = new CANSparkMax(rightFrontPort, MotorType.kBrushless);
+    /**Defining the two speed controller groups using the SpeedControllerGroup(motor, motor) method */
+    leftSide = new SpeedControllerGroup(leftFront, leftBack);
+    rightSide = new SpeedControllerGroup(rightFront, rightBack);
+
+    /**Defining the differential drive object using the DifferentialDrive(speedcontrollergroup, speedcontrollergroup) method */
+    diffDrive = new DifferentialDrive(leftSide, rightSide);
+  }
+
+  /**Method we will use to actually control the robot, which need the parameters leftSpeed and rightSpeed, which will be defined in robot */
+  public void robotDrive(double leftSpeed, double rightSpeed){
+    /** A method tied to the DifferentialDrive object in the Wpilib libraries, basically lets us control the motors in a tank drive fashion  */
+    diffDrive.tankDrive(leftSpeed, rightSpeed);
+    /** Creates a minimum required output for the joystick to run the motor */
+    diffDrive.setDeadband(0.1);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run (50 times a second)
+  }
+}

src/main/java/org/team696/robot/Constants.java

@@ -57,8 +57,8 @@ public static final class SpindexerConstants {
     public static final double KickMotorSpeed = 1.;
     public static final double KickMotorReverseSpeed = -0.1;
 
-    public static final double loadingDrumPower = 0.22;
-    public static final double continuousShootDrumPower = 0.55;
+    public static final double loadingDrumPower = 0.22 ;
+    public static final double continuousShootDrumPower = 0.7;
     public static final double stopDrumPower = 0;
 
     public static final double timeBetweenIndex = 1;
@@ -167,6 +167,8 @@ public static final class OperatorConstants {
     public static final int climberDownButton = 17;
     public static final int colorWheelRotationControl = 18;
     public static final int driveAssistButton = 19;
+    public static final int intakeDownButton = 6;
+    public static final int intakeUpButton = 5;
 
     public static final int shooterManualAxis = 3;
     public static final int turretManualAxis = 2;
@@ -218,12 +220,60 @@ public static final class DrivetrainConstants {
     public static final int slowTasksSpeed = 50;
   }
 
-  public static final class IntakeConstants {
-    public static final int IntakeMotorPort = 50;
+  public static final class IntakeConstants { 
+    public static final double leftIntkP = 0.08;
+    public static final double leftIntkI = 0;
+    public static final double leftIntkD = 1;
+    public static final double leftIntIZone = 0;
+    public static final double leftIntFF = 0;
+    public static final double leftIntMin = -0.5;
+    public static final double leftIntMax = 0.5;
+
+    public static final double rightIntkP = 0.08;
+    public static final double rightIntkI = 0;
+    public static final double rightIntkD = 1;
+    public static final double rightIntIZone = 0;
+    public static final double rightIntFF = 0;
+    public static final double rightIntMin = -0.5;
+    public static final double rightIntMax = 0.5;
+
+    public static final int intakeTimeout = 30;
+    public static final int rollerPIDSlot = 1;
+    public static final double rollerkF = 0.06;
+    public static final double rollerkP = 0.6;
+    public static final double rollerkI = 0.0;
+    public static final double rollerkD = 0.0;
+
+    public static final double intakeDownPosition = 23;
+    public static final double intakeUpPosition = 2;
+    public static final double intakeMidPosition = 13;
+
+    public static final double rollerVelocity = 0.3;
+
+    public static final int leftIntakeMotorPort =  50;
+
+    public static final int rightIntakeMotorPort = 30;
     public static final boolean IntakeInverted = false;
 
-    public static final double intakePower = 0.7;
+    public static final double intakePower = 0.3;
     public static final double outtakePower = -1;
     public static final double stopIntakePower = 0;
+  }
+  public static final class ClimberConstants{
+
+    public static final int climberPIDSlot = 3;
+    public static final int climberTimeout = 30;
+    public static final double climberkF = 0;
+    public static final double climberkP = 0.1;
+    public static final double climberkI = 0;
+    public static final double climberkD = 1.0;
+    public static final double climberPos = 20;
+    public static final double climberServoUnlock = 105;
+    public static final double climberServoLock = 1;
+
+
+
+
+
   }
 }