Vision code

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

@@ -36,12 +36,29 @@ public final class Constants {
     public final static int SLOT_1 = 1;
     public final static int SLOT_2 = 2;
     public final static int SLOT_3 = 3;
-
+    public final static int VELOCITY_CONTROL=1;
+    public final static int HEADING_CONTROL=2;
+
+    public final static double kP = 0.000600;
+    public final static double kI = 1e-6;
+    public final static double  kD = .000013;
+    public final static double kIz = 0;
+    public final static double kFF = 0.000000;
+    public final static double kMaxOutput = 1;
+    public final static double kMinOutput = -1;
 
     public final static int kSlot_Distance = SLOT_0;
     public final static int kSlot_Turning = SLOT_1;
     public final static int kSlot_Velocity = SLOT_2;
     public final static int kSlot_MotProf = SLOT_3;
+
+    public final static int LL_LIGHT_DEFAULT = 0;
+    public final static int LL_LIGHT_OFF = 1;
+    public final static int LL_LIGHT_BLINK = 2;
+    public final static int LL_LIGHT_ON = 3;
+
 
     public static boolean onOrOff = false;
+
 }
+

src/main/java/frc/robot/Path.java

@@ -2,39 +2,42 @@
 import java.io.File;
 import java.io.IOException;
 import java.util.ArrayList;
-import java.util.List;
 import java.util.Scanner; 
 import java.util.logging.*;
 
 public class Path{
-    private ArrayList rightString = new ArrayList<>();
-    private ArrayList leftString = new ArrayList<>();
+    private ArrayList<String> m_rightString = new ArrayList<String>();
+    private ArrayList<String> m_leftString = new ArrayList<String>();
     private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
-    public Path(){}    
+    private String pathName;
+    public Path(String PathName){
+        pathName = PathName;
+    }    
 
-    public ArrayList returnLeftList() throws IOException{
-            File file = new File("/paths/StraightTen.left.pf1.csv");
+    public ArrayList<String> returnLeftList() throws IOException{
+            File file = new File("/home/lvuser/deploy/output/" + pathName + ".left.pf1.csv");
             Scanner sc = new Scanner(file);
             String[] testArray;
 
         sc.next();
         while(sc.hasNext()){
             testArray = sc.next().split(",");
-            leftString.add(testArray[4]);
+            m_leftString.add(testArray[4]);
         }
-
-        return leftString;
+        sc.close();
+        return m_leftString;
     }
 
-    public ArrayList returnRightList() throws IOException{
-        File file = new File("/paths/StraightTen.right.pf1.csv");
+    public ArrayList<String> returnRightList() throws IOException{
+        File file = new File("/home/lvuser/deploy/output/" + pathName + ".right.pf1.csv");
         Scanner sc = new Scanner(file);
         String[] testArray;
         sc.next();
         while(sc.hasNext()){
             testArray = sc.next().split(",");
-            rightString.add(testArray[4]);
+            m_rightString.add(testArray[4]);
         }
-        return rightString;
+        sc.close();
+        return m_rightString;
     }
 }

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

@@ -7,12 +7,12 @@
 
 package frc.robot;
 
+import java.util.logging.Logger;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.ScheduleCommand;
-import frc.robot.commands.DriveCommand;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -22,18 +22,21 @@
  */
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
-
-  private RobotContainer m_robotContainer;
+  private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
+  public static RobotContainer m_robotContainer;
 
   /**
    * 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();
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
   }
 
   /**
@@ -45,9 +48,15 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
-    SmartDashboard.putNumber("Left Velocity", -1 * m_robotContainer.mDriveSubsystem.leftVelocity());    
-    SmartDashboard.putNumber("Right Velocity", -1 * m_robotContainer.mDriveSubsystem.rightVelocity());
-    m_robotContainer.mDriveSubsystem.updatePID();
+    SmartDashboard.putNumber("Left Velocity", -1 * RobotContainer.mDriveSubsystem.leftVelocity());    
+    SmartDashboard.putNumber("Right Velocity", -1 * RobotContainer.mDriveSubsystem.rightVelocity());
+    RobotContainer.mDriveSubsystem.updatePID();
+    double x = RobotContainer.tx.getDouble(0.0);
+    double y = RobotContainer.ty.getDouble(0.0);
+    double area = RobotContainer.ta.getDouble(0.0);
+    SmartDashboard.putNumber("LimelightX", x);
+    SmartDashboard.putNumber("LimelightY", y);
+    SmartDashboard.putNumber("LimelightArea", area);
     // Runs the Smcheduler.  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
@@ -84,6 +93,7 @@ public void autonomousInit() {
    */
   @Override
   public void autonomousPeriodic() {
+    CommandScheduler.getInstance().run();
   }
 
   @Override
@@ -95,7 +105,6 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    m_robotContainer.mDriveCommand.schedule();
   }
 
   /**

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

@@ -11,11 +11,20 @@
 import java.util.ArrayList;
 import java.util.logging.Logger;
 
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.Aiming;
 import frc.robot.commands.DriveCommand;
 import frc.robot.commands.RunPath;
-import frc.robot.commands.RunVelocity;
+import frc.robot.commands.RunPathBack;
+import frc.robot.commands.TurnAimShoot;
+import frc.robot.commands.autoDeCorrect;
+import frc.robot.commands.autoStoreValue;
 import frc.robot.commands.toggleOff;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.Path;
@@ -32,23 +41,35 @@ public class RobotContainer {
   public static DriveSubsystem mDriveSubsystem = new DriveSubsystem();
   public static DriveCommand mDriveCommand = new DriveCommand();
   private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
-  public static Path path = new Path();
-  public static ArrayList leftArray;
-  public static ArrayList rightArray;
+  private static Path path1 = new Path("trenchAndAim");
+  private static Path path2 = new Path("DriveTrench");
+  private static ArrayList<String> leftArray1;
+  private static ArrayList<String>  rightArray1;
+  private static ArrayList<String>  leftArray2;
+  private static ArrayList<String>  rightArray2;
+  public static NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+  public static NetworkTableEntry tx = table.getEntry("tx");
+  public static NetworkTableEntry ty = table.getEntry("ty");
+  public static NetworkTableEntry ta = table.getEntry("ta");
+  public static NetworkTableEntry light = table.getEntry("ledMode");
+
 
   /**
    * The container for the robot.  Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
     // Configure the button bindings
     try{
-      rightArray = path.returnRightList();
-      leftArray = path.returnLeftList();
+      rightArray1 = path1.returnRightList();
+      leftArray1 = path1.returnLeftList();
+      rightArray2 = path2.returnRightList();
+      leftArray2 = path2.returnLeftList();
     }
       catch(IOException e){
         LOGGER.warning("real really test");
       }
      configureButtonBindings();
+     mDriveSubsystem.setDefaultCommand(mDriveCommand);
    }
 
   /**
@@ -58,8 +79,9 @@ public RobotContainer() {
    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
-    mOI.buttonOne.whenPressed(new RunPath(leftArray, rightArray));
-    mOI.buttonTwo.whenPressed(new toggleOff());
+    mOI.buttonOne.whenPressed(new autoStoreValue());
+    mOI.buttonFive.whileHeld(new Aiming(), false);
+    mOI.buttonSix.whenPressed(new SequentialCommandGroup(new RunPath(leftArray1, rightArray1), new autoStoreValue(), new TurnAimShoot(), new autoDeCorrect(), new RunPath(leftArray2, rightArray2), new RunPathBack(leftArray2, rightArray2)));
   }
 
 
@@ -68,8 +90,8 @@ private void configureButtonBindings() {
    *
    * @return the command to run in autonomous
    */
-  //public Command getAutonomousCommand() {
+  public Command getAutonomousCommand() {
     // An ExampleCommand will run in autonomous
-  //  return m_autoCommand;
-  //}
+    return new RunPath(leftArray2, rightArray2);
+  }
 }

