From 8e643c1eef159f785b3d1cf22e3688df65679e13 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 10 Jan 2020 21:29:45 -0700 Subject: [PATCH 1/3] Changed extension for drive subsystem Added some new parts to motion profiling, not complete shell for code yet --- .../java/frc4388/robot/subsystems/Drive.java | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 4a369d9..66729cd 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -13,14 +13,18 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.sensors.PigeonIMU; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import frc4388.robot.Constants.DriveConstants; /** * Add your docs here. */ -public class Drive extends SubsystemBase { +public class Drive extends ProfiledPIDSubsystem { // Put methods for controlling this subsystem // here. Call these from Commands. @@ -35,7 +39,10 @@ public class Drive extends SubsystemBase { /** * Add your docs here. */ - public Drive(){ + public Drive() { + /* */ + super(new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(0, 0)), 0); + /* factory default values */ m_leftFrontMotor.configFactoryDefault(); m_rightFrontMotor.configFactoryDefault(); @@ -78,7 +85,7 @@ public class Drive extends SubsystemBase { m_pigeon.getYawPitchRoll(ypr); return ypr[1]; } - + public double getGyroRoll() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); @@ -89,4 +96,16 @@ public class Drive extends SubsystemBase { m_pigeon.setYaw(0); m_pigeon.setAccumZAngle(0); } + + @Override + protected void useOutput(double output, TrapezoidProfile.State setpoint) { + // TODO Auto-generated method stub + + } + + @Override + protected double getMeasurement() { + // TODO Auto-generated method stub + return 0; + } } From 4ea26e8b4ab38633e264e3a50d3657f7a4d12955 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 10 Jan 2020 23:13:59 -0700 Subject: [PATCH 2/3] Switch TalonSRX to TalonFX --- src/main/java/frc4388/robot/subsystems/Drive.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 66729cd..915b7ab 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -9,6 +9,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.sensors.PigeonIMU; @@ -28,10 +29,10 @@ public class Drive extends ProfiledPIDSubsystem { // Put methods for controlling this subsystem // here. Call these from Commands. - public static WPI_TalonSRX m_leftFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); - public static WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); - public static WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); - public static WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); + public static WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); + public static WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); + public static WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); + public static WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); public static DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); From cdd897d3280e194b801e0065b8663e8c726d216f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 11 Jan 2020 11:26:59 -0700 Subject: [PATCH 3/3] Added Velocities to Smart Dashboard Right and Left Front Motor Velocities Added --- src/main/java/frc4388/robot/subsystems/Drive.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index c5640e1..7821c97 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -68,6 +68,8 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity()); } catch (Exception e) { System.err.println("The operation failed successfully."); }