mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Merge pull request #29 from Team4388/add-ramsete-controller-and-trajectories
Beeg Drive Train Updates
This commit is contained in:
ryan123rudder
@@ -7,6 +7,7 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
@@ -28,13 +29,19 @@ public final class Constants {
|
||||
|
||||
/* 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.1, 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.5);
|
||||
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 = 20000;
|
||||
public static final int DRIVE_ACCELERATION = 7000;
|
||||
public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3);
|
||||
public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.05);
|
||||
public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.5);
|
||||
//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 = 20000;
|
||||
//public static final int DRIVE_ACCELERATION = 7000;
|
||||
|
||||
/* Trajectory Constants */
|
||||
public static final double MAX_SPEED_METERS_PER_SECOND = 3;
|
||||
public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3;
|
||||
public static final double TRACK_WIDTH_METERS = 0.648;
|
||||
public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
|
||||
|
||||
/* Remote Sensors */
|
||||
public final static int REMOTE_0 = 0;
|
||||
public final static int REMOTE_1 = 1;
|
||||
@@ -50,19 +57,21 @@ public final class Constants {
|
||||
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 TICKS_PER_MOTOR_REV = 2048;
|
||||
public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13;
|
||||
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 WHEEL_ROT_PER_MOTOR_ROT = 1/MOTOR_ROT_PER_WHEEL_ROT;
|
||||
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT;
|
||||
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 double INCHES_PER_METER = 39.370;
|
||||
public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
|
||||
}
|
||||
|
||||
public static final class IntakeConstants {
|
||||
|
||||
@@ -7,9 +7,11 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
@@ -34,6 +36,7 @@ public class Robot extends TimedRobot {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
SmartDashboard.putString("Auto?", "NAH");
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -61,6 +64,7 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
|
||||
//m_robotContainer.setDriveGearState(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -75,6 +79,10 @@ public class Robot extends TimedRobot {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
|
||||
m_robotContainer.setDriveGearState(true);
|
||||
m_robotContainer.resetOdometry();
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
/*
|
||||
* String autoSelected = SmartDashboard.getString("Auto Selector",
|
||||
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
|
||||
@@ -85,6 +93,7 @@ public class Robot extends TimedRobot {
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
System.err.println("Auto Start");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -98,6 +107,9 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
|
||||
m_robotContainer.setDriveGearState(true);
|
||||
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
|
||||
|
||||
// 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
|
||||
@@ -105,6 +117,8 @@ public class Robot extends TimedRobot {
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
|
||||
SmartDashboard.putString("Auto?", "NAH");
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -7,19 +7,34 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.controller.RamseteController;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.trajectory.Trajectory;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RamseteCommand;
|
||||
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.DriveStraightToPositionMM;
|
||||
import frc4388.robot.commands.DriveStraightToPositionPID;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
|
||||
import frc4388.robot.commands.DriveWithJoystickDriveStraight;
|
||||
import frc4388.robot.commands.RunClimberWithTriggers;
|
||||
import frc4388.robot.commands.RunExtenderOutIn;
|
||||
import frc4388.robot.commands.RunIntakeWithTriggers;
|
||||
@@ -74,7 +89,7 @@ public class RobotContainer {
|
||||
|
||||
/* Default Commands */
|
||||
// drives the robot with a two-axis input from the driver controller
|
||||
m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController()));
|
||||
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
|
||||
// drives intake with input from triggers on the opperator controller
|
||||
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
|
||||
// drives climber with input from triggers on the opperator controller
|
||||
@@ -99,7 +114,7 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144));
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive));
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
@@ -127,19 +142,19 @@ public class RobotContainer {
|
||||
|
||||
// resets the yaw of the pigeon
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72));
|
||||
.whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive));
|
||||
|
||||
// turn 45 degrees
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive));
|
||||
.whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
|
||||
|
||||
|
||||
// sets solenoids into high gear
|
||||
new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON)
|
||||
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
|
||||
|
||||
// sets solenoids into low gear
|
||||
new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON)
|
||||
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
|
||||
|
||||
// interrupts any running command
|
||||
@@ -155,7 +170,7 @@ public class RobotContainer {
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whileHeld(new TrackTarget(m_robotShooter));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
* @param mode NeutralMode to set motors to
|
||||
@@ -164,13 +179,59 @@ public class RobotContainer {
|
||||
m_robotDrive.setDriveTrainNeutralMode(mode);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the gear of the drivetrain
|
||||
* @param state the gearing of the gearbox (true is high, false is low)
|
||||
*/
|
||||
public void setDriveGearState(boolean state) {
|
||||
m_robotDrive.setShiftState(state);
|
||||
}
|
||||
|
||||
public void configDriveTrainSensors(FeedbackDevice type) {
|
||||
m_robotDrive.configMotorSensor(type);
|
||||
}
|
||||
|
||||
public void resetOdometry() {
|
||||
m_robotDrive.resetGyroAngles();
|
||||
m_robotDrive.setOdometry(new Pose2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// no auto
|
||||
|
||||
// Create config for trajectory
|
||||
/*TrajectoryConfig config = new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND,
|
||||
DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
.setKinematics(DriveConstants.kDriveKinematics);
|
||||
|
||||
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
new Pose2d(0, 0, new Rotation2d(0)),
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
List.of(
|
||||
new Translation2d(10, 0)
|
||||
),
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
new Pose2d(20, 20, new Rotation2d(0)),
|
||||
// Pass config
|
||||
config);
|
||||
// 10 = 20, 20 = 35, 30 = 53.5
|
||||
// (0,10) = (8,22)
|
||||
RamseteCommand ramseteCommand = new RamseteCommand(
|
||||
exampleTrajectory,
|
||||
m_robotDrive::getPose,
|
||||
new RamseteController(),
|
||||
DriveConstants.kDriveKinematics,
|
||||
m_robotDrive::tankDriveVelocity,
|
||||
m_robotDrive);
|
||||
|
||||
// Run path following command, then stop at the end.
|
||||
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/
|
||||
return new InstantCommand();
|
||||
}
|
||||
|
||||
|
||||
@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
|
||||
@Override
|
||||
public void execute() {
|
||||
//System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro);
|
||||
m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -47,7 +47,7 @@ public class DriveStraightToPositionPID extends CommandBase {
|
||||
//System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
|
||||
//System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
|
||||
//System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
|
||||
m_drive.runDriveStraightPositionPID(m_targetPosOut, m_targetGyro);
|
||||
m_drive.runDrivePositionPID(m_targetPosOut, m_targetGyro);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -38,7 +38,7 @@ public class DriveWithJoystick extends CommandBase {
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double moveInput = m_controller.getLeftYAxis();
|
||||
double moveInput = -m_controller.getLeftYAxis();
|
||||
double steerInput = m_controller.getRightXAxis();
|
||||
double moveOutput = 0;
|
||||
double steerOutput = 0;
|
||||
|
||||
@@ -0,0 +1,118 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 edu.wpi.first.wpiutil.math.MathUtil;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class DriveWithJoystickDriveStraight extends CommandBase {
|
||||
Drive m_drive;
|
||||
double m_targetGyro, m_currentGyro;
|
||||
double m_stopPos;
|
||||
long m_currTime, m_deltaTime;
|
||||
long m_deadTimeSteer, m_deadTimeMove;
|
||||
long m_deadTimeout = 100;
|
||||
IHandController m_controller;
|
||||
|
||||
/**
|
||||
* Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
|
||||
* Applies a curved ramp to the input from the controllers to make the robot less "touchy".
|
||||
* Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
|
||||
* @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
|
||||
* @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
|
||||
* {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
|
||||
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
|
||||
*/
|
||||
public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_controller = controller;
|
||||
addRequirements(m_drive);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_currTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1);
|
||||
double moveInput = -m_controller.getLeftYAxis();
|
||||
double steerInput = m_controller.getRightXAxis();
|
||||
double moveOutput = 0;
|
||||
m_deltaTime = System.currentTimeMillis() - m_currTime;
|
||||
m_currTime = System.currentTimeMillis();
|
||||
|
||||
/* If steer stick is being used */
|
||||
if (steerInput != 0) {
|
||||
m_deadTimeSteer = m_currTime;
|
||||
}
|
||||
|
||||
/* Curves the moveInput to be slightly more gradual at first */
|
||||
if (moveInput >= 0) {
|
||||
moveOutput = -Math.cos(1.571*moveInput)+1;
|
||||
} else {
|
||||
moveOutput = Math.cos(1.571*moveInput)-1;
|
||||
}
|
||||
|
||||
/* If steer stick has not been used for less than 1 sec */
|
||||
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
|
||||
runDriveWithInput(moveOutput, steerInput);
|
||||
resetGyroTarget();
|
||||
}
|
||||
/* If steer stick has not been used for 1 sec */
|
||||
else {
|
||||
runDriveStraight(moveOutput);
|
||||
}
|
||||
}
|
||||
|
||||
private void runDriveWithInput(double move, double steer) {
|
||||
double cosMultiplier = .45;
|
||||
double steerOutput = 0;
|
||||
double deadzone = .2;
|
||||
/* Curves the steer output to be similarily gradual */
|
||||
if (steer > 0){
|
||||
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
|
||||
} else {
|
||||
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
|
||||
}
|
||||
m_drive.driveWithInput(move, steerOutput);
|
||||
System.out.println("Driving With Input");
|
||||
}
|
||||
|
||||
private void runDriveStraight(double move) {
|
||||
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
|
||||
System.out.println("Driving Straight with Target: " + m_targetGyro);
|
||||
}
|
||||
|
||||
/**
|
||||
* set target angle to current angle (prevents buildup of gyro error).
|
||||
*/
|
||||
private void resetGyroTarget() {
|
||||
//m_targetGyro = m_currentGyro;
|
||||
m_targetGyro = m_currentGyro
|
||||
+ m_drive.getTurnRate();
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
@@ -15,10 +15,12 @@ import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
|
||||
Drive m_drive;
|
||||
double m_targetGyro;
|
||||
long lastTime;
|
||||
double m_targetGyro, m_currentGyro;
|
||||
double m_stopPos;
|
||||
long m_currTime, m_deltaTime;
|
||||
long m_deadTimeSteer, m_deadTimeMove;
|
||||
long m_deadTimeout = 100;
|
||||
IHandController m_controller;
|
||||
boolean isAuxPIDEnabled = false;
|
||||
|
||||
/**
|
||||
* Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
|
||||
@@ -30,7 +32,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
|
||||
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
|
||||
*/
|
||||
public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_drive = subsystem;
|
||||
m_controller = controller;
|
||||
addRequirements(m_drive);
|
||||
@@ -39,35 +41,32 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
lastTime = System.currentTimeMillis();
|
||||
m_currTime = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
|
||||
double moveInput = m_controller.getLeftYAxis();
|
||||
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
|
||||
double moveInput = -m_controller.getLeftYAxis();
|
||||
double steerInput = m_controller.getRightXAxis();
|
||||
double moveOutput = 0;
|
||||
double steerOutput = 0;
|
||||
long deltaTime = System.currentTimeMillis() - lastTime;
|
||||
lastTime = System.currentTimeMillis();
|
||||
|
||||
/* If AuxPID is enabled, then update using the steer input */
|
||||
if (isAuxPIDEnabled) {
|
||||
m_targetGyro += 2 * steerInput * deltaTime;
|
||||
|
||||
m_targetGyro = MathUtil.clamp( m_targetGyro,
|
||||
currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
|
||||
currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
|
||||
}
|
||||
/* Otherwise set target angle to current angle (prevents buildup of gyro error) */
|
||||
else {
|
||||
m_targetGyro = currentGyro;
|
||||
}
|
||||
m_deltaTime = System.currentTimeMillis() - m_currTime;
|
||||
m_currTime = System.currentTimeMillis();
|
||||
|
||||
/* If move stick is being used */
|
||||
if (moveInput != 0) {
|
||||
m_deadTimeMove = m_currTime;
|
||||
m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
|
||||
+ (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
|
||||
}
|
||||
/* If steer stick is being used */
|
||||
if (steerInput != 0) {
|
||||
m_deadTimeSteer = m_currTime;
|
||||
}
|
||||
|
||||
/* If move stick has been pressed within 1 sec */
|
||||
if (m_currTime - m_deadTimeMove < m_deadTimeout) {
|
||||
/* Curves the moveInput to be slightly more gradual at first */
|
||||
if (moveInput >= 0) {
|
||||
moveOutput = -Math.cos(1.571*moveInput)+1;
|
||||
@@ -75,32 +74,66 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
|
||||
moveOutput = Math.cos(1.571*moveInput)-1;
|
||||
}
|
||||
|
||||
/* If steer stick is being used. */
|
||||
if (steerInput != 0) {
|
||||
double cosMultiplier = .45;
|
||||
double deadzone = .2;
|
||||
/* Curves the steer output to be similarily gradual */
|
||||
if (steerInput > 0){
|
||||
steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
|
||||
} else {
|
||||
steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
|
||||
}
|
||||
m_drive.driveWithInput(moveOutput, steerOutput);
|
||||
isAuxPIDEnabled = false;
|
||||
/* If steer stick has not been used for less than 1 sec */
|
||||
if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
|
||||
runDriveWithInput(moveOutput, steerInput);
|
||||
resetGyroTarget();
|
||||
}
|
||||
/* If only the move stick is being used */
|
||||
/* If steer stick has not been used for 1 sec */
|
||||
else {
|
||||
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
|
||||
isAuxPIDEnabled = true;
|
||||
runDriveStraight(moveOutput);
|
||||
}
|
||||
}
|
||||
/* If the move stick is not being used */
|
||||
/* If the move stick has not been used for 1 sec */
|
||||
else {
|
||||
m_drive.runDriveStraightVelocityPID(0, m_targetGyro);
|
||||
isAuxPIDEnabled = true;
|
||||
runStoppedTurn(steerInput);
|
||||
}
|
||||
}
|
||||
|
||||
private void runDriveWithInput(double move, double steer) {
|
||||
double cosMultiplier = .45;
|
||||
double steerOutput = 0;
|
||||
double deadzone = .2;
|
||||
/* Curves the steer output to be similarily gradual */
|
||||
if (steer > 0){
|
||||
steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
|
||||
} else {
|
||||
steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
|
||||
}
|
||||
m_drive.driveWithInput(move, steerOutput);
|
||||
System.out.println("Driving With Input");
|
||||
}
|
||||
|
||||
private void runDriveStraight(double move) {
|
||||
m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
|
||||
System.out.println("Driving Straight with Target: " + m_targetGyro);
|
||||
}
|
||||
|
||||
private void runStoppedTurn(double steer) {
|
||||
updateGyroTarget(steer);
|
||||
m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
|
||||
System.out.println("Turning with Target: " + m_targetGyro);
|
||||
}
|
||||
|
||||
/**
|
||||
* If AuxPID is enabled, then update using the steer input
|
||||
*/
|
||||
private void updateGyroTarget(double steerInput) {
|
||||
m_targetGyro -= 5 * steerInput * m_deltaTime;
|
||||
m_targetGyro = MathUtil.clamp( m_targetGyro,
|
||||
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8),
|
||||
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8));
|
||||
}
|
||||
|
||||
/**
|
||||
* set target angle to current angle (prevents buildup of gyro error).
|
||||
*/
|
||||
private void resetGyroTarget() {
|
||||
m_targetGyro = m_currentGyro;
|
||||
m_targetGyro = m_currentGyro
|
||||
+ m_drive.getTurnRate();
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
|
||||
@@ -36,11 +36,17 @@ import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
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.wpiutil.math.MathUtil;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.Gains;
|
||||
|
||||
@@ -58,17 +64,29 @@ public class Drive extends SubsystemBase {
|
||||
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
|
||||
public Orchestra m_orchestra = new Orchestra();
|
||||
|
||||
public double m_rightFrontMotorPos;
|
||||
|
||||
public double m_rightFrontMotorVel;
|
||||
|
||||
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;
|
||||
//public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
|
||||
|
||||
public final DifferentialDriveOdometry m_odometry;
|
||||
|
||||
public DoubleSolenoid m_speedShift;
|
||||
public DoubleSolenoid m_coolFalcon;
|
||||
|
||||
SendableChooser<String> m_songChooser = new SendableChooser<String>();
|
||||
|
||||
public DoubleSolenoid speedShift;
|
||||
public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
|
||||
public long m_lastTime, m_deltaTime; //in milliseconds
|
||||
|
||||
public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
@@ -82,18 +100,18 @@ public class Drive extends SubsystemBase {
|
||||
m_pigeon.configFactoryDefault();
|
||||
resetGyroYaw();
|
||||
|
||||
speedShift = new DoubleSolenoid(7,0,1);
|
||||
m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()),
|
||||
new Pose2d(0, 0, new Rotation2d()) );
|
||||
|
||||
m_speedShift = new DoubleSolenoid(7,0,1);
|
||||
m_coolFalcon = new DoubleSolenoid(7,3,2);
|
||||
|
||||
coolFalcon(false);
|
||||
|
||||
/* set back motors as followers */
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
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
|
||||
|
||||
/* flip input so forward becomes back, etc */
|
||||
m_leftFrontMotor.setInverted(false);
|
||||
m_rightFrontMotor.setInverted(true);
|
||||
@@ -101,12 +119,21 @@ public class Drive extends SubsystemBase {
|
||||
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);
|
||||
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_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
|
||||
//m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
|
||||
|
||||
/* PID for Front Motor Control in Teleop */
|
||||
//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);
|
||||
@@ -114,47 +141,65 @@ public class Drive extends SubsystemBase {
|
||||
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);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.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);
|
||||
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
//m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, 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);
|
||||
//m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* PID for Back Motor control in Auto */
|
||||
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.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);
|
||||
resetEncoders();
|
||||
|
||||
/* 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
|
||||
m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Configure the left back Talon's selected sensor as local QuadEncoder */
|
||||
m_leftBackMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
|
||||
DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Configure the right back Talon's selected sensor as local QuadEncoder */
|
||||
m_rightBackMotor.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
|
||||
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);
|
||||
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);
|
||||
@@ -165,94 +210,81 @@ public class Drive extends SubsystemBase {
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, 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
|
||||
configMotorSensor(FeedbackDevice.SensorDifference);
|
||||
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
/*
|
||||
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) DOESN'T WORK */
|
||||
//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);
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
//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);
|
||||
m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, 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.
|
||||
*/
|
||||
* 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);
|
||||
|
||||
m_leftBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.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);
|
||||
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
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
|
||||
*/
|
||||
* 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);
|
||||
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
|
||||
m_orchestra.addInstrument(m_leftBackMotor);
|
||||
m_orchestra.addInstrument(m_rightFrontMotor);
|
||||
m_orchestra.addInstrument(m_rightBackMotor);
|
||||
@@ -265,35 +297,64 @@ public class Drive extends SubsystemBase {
|
||||
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
|
||||
}
|
||||
Shuffleboard.getTab("Songs").add(m_songChooser);
|
||||
}
|
||||
}
|
||||
|
||||
String currentSong = "";
|
||||
@Override
|
||||
public void periodic() {
|
||||
m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
|
||||
SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis());
|
||||
|
||||
if (m_currentTimeSec % 30 == 0) {
|
||||
coolFalcon(true);
|
||||
SmartDashboard.putBoolean("Solenoid", true);
|
||||
} else if ((m_currentTimeSec - 1) % 30 == 0) {
|
||||
coolFalcon(false);
|
||||
SmartDashboard.putBoolean("Solenoid", false);
|
||||
}
|
||||
|
||||
m_deltaTime = System.currentTimeMillis() - m_lastTime;
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
m_lastAngleYaw = m_currentAngleYaw;
|
||||
m_currentAngleYaw = getGyroYaw();
|
||||
|
||||
m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
|
||||
m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
|
||||
|
||||
try {
|
||||
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
|
||||
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
|
||||
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("Pigeon Pitch", getGyroPitch());
|
||||
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
|
||||
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
|
||||
|
||||
SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.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("Right Motor Temp", m_rightFrontMotor.getTemperature());
|
||||
SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
|
||||
|
||||
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));
|
||||
//SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
|
||||
//SmartDashboard.putNumber("Left Front Motor Current Stator", 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));
|
||||
|
||||
SmartDashboard.putString("Odometry Values Meters", getPose().toString());
|
||||
SmartDashboard.putNumber("Odometry Heading", getHeading());
|
||||
|
||||
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
|
||||
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
|
||||
|
||||
if (currentSong != m_songChooser.getSelected()){
|
||||
currentSong = m_songChooser.getSelected();
|
||||
@@ -302,12 +363,17 @@ public class Drive extends SubsystemBase {
|
||||
}
|
||||
} catch (Exception e) {
|
||||
System.err.println("Error in the Drive Subsystem");
|
||||
//e.printStackTrace(System.err);
|
||||
// e.printStackTrace(System.err);
|
||||
}
|
||||
|
||||
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
|
||||
inchesToMeters(getDistanceInches(m_leftBackMotor)),
|
||||
-inchesToMeters(getDistanceInches(m_rightBackMotor)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
*
|
||||
* @param mode NeutralMode to set motors to
|
||||
*/
|
||||
public void setDriveTrainNeutralMode(NeutralMode mode) {
|
||||
@@ -317,59 +383,14 @@ public class Drive extends SubsystemBase {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs percent output control on the moving and steering of the drive train,
|
||||
* using the Differential Drive class to manage the two inputs
|
||||
*/
|
||||
public void driveWithInput(double move, double steer){
|
||||
//m_driveTrain.arcadeDrive(move, steer);
|
||||
public void driveWithInput(double move, double steer) {
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -382,35 +403,44 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs a position PID while driving straight
|
||||
* @param targetPos The position to drive to in units
|
||||
* Runs position PID.
|
||||
* Position is absolute and displacement should be handled on the command side.
|
||||
* @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) {
|
||||
public void runDrivePositionPID(double targetPos, double targetGyro) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
//m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs velocity PID while driving straight
|
||||
* @param targetVel The velocity to drive at in units
|
||||
* Runs velocity PID
|
||||
*
|
||||
* @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) {
|
||||
public void runDriveVelocityPID(double targetVel, double targetGyro) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
|
||||
m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
//m_driveTrain.feedWatchdog();
|
||||
}
|
||||
@@ -420,12 +450,14 @@ public class Drive extends SubsystemBase {
|
||||
* @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){
|
||||
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(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
|
||||
@@ -433,12 +465,61 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
/**
|
||||
* 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);
|
||||
public void runTurningPID(double targetAngle) {
|
||||
double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
|
||||
runDriveVelocityPID(0, targetGyro);
|
||||
}
|
||||
|
||||
/**
|
||||
* Controls the left and right sides of the drive with velocity targets.
|
||||
*
|
||||
* @param leftSpeed the commanded left speed
|
||||
* @param rightSpeed the commanded right speed
|
||||
*/
|
||||
public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
|
||||
//DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
|
||||
//ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
|
||||
//double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
|
||||
//double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
|
||||
//double angleVelDeg = Math.toDegrees(angleVelRad);
|
||||
|
||||
//m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000);
|
||||
//m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle,
|
||||
// m_currentAngleYaw-(360),
|
||||
// m_currentAngleYaw+(360));
|
||||
//double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
double moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
|
||||
|
||||
//SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
|
||||
//SmartDashboard.putNumber("Move Vel Right", moveVelRight);
|
||||
|
||||
//runDriveVelocityPID(moveVel*2, targetGyro);
|
||||
|
||||
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
|
||||
System.err.println(moveVelLeft);
|
||||
|
||||
m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight);
|
||||
m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
|
||||
m_leftFrontMotor.follow(m_leftBackMotor);
|
||||
m_rightFrontMotor.follow(m_rightBackMotor);
|
||||
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
* Selects the feedback device for the motors.
|
||||
* @param feedbackDevice The feedback device to set it to, usually SensorDifference or
|
||||
*/
|
||||
public void configMotorSensor(FeedbackDevice type) {
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -446,17 +527,17 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
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];
|
||||
}
|
||||
@@ -466,7 +547,7 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
public double getGyroRoll() {
|
||||
double[] ypr = new double[3];
|
||||
|
||||
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
return ypr[2];
|
||||
}
|
||||
@@ -477,9 +558,132 @@ public class Drive extends SubsystemBase {
|
||||
public void resetGyroYaw() {
|
||||
m_pigeon.setYaw(0);
|
||||
m_pigeon.setAccumZAngle(0);
|
||||
resetGyroAngles();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add docs here
|
||||
*/
|
||||
public void resetGyroAngles() {
|
||||
m_lastAngleYaw = 0;
|
||||
m_currentAngleYaw = 0;
|
||||
m_kinematicsTargetAngle = 0;
|
||||
}
|
||||
//lol
|
||||
//sko
|
||||
//ridge
|
||||
/**
|
||||
//brayden=bad coder
|
||||
* Returns the heading of the robot
|
||||
* @return The robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return Math.IEEEremainder(getGyroYaw(), 360);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
public double getTurnRate() {
|
||||
double deltaYaw = m_currentAngleYaw - m_lastAngleYaw;
|
||||
double turnRate = 1000 * deltaYaw / m_deltaTime;
|
||||
return turnRate;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPoseMeters();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns current wheel speeds of robot.
|
||||
* @return The current wheel speeds.
|
||||
*/
|
||||
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
|
||||
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)),
|
||||
-inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the encoders for both motors.
|
||||
*/
|
||||
public void resetEncoders() {
|
||||
m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void setOdometry(Pose2d pose) {
|
||||
resetEncoders();
|
||||
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the encoder value (position) of a motor
|
||||
* @param falcon The motor to get the position of
|
||||
* @return The position of the motor in inches
|
||||
*/
|
||||
public double getDistanceInches(WPI_TalonFX falcon) {
|
||||
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the encoder value (velocity) of a motor
|
||||
* @param falcon The motor to get the velocity of
|
||||
* @return The velocity of the motor in inches per second
|
||||
*/
|
||||
public double getVelocityInchesPerSecond(WPI_TalonFX falcon) {
|
||||
return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition()/DriveConstants.TICK_TIME_TO_SECONDS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in ticks to inches.
|
||||
* @param ticks The value in ticks to convert
|
||||
* @return The converted value in inches
|
||||
*/
|
||||
public double ticksToInches(double ticks) {
|
||||
return ticks * DriveConstants.INCHES_PER_TICK;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in inches to ticks.
|
||||
* @param inches The value in inches to convert
|
||||
* @return The converted value in ticks
|
||||
*/
|
||||
public double inchesToTicks(double inches) {
|
||||
return inches * DriveConstants.TICKS_PER_INCH;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in inches to meters.
|
||||
* @param inches The value in inches to convert
|
||||
* @return The converted value in meters
|
||||
*/
|
||||
public double inchesToMeters(double inches) {
|
||||
return inches * DriveConstants.METERS_PER_INCH;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a value in meters to inches.
|
||||
* @param meters The value in meters to convert
|
||||
* @return The converted value in inches
|
||||
*/
|
||||
public double metersToInches(double meters) {
|
||||
return meters * DriveConstants.INCHES_PER_METER;
|
||||
}
|
||||
|
||||
/*
|
||||
* Plays Music!
|
||||
*/
|
||||
public void playSong() {
|
||||
@@ -500,10 +704,24 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
public void setShiftState(boolean state) {
|
||||
if (state == true) {
|
||||
speedShift.set(DoubleSolenoid.Value.kForward);
|
||||
m_speedShift.set(DoubleSolenoid.Value.kForward);
|
||||
}
|
||||
if (state == false) {
|
||||
speedShift.set(DoubleSolenoid.Value.kReverse);
|
||||
m_speedShift.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set to open or close solenoid that cools the falcon, true = open, false = close
|
||||
* @param state Chooses between open and close
|
||||
*/
|
||||
public void coolFalcon(boolean state) {
|
||||
if (state == true) {
|
||||
m_coolFalcon.set(DoubleSolenoid.Value.kForward);
|
||||
}
|
||||
if (state == false) {
|
||||
m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -56,7 +56,6 @@ public class Storage extends SubsystemBase {
|
||||
public void runStorage(final double input) {
|
||||
m_storageMotor.set(input);
|
||||
final boolean beam_on = m_beamSensors[0].get();
|
||||
|
||||
}
|
||||
|
||||
public void resetEncoder()
|
||||
|
||||
Reference in New Issue
Block a user