src/main/java/frc/robot/commands/Aiming.java

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import frc.robot.Constants;
+import frc.robot.OI;
+import frc.robot.RobotContainer;
+import frc.robot.*;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class Aiming extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private static final Logger LOGGER = Logger.getLogger(DriveCommand.class.getName());
+  private Joystick joystick = new Joystick(OI.joystick);
+  double tx = SmartDashboard.getNumber("LimelightX", 0);
+  double ty = SmartDashboard.getNumber("LimelightY", 0);
+  double d = 0;
+
+  double minSteerAdjust = .2;
+  double steeringAdjust = 0.0;
+  double headingCommand = 0;
+  double p = .013;
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public Aiming() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mDriveSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  // tx is the degrees the limelight detects we are off by the target 
+  // adjust p until it works
+  @Override
+  public void execute() {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_ON);
+    tx = SmartDashboard.getNumber("LimelightX", 0);
+    ty = SmartDashboard.getNumber("LimelightY", 0);
+    d = 73.5/Math.tan(Math.toRadians(ty+63));
+    SmartDashboard.putNumber("Distance", d);
+    if(tx>1.0){
+        steeringAdjust = p * tx +minSteerAdjust;
+    }
+    else if(tx<-1.0){
+        steeringAdjust = p * tx -minSteerAdjust;
+    }
+
+    if(tx != 0)
+      RobotContainer.mDriveSubsystem.arcadeDrive(joystick.getRawAxis(Constants.VELOCITY_CONTROL),steeringAdjust);
+    else
+      RobotContainer.mDriveSubsystem.arcadeDrive(joystick.getRawAxis(Constants.VELOCITY_CONTROL),joystick.getRawAxis(Constants.HEADING_CONTROL));
+      steeringAdjust = 0;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/DriveCommand.java

@@ -10,8 +10,6 @@
 import frc.robot.Constants;
 import frc.robot.OI;
 import frc.robot.RobotContainer;
-import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.ExampleSubsystem;
 
 import java.util.logging.Logger;