mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Merge pull request #9 from Team4388/add-falcon-500-motion-magic
Added Drive Train PIDs
This commit is contained in:
Keenan D. Buckley
+1
-1
@@ -1,6 +1,6 @@
|
||||
plugins {
|
||||
id "java"
|
||||
id "edu.wpi.first.GradleRIO" version "2020.1.2"
|
||||
id "edu.wpi.first.GradleRIO" version "2020.2.2"
|
||||
}
|
||||
|
||||
sourceCompatibility = JavaVersion.VERSION_11
|
||||
|
||||
@@ -19,11 +19,50 @@ import frc4388.utility.LEDPatterns;
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class DriveConstants {
|
||||
/* Drive Train IDs */
|
||||
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;
|
||||
public static final int PIGEON_ID = 6;
|
||||
|
||||
/* PID Constants Drive*/
|
||||
public static final int DRIVE_TIMEOUT_MS = 30;
|
||||
public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3);
|
||||
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.0, 0.0, 0.0, 0.1, 0, 1.0);
|
||||
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.3);
|
||||
public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
public static final int DRIVE_CRUISE_VELOCITY = 2000;
|
||||
public static final int DRIVE_ACCELERATION = 1000;
|
||||
|
||||
/* Remote Sensors */
|
||||
public final static int REMOTE_0 = 0;
|
||||
public final static int REMOTE_1 = 1;
|
||||
|
||||
/* PID Indexes */
|
||||
public final static int PID_PRIMARY = 0;
|
||||
public final static int PID_TURN = 1;
|
||||
|
||||
/* PID SLOTS */
|
||||
public final static int SLOT_DISTANCE = 0;
|
||||
public final static int SLOT_VELOCITY = 1;
|
||||
public final static int SLOT_TURNING = 2;
|
||||
public final static int SLOT_MOTION_MAGIC = 3;
|
||||
|
||||
/* Drive Train Characteristics */
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048*2;
|
||||
public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 6;
|
||||
public static final double TICKS_PER_GYRO_REV = 8192;
|
||||
|
||||
/* Ratio Calculation */
|
||||
public static final double TICK_TIME_TO_SECONDS = 0.1;
|
||||
public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
|
||||
public static final double WHEEL_TO_MOTOR_GEAR_RATIO = 1/MOTOR_TO_WHEEL_GEAR_RATIO;
|
||||
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_TO_WHEEL_GEAR_RATIO;
|
||||
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
|
||||
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
|
||||
public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
|
||||
}
|
||||
|
||||
public static final class IntakeConstants {
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class Gains {
|
||||
public double m_kP;
|
||||
public double m_kI;
|
||||
public double m_kD;
|
||||
public double m_kF;
|
||||
public int m_kIzone;
|
||||
public double m_kPeakOutput;
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIzone The zone of the I value.
|
||||
* @param kPeakOutput The peak output setting the motors to run the gains at.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput)
|
||||
{
|
||||
m_kP = kP;
|
||||
m_kI = kI;
|
||||
m_kD = kD;
|
||||
m_kF = kF;
|
||||
m_kIzone = kIzone;
|
||||
m_kPeakOutput = kPeakOutput;
|
||||
}
|
||||
}
|
||||
@@ -98,7 +98,6 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
|
||||
|
||||
// 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
|
||||
@@ -113,7 +112,6 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.commands.DriveStraightAtVelocityPID;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.RunIntakeWithTriggers;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
@@ -64,17 +65,31 @@ public class RobotContainer {
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
// test command to spin the robot while pressing A on the driver controller
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whileHeld(() -> m_robotDrive.driveWithInput(0, 1));
|
||||
//new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
// .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36));
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
||||
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
|
||||
/* PID Test Command */
|
||||
// runs velocity PID while driving straight
|
||||
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
|
||||
.whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500))
|
||||
.whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
|
||||
// resets the yaw of the pigeon
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive));
|
||||
|
||||
//new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
// .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive));
|
||||
// interrupts any running command
|
||||
new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
* @param mode NeutralMode to set motors to
|
||||
|
||||
@@ -0,0 +1,53 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
|
||||
public class DriveStraightAtVelocityPID extends CommandBase {
|
||||
Drive m_drive;
|
||||
double m_targetVel;
|
||||
double m_targetGyro;
|
||||
/**
|
||||
* Creates a new DriveStraightAtVelocityPID.
|
||||
* @param subsystem The drive subsystem
|
||||
* @param targetVel The target velocity for the motors in units
|
||||
*/
|
||||
public DriveStraightAtVelocityPID(Drive subsystem, double targetVel) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_targetVel = targetVel;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
|
||||
public class DriveStraightToPositionPID extends CommandBase {
|
||||
Drive m_drive;
|
||||
double m_targetPos;
|
||||
double m_targetGyro;
|
||||
|
||||
/**
|
||||
* Creates a new DriveToDistancePID.
|
||||
* @param subsystem drive subsystem
|
||||
* @param targetPos distance to travel in inches
|
||||
*/
|
||||
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH;
|
||||
addRequirements(m_drive);
|
||||
SmartDashboard.putNumber("Distance Target Inches", targetPos);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
m_drive.runDriveStraightPositionPID(m_targetPos, m_targetGyro);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_targetPos) < 500){
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
|
||||
public class DriveToDistanceMM extends CommandBase {
|
||||
Drive m_drive;
|
||||
double m_distance;
|
||||
double m_leftTarget;
|
||||
double m_rightTarget;
|
||||
|
||||
/**
|
||||
* Creates a new DriveToDistancePID.
|
||||
* @param subsystem drive subsystem
|
||||
* @param distance distance to travel in inches
|
||||
*/
|
||||
public DriveToDistanceMM(Drive subsystem, double distance) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_distance = distance * DriveConstants.TICKS_PER_INCH;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance;
|
||||
m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance);
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
//m_drive.runMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
|
||||
//m_drive.runMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -7,44 +7,52 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.DemandType;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.FollowerType;
|
||||
import com.ctre.phoenix.motorcontrol.InvertType;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.SensorTerm;
|
||||
import com.ctre.phoenix.motorcontrol.StatusFrame;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
|
||||
|
||||
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.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
|
||||
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.Gains;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class Drive extends ProfiledPIDSubsystem {
|
||||
public class Drive extends SubsystemBase {
|
||||
// Put methods for controlling this subsystem
|
||||
// here. Call these from Commands.
|
||||
|
||||
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 WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
|
||||
public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
|
||||
public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
|
||||
public 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);
|
||||
public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
|
||||
|
||||
SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
|
||||
public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
|
||||
public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
|
||||
public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
|
||||
public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Drive() {
|
||||
/* */
|
||||
super(new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(0, 0)), 0);
|
||||
|
||||
/* factory default values */
|
||||
m_leftFrontMotor.configFactoryDefault();
|
||||
m_rightFrontMotor.configFactoryDefault();
|
||||
@@ -57,17 +65,162 @@ public class Drive extends ProfiledPIDSubsystem {
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
/* set neutral mode */
|
||||
setDriveTrainNeutralMode(NeutralMode.Brake);
|
||||
setDriveTrainNeutralMode(NeutralMode.Coast);
|
||||
|
||||
/* deadbands */
|
||||
m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
|
||||
m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); //Ensures motors run at the same speed
|
||||
|
||||
m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE
|
||||
m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed
|
||||
|
||||
/* flip input so forward becomes back, etc */
|
||||
m_leftFrontMotor.setInverted(false);
|
||||
m_rightFrontMotor.setInverted(false);
|
||||
m_rightFrontMotor.setInverted(true);
|
||||
m_driveTrain.setRightSideInverted(false);
|
||||
m_leftBackMotor.setInverted(InvertType.FollowMaster);
|
||||
m_rightBackMotor.setInverted(InvertType.FollowMaster);
|
||||
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Setup Sensors for WPI_TalonFXs */
|
||||
m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Configure the left Talon's selected sensor as local QuadEncoder */
|
||||
m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/*m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);*/ // Configuration Timeout
|
||||
|
||||
/* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */
|
||||
m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
|
||||
RemoteSensorSource.TalonSRX_SelectedSensor,
|
||||
DriveConstants.REMOTE_0, // Source number [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */
|
||||
m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(),
|
||||
RemoteSensorSource.Pigeon_Yaw,
|
||||
DriveConstants.REMOTE_1,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Setup Sum signal to be used for Distance */
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Diff Signal */
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference,
|
||||
DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
|
||||
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient
|
||||
DriveConstants.PID_PRIMARY, // PID Slot of Source
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
|
||||
DriveConstants.PID_TURN,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
|
||||
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1,
|
||||
DriveConstants.PID_TURN,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
|
||||
m_leftFrontMotor.configSelectedFeedbackCoefficient( 1,
|
||||
DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Set status frame periods to ensure we don't have stale data */
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Smart Dashboard Initial Values */
|
||||
|
||||
/* Set up Chooser */
|
||||
m_chooser.setDefaultOption("Distance PID", m_gainsDistance);
|
||||
//setDriveTrainGains("Distance PID", m_gainsDistance);
|
||||
m_chooser.addOption("Velocity PID", m_gainsVelocity);
|
||||
//setDriveTrainGains("Velocity PID", m_gainsVelocity);
|
||||
m_chooser.addOption("Turning PID", m_gainsTurning);
|
||||
//setDriveTrainGains("Turning PID", m_gainsTurning);
|
||||
m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic);
|
||||
//setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic);
|
||||
Shuffleboard.getTab("PID").add(m_chooser);
|
||||
|
||||
/* Gyro */
|
||||
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
|
||||
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
|
||||
/* Sensor Values */
|
||||
SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
|
||||
SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition());
|
||||
|
||||
/* PID */
|
||||
Gains gains = m_chooser.getSelected();
|
||||
Shuffleboard.getTab("PID").add("P Value Drive", gains.m_kP);
|
||||
Shuffleboard.getTab("PID").add("I Value Drive", gains.m_kI);
|
||||
Shuffleboard.getTab("PID").add("D Value Drive", gains.m_kD);
|
||||
Shuffleboard.getTab("PID").add("F Value Drive", gains.m_kF);
|
||||
|
||||
|
||||
/**
|
||||
* Max out the peak output (for all modes).
|
||||
* However you can limit the output of a given PID object with configClosedLoopPeakOutput().
|
||||
*/
|
||||
m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* 1ms per loop. PID loop can be slowed down if need be.
|
||||
* For example,
|
||||
* - if sensor updates are too slow
|
||||
* - sensor deltas are very small per update, so derivative error never gets large enough to be useful.
|
||||
* - sensor movement is very slow causing the derivative error to be near zero.
|
||||
*/
|
||||
int closedLoopTimeMs = 1;
|
||||
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
/**
|
||||
* configAuxPIDPolarity(boolean invert, int timeoutMs)
|
||||
* false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
|
||||
* true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
|
||||
*/
|
||||
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -76,10 +229,29 @@ public class Drive extends ProfiledPIDSubsystem {
|
||||
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("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
|
||||
SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
|
||||
SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
|
||||
SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent());
|
||||
|
||||
SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
|
||||
SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
|
||||
SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
|
||||
SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
|
||||
SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
|
||||
|
||||
} catch (Exception e) {
|
||||
System.err.println("The operation failed successfully.");
|
||||
System.err.println("Error in the Drive Subsystem");
|
||||
//e.printStackTrace(System.err);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -90,8 +262,55 @@ public class Drive extends ProfiledPIDSubsystem {
|
||||
public void setDriveTrainNeutralMode(NeutralMode mode) {
|
||||
m_leftFrontMotor.setNeutralMode(mode);
|
||||
m_rightFrontMotor.setNeutralMode(mode);
|
||||
m_leftFrontMotor.setNeutralMode(mode);
|
||||
m_rightFrontMotor.setNeutralMode(mode);
|
||||
m_leftBackMotor.setNeutralMode(mode);
|
||||
m_rightBackMotor.setNeutralMode(mode);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initializes the drive train gains kP, kI, kD, and kF
|
||||
* @param slot Either "Distance PID", "Velocity PID", "Motion Magic PID", or "Turning PID"
|
||||
* @param gains A gains object which is the gains that are set for the slot
|
||||
*/
|
||||
public void setDriveTrainGains(String slot, Gains gains){
|
||||
/* Distance */
|
||||
if (slot.equals("Distance PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/* Velocity */
|
||||
if (slot.equals("Velocity PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
/* Turning */
|
||||
if (slot.equals("Turning PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/* Motion Magic */
|
||||
if (slot.equals("Motion Magic PID")) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -101,38 +320,98 @@ public class Drive extends ProfiledPIDSubsystem {
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs a position PID while driving straight (has not been tested)
|
||||
* @param targetPos The position to drive to in units
|
||||
* @param targetGyro The angle to drive at in units
|
||||
*/
|
||||
public void runDriveStraightPositionPID(double targetPos, double targetGyro) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
targetPos *= 2;
|
||||
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs velocity PID while driving straight
|
||||
* @param targetVel The velocity to drive at in units
|
||||
* @param targetGyro The angle to drive at in units
|
||||
*/
|
||||
public void runDriveStraightVelocityPID(double targetVel, double targetGyro) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
targetVel *= 2;
|
||||
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs motion magic PID while driving straight (has not been tested)
|
||||
* @param targetPos The position to drive to in units
|
||||
* @param targetGyro The angle to drive at in units
|
||||
*/
|
||||
public void runMotionMagicPID(double targetPos, double targetGyro){
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs a Turning PID to rotate a to a target angle
|
||||
* @param targetAngle target angle in degrees
|
||||
*/
|
||||
public void runTurningPID(double targetAngle){
|
||||
double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV;
|
||||
|
||||
runDriveStraightVelocityPID(0, targetGyro);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current yaw of the pigeon
|
||||
*/
|
||||
public double getGyroYaw() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[0];
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current pitch of the pigeon
|
||||
*/
|
||||
public double getGyroPitch() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[1];
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current roll of the pigeon
|
||||
*/
|
||||
public double getGyroRoll() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the yaw of the pigeon
|
||||
*/
|
||||
public void resetGyroYaw() {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user