From 7a19b683f6652c04c8f58e137e87b2666d21af2c Mon Sep 17 00:00:00 2001 From: HFocus Date: Wed, 7 Aug 2019 18:48:49 -0600 Subject: [PATCH] Setup Motors and Controllers --- src/main/java/frc4388/robot/OI.java | 35 ++++++++ src/main/java/frc4388/robot/RobotMap.java | 13 +++ .../java/frc4388/robot/subsystems/Drive.java | 46 ++++++++++ vendordeps/Phoenix.json | 87 +++++++++++++++++++ 4 files changed, 181 insertions(+) create mode 100644 vendordeps/Phoenix.json diff --git a/src/main/java/frc4388/robot/OI.java b/src/main/java/frc4388/robot/OI.java index 9c8ca3e..828c8a2 100644 --- a/src/main/java/frc4388/robot/OI.java +++ b/src/main/java/frc4388/robot/OI.java @@ -7,6 +7,10 @@ package frc4388.robot; +import edu.wpi.first.wpilibj.Joystick; +import frc4388.utility.controller.IHandController; +import frc4388.utility.controller.XboxController; + /** * 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,35 @@ 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 static OI instance; + + private static XboxController m_driverXbox; + private static XboxController m_operatorXbox; + + public OI() { + m_driverXbox = new XboxController(RobotMap.XBOX_DRIVER_ID); + m_operatorXbox = new XboxController(RobotMap.XBOX_OPERATOR_ID); + } + + public static OI getInstance() { + if(instance == null) { + instance = new OI(); + } + return instance; + } + + public IHandController getDriverController() { + return m_driverXbox; + } + + public IHandController getOperatorController() + { + return m_operatorXbox; + } + + public Joystick getOperatorJoystick() + { + return m_operatorXbox.getJoyStick(); + } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 7feb605..fe7fe7e 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,6 +7,8 @@ package frc4388.robot; +import frc4388.utility.controller.XboxController; + /** * 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 @@ -24,5 +26,16 @@ public class RobotMap { // public static int rangefinderPort = 1; // public static int rangefinderModule = 1; + /* Xbox Controllers */ + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + + /* Blinkin LED Strip */ public static final int LED_SPARK_ID = 0; + + /* Drive Train */ + public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; + public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4; + public static final int DRIVE_LEFT_BACK_CAN_ID = 3; + public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3627fe9..d6be580 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -7,7 +7,18 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.Faults; +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import frc4388.robot.RobotMap; +import frc4388.robot.Robot; +import frc4388.utility.controller.XboxController; /** * Add your docs here. @@ -16,6 +27,41 @@ public class Drive extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. + WPI_TalonSRX m_leftFrontMotor = new WPI_TalonSRX(RobotMap.DRIVE_LEFT_FRONT_CAN_ID); + WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(RobotMap.DRIVE_RIGHT_FRONT_CAN_ID); + WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(RobotMap.DRIVE_LEFT_BACK_CAN_ID); + WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(RobotMap.DRIVE_RIGHT_BACK_CAN_ID); + + DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + + public Drive(){ + /* factory default values */ + m_leftFrontMotor.configFactoryDefault(); + m_rightFrontMotor.configFactoryDefault(); + m_leftBackMotor.configFactoryDefault(); + m_rightBackMotor.configFactoryDefault(); + + /* set back motors as followers */ + m_leftBackMotor.follow(m_leftFrontMotor); + m_rightBackMotor.follow(m_rightFrontMotor); + + /* set neutral mode */ + m_leftFrontMotor.setNeutralMode(NeutralMode.Brake); + m_rightFrontMotor.setNeutralMode(NeutralMode.Brake); + m_leftFrontMotor.setNeutralMode(NeutralMode.Brake); + m_rightFrontMotor.setNeutralMode(NeutralMode.Brake); + + /* flip input so forward becomes back, etc */ + m_leftFrontMotor.setInverted(false); + m_rightFrontMotor.setInverted(false); + m_leftBackMotor.setInverted(InvertType.FollowMaster); + m_rightBackMotor.setInverted(InvertType.FollowMaster); + } + + @Override + public void periodic() { + } + @Override public void initDefaultCommand() { // Set the default command for a subsystem here. diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..d4da1ce --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,87 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.12.0", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.12.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.12.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.12.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.12.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.12.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.12.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.12.0", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers" + } + ] +} \ No newline at end of file