Skip to content

Commit

Permalink
Made the changes which weren't being made
Browse files Browse the repository at this point in the history
  • Loading branch information
@jordan-powers
jordan-powers committed Feb 24, 2019
1 parent f21965d commit 6ccf665
Show file tree
Hide file tree
Showing 28 changed files with 430 additions and 753 deletions.
7 changes: 6 additions & 1 deletion .vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,10 @@
".classpath": true,
".project": true
},
"wpilib.deployOffline": false
"wpilib.deployOffline": false,

"editor.tabSize": 2,
"editor.insertSpaces": true,
"editor.detectIndentation": false,
"editor.formatOnSave": true
}
54 changes: 0 additions & 54 deletions src/main/java/frc/robot/Dashboard.java
View file

This file was deleted.

11 changes: 11 additions & 0 deletions src/main/java/frc/robot/OI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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());
}

}
226 changes: 52 additions & 174 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,24 +13,18 @@
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
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.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.LiftCommandGroup;
import frc.robot.commands.lift.LiftCommandGroup;
import frc.robot.commands.WheelArmCommand;
import frc.robot.commands.ArmAngleCommand;
import frc.robot.commands.arm.ArmAngleCommand;
import frc.robot.controls.*;
import frc.robot.subsystems.*;
import frc.robot.subsystems.vision.Cameras;
Expand All @@ -50,161 +44,59 @@ public class Robot extends TimedRobot {
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<>();
public AHRS ahrs;

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);

/*
// hatchCamera = CameraServer.getInstance().startAutomaticCapture("hatch",0);
// hatchCamera.setResolution(640, 480);
portCamera = CameraServer.getInstance().startAutomaticCapture("port",1);
portCamera.setResolution(1280, 960);
VideoSource lifeCam = new UsbCamera("Driver Camera", 0);
lifeCam.setResolution(640, 480);
lifeCam.setFPS(15);
CameraServer.getInstance().startAutomaticCapture(lifeCam);

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);
Cameras.setup(); // Setup and Connection to Pixy2
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);
}
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() {
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());
if (ahrs != null) {
SmartDashboard.putData(ahrs);
}

/* 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();
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() {
Expand All @@ -217,30 +109,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();
}

/**
Expand All @@ -253,17 +137,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();
new LiftCommandGroup().start();
new ArmAngleCommand().start();
Scheduler.getInstance().removeAll();
driveCommand.start();
armCommand.start();
liftCommand.start();
armAngleCommand.start();
}

/**
Expand Down
Loading

0 comments on commit 6ccf665

Please sign in to comment.