Command group test

.vscode/settings.json

@@ -9,5 +9,11 @@
     "bin/": true,
     ".classpath": true,
     ".project": true
-  }
+  },
+  "wpilib.deployOffline": false,
+
+  "editor.tabSize": 2,
+  "editor.insertSpaces": true,
+  "editor.detectIndentation": false,
+  "editor.formatOnSave": true
 }

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.2.1"
+    id "edu.wpi.first.GradleRIO" version "2019.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11
@@ -46,12 +46,14 @@ repositories {
     maven {
         url "https://raw.githubusercontent.com/momentumfrc/Momentum-Tools/master/mvn-repo"
     }
+    maven { url 'https://nexus.otake.pw/repository/maven-public/' }
 }
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.
 dependencies {
     implementation group:'org.usfirst.frc.team4999', name: 'momentum-utils', version: '1.0'
+    compile 'pw.otake.pseudoresonance:pixy2-java-api:1.3.1'
     compile wpi.deps.wpilib()
     compile wpi.deps.vendor.java()
     nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)

src/main/java/frc/robot/OI.java

@@ -7,6 +7,10 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.buttons.Trigger;
+import frc.robot.triggers.*;
+import frc.robot.commands.hatchpusher.ToggleHatchPusher;
+
 /**
  * This class is the glue that binds the controls on the physical operator
  * interface to the commands and command groups that allow control of the robot.
@@ -39,4 +43,11 @@ public class OI {
   // Start the command when the button is released and let it run the command
   // until it is finished as determined by it's isFinished method.
   // button.whenReleased(new ExampleCommand());
+
+  private Trigger toggleHatchPusher = new BooleanTrigger(Robot.controller::getToggleHatchPusher);
+
+  public OI() {
+    toggleHatchPusher.whenActive(new ToggleHatchPusher());
+  }
+
 }

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

@@ -7,16 +7,29 @@
 
 package frc.robot;
 
+import com.kauailabs.navx.frc.AHRS;
+import com.kauailabs.navx.frc.AHRS.SerialDataType;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 import frc.robot.commands.DriveCommand;
-import frc.robot.commands.HatchPusherCommand;
-import frc.robot.commands.WheelArmComand;
+import frc.robot.commands.lift.LiftCommandGroup;
+import frc.robot.commands.WheelArmCommand;
+import frc.robot.commands.arm.ArmAngleCommand;
 import frc.robot.controls.*;
 import frc.robot.subsystems.*;
+import frc.robot.subsystems.vision.Cameras;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -26,43 +39,72 @@
  * project.
  */
 public class Robot extends TimedRobot {
-  public static Drive drive = new Drive();
+
   public static HatchPusher hatchPusher = new HatchPusher();
   public static WheelArm wheelArm = new WheelArm();
   public static LiftSystem liftSystem = new LiftSystem();
+  public static ArmAngle armAngle = new ArmAngle();
   public static DriveController controller = new F310Controller();
+  public static Drive drive1 = new Drive();
   public static OI m_oi;
 
-  Command m_autonomousCommand;
-  SendableChooser<Command> m_chooser = new SendableChooser<>();
+  public AHRS ahrs;
+
+  public static SpeedControllerGroup left = new SpeedControllerGroup(RobotMap.leftFront, RobotMap.leftBack);
+  public static SpeedControllerGroup right = new SpeedControllerGroup(RobotMap.rightFront, RobotMap.rightBack);
+
+  public static DifferentialDrive drive = new DifferentialDrive(left, right);
+
+  private Command driveCommand = new DriveCommand();
+  private Command armCommand = new WheelArmCommand();
+  private Command liftCommand = new LiftCommandGroup();
+  private Command armAngleCommand = new ArmAngleCommand();
 
   /**
-   * This function is run when the robot is first started up and should be
-   * used for any initialization code.
+   * This function is run when the robot is first started up and should be used
+   * for any initialization code.
    */
   @Override
   public void robotInit() {
     m_oi = new OI();
-    // chooser.addOption("My Auto", new MyAutoCommand());
-    SmartDashboard.putData("Auto mode", m_chooser);
+
+    VideoSource lifeCam = new UsbCamera("Driver Camera", 0);
+    lifeCam.setResolution(640, 480);
+    lifeCam.setFPS(15);
+    CameraServer.getInstance().startAutomaticCapture(lifeCam);
+
+    Cameras.setup(); // Setup and Connection to Pixy2
+    try {
+      ahrs = new AHRS(SerialPort.Port.kMXP);
+    } catch (RuntimeException ex) {
+      DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), false);
+    }
+
   }
 
   /**
-   * 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.
+   * 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.
+   * <p>
+   * This runs after the mode specific periodic functions, but before LiveWindow
+   * and SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
+
+    if (ahrs != null) {
+      SmartDashboard.putData(ahrs);
+    }
+
+    Cameras.run();
   }
 
   /**
-   * This function is called once each time the robot enters Disabled mode.
-   * You can use it to reset any subsystem information you want to clear when
-   * the robot is disabled.
+   * This function is called once each time the robot enters Disabled mode. You
+   * can use it to reset any subsystem information you want to clear when the
+   * robot is disabled.
    */
   @Override
   public void disabledInit() {
@@ -75,30 +117,22 @@ public void disabledPeriodic() {
 
   /**
    * This autonomous (along with the chooser code above) shows how to select
-   * between different autonomous modes using the dashboard. The sendable
-   * chooser code works with the Java SmartDashboard. If you prefer the
-   * LabVIEW Dashboard, remove all of the chooser code and uncomment the
-   * getString code to get the auto name from the text box below the Gyro
+   * between different autonomous modes using the dashboard. The sendable chooser
+   * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+   * remove all of the chooser code and uncomment the getString code to get the
+   * auto name from the text box below the Gyro
    *
-   * <p>You can add additional auto modes by adding additional commands to the
-   * chooser code above (like the commented example) or additional comparisons
-   * to the switch structure below with additional strings & commands.
+   * <p>
+   * You can add additional auto modes by adding additional commands to the
+   * chooser code above (like the commented example) or additional comparisons to
+   * the switch structure below with additional strings & commands.
    */
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_chooser.getSelected();
-
-    /*
-     * String autoSelected = SmartDashboard.getString("Auto Selector",
-     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
-     * = new MyAutoCommand(); break; case "Default Auto": default:
-     * autonomousCommand = new ExampleCommand(); break; }
-     */
-
-    // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.start();
-    }
+    driveCommand.start();
+    armCommand.start();
+    liftCommand.start();
+    armAngleCommand.start();
   }
 
   /**
@@ -111,15 +145,11 @@ 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();
-    new DriveCommand().start();
-    new HatchPusherCommand().start();
-    new WheelArmCommand().start();
+    Scheduler.getInstance().removeAll();
+    driveCommand.start();
+    armCommand.start();
+    liftCommand.start();
+    armAngleCommand.start();
   }
 
   /**
@@ -128,6 +158,9 @@ public void teleopInit() {
   @Override
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+    double move_request = 0; // controller.get
+    double turn_request = 0; // controller.getRawAxis(4);
+    drive.arcadeDrive(move_request, turn_request);
   }
 
   /**

src/main/java/frc/robot/RobotMap.java

@@ -6,48 +6,37 @@
 /*----------------------------------------------------------------------------*/
 
 package frc.robot;
