Command group test

.vscode/settings.json

@@ -9,5 +9,6 @@
     "bin/": true,
     ".classpath": true,
     ".project": true
-  }
+  },
+  "wpilib.deployOffline": false
 }

build.gradle

@@ -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/Dashboard.java

@@ -0,0 +1,54 @@
+package frc.robot;
+
+//import java.util.HashMap;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.Sendable;
+//import edu.wpi.first.wpilibj.command.Command;
+
+public class Dashboard {
+
+    private static Dashboard instance;
+
+    private boolean shouldDisplayField(String key) {
+        return true;
+    }
+
+    public boolean showDiagnostics;
+
+    public void init(boolean showDiagonistics) {
+        this.showDiagnostics = showDiagonistics;
+    }
+
+    public void putString(boolean isCompetition, String key, String value) {
+        if (shouldDisplayField(key)) {
+            SmartDashboard.putString(key, value);
+        }
+    }
+
+    public void putNumber(boolean isCompetition, String key, double value) {
+        if (shouldDisplayField(key))
+
+        {
+            SmartDashboard.putNumber(key, value);
+        }
+    }
+
+    public void putBoolean(boolean isCompetition, String key, boolean value) {
+        if (shouldDisplayField(key)) {
+            SmartDashboard.putBoolean(key, value);
+        }
+    }
+
+    public void putData(boolean isCompetition, String key, Sendable value) {
+        if (shouldDisplayField(key)) {
+            SmartDashboard.putData(key, value);
+        }
+    }
+
+    public static Dashboard getInstance() {
+        if (instance == null)
+            instance = new Dashboard();
+        return instance;
+    }
+
+}

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

@@ -7,16 +7,33 @@
 
 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.DriverStation;
+import edu.wpi.first.wpilibj.SerialPort;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/*import org.opencv.core.Mat;
+import org.opencv.imgproc.Imgproc;
+
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.CvSource;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.cameraserver.CameraServer;
+*/
 import frc.robot.commands.DriveCommand;
 import frc.robot.commands.HatchPusherCommand;