+
 import org.usfirst.frc.team4999.controllers.LogitechF310;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.Spark;
-import edu.wpi.first.wpilibj.VictorSP;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.Encoder;
+
 /**
  * The RobotMap is a mapping from the ports sensors and actuators are wired into
  * to a variable name. This provides flexibility changing wiring, makes checking
  * the wiring easier and significantly reduces the number of magic numbers
  * floating around.
  */
 public class RobotMap {
-  // For example to map the left and right motors, you could define the
-  // following variables to use with your drivetrain subsystem.
-  // public static int leftMotor = 1;
-  // public static int rightMotor = 2;
-
-  // If you are using multiple modules, make sure to define both the port
-  // number and the module. For example you with a rangefinder:
-  // public static int rangefinderPort = 1;
-  // public static int rangefinderModule = 1;
-  public static final Spark leftFront = new Spark(0);// TODO: Set to actual port numbers
-  public static final Spark rightFront = new Spark(1);
-  public static final Spark leftBack = new Spark(2);
-  public static final Spark rightBack = new Spark(3);
-
-  public static final VictorSP wheelArmLeft = new VictorSP(4);;// TODO: Set to actual port numbers
-  public static final VictorSP wheelArmRight = new VictorSP(5);
-  public static final VictorSP wheelArmAngle = new VictorSP(6);
-  public static final VictorSP leftLiftWheel = new VictorSP(7); 
-  public static final VictorSP rightLiftWheel = new VictorSP(8);
-
-  public static final Encoder wheelArmEncoder = new Encoder(0,1,false);//TODO: Fix encoder constructor
-
-  public static final DoubleSolenoid hatchSolenoidTop = new DoubleSolenoid(0,1); // TODO: set to actual solenoid values
-  public static final DoubleSolenoid hatchSolenoidLeft = new DoubleSolenoid(2,3);
-  public static final DoubleSolenoid hatchSolenoidRight = new DoubleSolenoid(4,5); //placeholder values
-  public static final DoubleSolenoid liftSystemBackLeft = new DoubleSolenoid(6,7); // Do we have this many?
-  public static final DoubleSolenoid liftSystemBackRight = new DoubleSolenoid(8,9);
-  public static final DoubleSolenoid liftSystemFrontLeft = new DoubleSolenoid(10,11);
-  public static final DoubleSolenoid liftSystemFrontRight = new DoubleSolenoid(12,13);
-
-  public static final LogitechF310 controller1 = new LogitechF310(0);// TODO: PORT NUMBERS
+  public static final Spark leftFront = new Spark(8);// TODO: Set to actual port numbers
+  public static final Spark leftBack = new Spark(7);
+  public static final Spark rightFront = new Spark(6);
+  public static final Spark rightBack = new Spark(5);
+
+  public static final PWMVictorSPX wheelArmLeft = new PWMVictorSPX(4);// TODO: Set to actual port numbers
+  public static final PWMVictorSPX wheelArmRight = new PWMVictorSPX(1);
+  public static final PWMVictorSPX wheelArmAngle = new PWMVictorSPX(0);
+  public static final PWMVictorSPX leftLiftWheel = new PWMVictorSPX(3);
+  public static final PWMVictorSPX rightLiftWheel = new PWMVictorSPX(2);
+
+  public static final Encoder wheelArmEncoder = new Encoder(0, 1, false);// TODO: Fix encoder constructor
+
+  public static final DoubleSolenoid hatchSolenoidTop = new DoubleSolenoid(0, 3); // TODO: set to actual solenoid values
+  public static final DoubleSolenoid liftSystemBack = new DoubleSolenoid(1, 5);
+  public static final DoubleSolenoid liftSystemFront = new DoubleSolenoid(2, 4);
+
+  public static final LogitechF310 controller1 = new LogitechF310(1);
 }

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