-import frc.robot.commands.WheelArmComand;
+import frc.robot.commands.LiftCommandGroup;
+import frc.robot.commands.WheelArmCommand;
+import frc.robot.commands.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
@@ -30,8 +47,11 @@ public class Robot extends TimedRobot {
   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 OI m_oi;
+  public int myCounter = 0;
+  AHRS ahrs;
 
   Command m_autonomousCommand;
   SendableChooser<Command> m_chooser = new SendableChooser<>();
@@ -44,7 +64,49 @@ public class Robot extends TimedRobot {
   public void robotInit() {
     m_oi = new OI();
     // chooser.addOption("My Auto", new MyAutoCommand());
-    SmartDashboard.putData("Auto mode", m_chooser);
+    //SmartDashboard.putData("Auto mode", m_chooser);
+
+    /*
+    // hatchCamera = CameraServer.getInstance().startAutomaticCapture("hatch",0);
+    // hatchCamera.setResolution(640, 480);
+     portCamera = CameraServer.getInstance().startAutomaticCapture("port",1);
+     portCamera.setResolution(1280, 960);
+
+    new Thread(()->{
+      UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(0);
+      camera.setResolution(640, 480);
+      CvSink cvSink = CameraServer.getInstance().getVideo();
+      CvSource outputStream = CameraServer.getInstance().putVideo("7042", 640, 480);
+      Mat source = new Mat();
+      Mat output = new Mat();
+      while(!Thread.interrupted()){
+        cvSink.grabFrame(source);
+        Imgproc.cvtColor(source, output, Imgproc.COLOR_BGR2GRAY);
+        outputStream.putFrame(output);
+      }
+    }).start();
+    */
+		Cameras.setup(); // Setup and Connection to Pixy2 and Microsoft Camera
+		SmartDashboard.putNumber("Iteration", myCounter);
+    try {
+			/***********************************************************************
+			 * navX-MXP:
+			 * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
+			 * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
+			 * 
+			 * navX-Micro:
+			 * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
+			 * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
+			 * 
+			 * Multiple navX-model devices on a single robot are supported.
+			 ************************************************************************/
+            //ahrs = new AHRS(SerialPort.Port.kUSB1);
+            ahrs = new AHRS(SerialPort.Port.kMXP, SerialDataType.kProcessedData, (byte)50);
+            ahrs.enableLogging(true);
+        } catch (RuntimeException ex ) {
+            DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
+        }
+
   }
 
   /**
@@ -57,6 +119,86 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+		SmartDashboard.putNumber("Iteration", myCounter);
+            /* Display 6-axis Processed Angle Data                                      */
+            SmartDashboard.putBoolean(  "IMU_Connected",        ahrs.isConnected());
+            SmartDashboard.putBoolean(  "IMU_IsCalibrating",    ahrs.isCalibrating());
+            SmartDashboard.putNumber(   "IMU_Yaw",              ahrs.getYaw());
+            SmartDashboard.putNumber(   "IMU_Pitch",            ahrs.getPitch());
+            SmartDashboard.putNumber(   "IMU_Roll",             ahrs.getRoll());
+
+            /* Display tilt-corrected, Magnetometer-based heading (requires             */
+            /* magnetometer calibration to be useful)                                   */
+
+            SmartDashboard.putNumber(   "IMU_CompassHeading",   ahrs.getCompassHeading());
+
+            /* Display 9-axis Heading (requires magnetometer calibration to be useful)  */
+            SmartDashboard.putNumber(   "IMU_FusedHeading",     ahrs.getFusedHeading());
+
+            /* These functions are compatible w/the WPI Gyro Class, providing a simple  */
+            /* path for upgrading from the Kit-of-Parts gyro to the navx MXP            */
+
+            SmartDashboard.putNumber(   "IMU_TotalYaw",         ahrs.getAngle());
+            SmartDashboard.putNumber(   "IMU_YawRateDPS",       ahrs.getRate());
+
+            /* Display Processed Acceleration Data (Linear Acceleration, Motion Detect) */
+
+            SmartDashboard.putNumber(   "IMU_Accel_X",          ahrs.getWorldLinearAccelX());
+            SmartDashboard.putNumber(   "IMU_Accel_Y",          ahrs.getWorldLinearAccelY());
+            SmartDashboard.putBoolean(  "IMU_IsMoving",         ahrs.isMoving());
+            SmartDashboard.putBoolean(  "IMU_IsRotating",       ahrs.isRotating());
+
+            /* Display estimates of velocity/displacement.  Note that these values are  */
+            /* not expected to be accurate enough for estimating robot position on a    */
+            /* FIRST FRC Robotics Field, due to accelerometer noise and the compounding */
+            /* of these errors due to single (velocity) integration and especially      */
+            /* double (displacement) integration.                                       */
+
+            SmartDashboard.putNumber(   "Velocity_X",           ahrs.getVelocityX());
+            SmartDashboard.putNumber(   "Velocity_Y",           ahrs.getVelocityY());
+            SmartDashboard.putNumber(   "Displacement_X",       ahrs.getDisplacementX());
+            SmartDashboard.putNumber(   "Displacement_Y",       ahrs.getDisplacementY());
+
+            /* Display Raw Gyro/Accelerometer/Magnetometer Values                       */
+            /* NOTE:  These values are not normally necessary, but are made available   */
+            /* for advanced users.  Before using this data, please consider whether     */
+            /* the processed data (see above) will suit your needs.                     */
+
+            SmartDashboard.putNumber(   "RawGyro_X",            ahrs.getRawGyroX());
+            SmartDashboard.putNumber(   "RawGyro_Y",            ahrs.getRawGyroY());
+            SmartDashboard.putNumber(   "RawGyro_Z",            ahrs.getRawGyroZ());
+            SmartDashboard.putNumber(   "RawAccel_X",           ahrs.getRawAccelX());
+            SmartDashboard.putNumber(   "RawAccel_Y",           ahrs.getRawAccelY());
+            SmartDashboard.putNumber(   "RawAccel_Z",           ahrs.getRawAccelZ());
+            SmartDashboard.putNumber(   "RawMag_X",             ahrs.getRawMagX());
+            SmartDashboard.putNumber(   "RawMag_Y",             ahrs.getRawMagY());
+            SmartDashboard.putNumber(   "RawMag_Z",             ahrs.getRawMagZ());
+            SmartDashboard.putNumber(   "IMU_Temp_C",           ahrs.getTempC());
+            SmartDashboard.putNumber(   "IMU_Timestamp",        ahrs.getLastSensorTimestamp());
+
+            /* Omnimount Yaw Axis Information                                           */
+            /* For more info, see http://navx-mxp.kauailabs.com/installation/omnimount  */
+            AHRS.BoardYawAxis yaw_axis = ahrs.getBoardYawAxis();
+            SmartDashboard.putString(   "YawAxisDirection",     yaw_axis.up ? "Up" : "Down" );
+            SmartDashboard.putNumber(   "YawAxis",              yaw_axis.board_axis.getValue() );
+
+            /* Sensor Board Information                                                 */
+            SmartDashboard.putString(   "FirmwareVersion",      ahrs.getFirmwareVersion());
+
+            /* Quaternion Data                                                          */
+            /* Quaternions are fascinating, and are the most compact representation of  */
+            /* orientation data.  All of the Yaw, Pitch and Roll Values can be derived  */
+            /* from the Quaternions.  If interested in motion processing, knowledge of  */
+            /* Quaternions is highly recommended.                                       */
+            SmartDashboard.putNumber(   "QuaternionW",          ahrs.getQuaternionW());
+            SmartDashboard.putNumber(   "QuaternionX",          ahrs.getQuaternionX());
+            SmartDashboard.putNumber(   "QuaternionY",          ahrs.getQuaternionY());
+            SmartDashboard.putNumber(   "QuaternionZ",          ahrs.getQuaternionZ());
+
+            /* Connectivity Debugging Support                                           */
+            SmartDashboard.putNumber(   "IMU_Byte_Count",       ahrs.getByteCount());
+            SmartDashboard.putNumber(   "IMU_Update_Count",     ahrs.getUpdateCount());
+		Cameras.run();
   }
 
   /**
@@ -120,6 +262,8 @@ public void teleopInit() {
     new DriveCommand().start();
     new HatchPusherCommand().start();
     new WheelArmCommand().start();
+    new LiftCommandGroup().start();
+    new ArmAngleCommand().start();
   }
 
   /**

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

@@ -10,6 +10,7 @@
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.Talon;
 import edu.wpi.first.wpilibj.VictorSP;
 import edu.wpi.first.wpilibj.Encoder;
 /**
@@ -28,26 +29,22 @@ public class RobotMap {
   // 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 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 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 VictorSP wheelArmLeft = new VictorSP(4);// TODO: Set to actual port numbers
+  public static final VictorSP wheelArmRight = new VictorSP(1);
+  public static final VictorSP wheelArmAngle = new VictorSP(0);
+  public static final VictorSP leftLiftWheel = new VictorSP(3); 
+  public static final VictorSP rightLiftWheel = new VictorSP(2);
 
   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 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/ArmAngleCommand.java

@@ -8,11 +8,12 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.Robot;
+import frc.robot.controls.DriveController;
 
 public class ArmAngleCommand extends Command {
   public ArmAngleCommand() {
-    // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
+    requires(Robot.armAngle);
   }
 
   // Called just before this Command runs the first time
@@ -23,6 +24,7 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    Robot.armAngle.setSpeed(Robot.controller.getArmsSpeed());
   }
 
   // Make this return true when this Command no longer needs to run execute()