@@ -12,9 +12,12 @@
 import frc.robot.controls.DriveController;
 
 public class DriveCommand extends Command {
+
   private boolean reverse = false;
+
   public DriveCommand() {
-    requires(Robot.drive);
+    super();
+    requires(Robot.drive1);
   }
 
   // Called just before this Command runs the first time
@@ -27,11 +30,14 @@ protected void initialize() {
   protected void execute() {
     DriveController controller = Robot.controller;
     double moveRequest = controller.getMoveRequest();
-    if(controller.getReverseDirection())
+
+    moveRequest = reverse ? -moveRequest : moveRequest;
+
+    if (controller.getReverseDirection())
       reverse = !reverse;
-    if(reverse)
-      moveRequest=-moveRequest;
-    Robot.drive.arcadeDrive(moveRequest, controller.getTurnRequest(), controller.getSpeedLimit());
+
+    // Robot.drive1.arcadeDrive(moveRequest, controller.getTurnRequest());
+    Robot.drive1.bad();
   }
 
   // Make this return true when this Command no longer needs to run execute()
@@ -43,11 +49,13 @@ protected boolean isFinished() {
   // Called once after isFinished returns true
   @Override
   protected void end() {
+    // Robot.drive1.stop();
   }
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
   protected void interrupted() {
+    // Robot.drive1.stop();
   }
 }