Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #34 from Team4388/arm

Browse Source 
Armfjklffjklfjefjfjiofjio

yes
This commit is contained in:
Aarav Shah
2023-03-01 16:59:48 -07:00
committed by GitHub
parent 29bfb74c8a 23a6337a96
commit 9fea461b8f
5 changed files with 156 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
build.gradle
+1 -1
View File
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2023.1.1"
id "edu.wpi.first.GradleRIO" version "2023.3.2"
}
sourceCompatibility = JavaVersion.VERSION_11
src/main/java/frc4388/robot/Constants.java
+20 -5
View File
@@ -105,13 +105,28 @@ public final class Constants {
public static final class GyroConstants {
public static final int ID = 14; // TODO: find the actual ID
}
public static final class ArmConstants {
public static final int MIN_ARM_LEN = 0;
public static final int MAX_ARM_LEN = 1;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
public static final double MIN_ARM_LEN = 1;
public static final double MAX_ARM_LEN = 2;
public static final double ARM_HEIGHT = 1;
public static final double CURVE_POWER = 2;
public static final double TELE_TICKS_PER_SECOND = (-5);
public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value
public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value
public static final double TELE_FORWARD_SOFT_LIMIT = 0;
public static final double TELE_REVERSE_SOFT_LIMIT = 0;
public static final double kP = 0;
public static final double kI = 0;
public static final double kD = 0;
public static final double OFFSET = 0;
}
public static final class LEDConstants {
// public static final int LED_SPARK_ID = 0;
src/main/java/frc4388/robot/RobotContainer.java
+6 -26
View File
@@ -7,40 +7,17 @@
package frc4388.robot;
import java.util.ArrayList;
import java.util.List;
import org.opencv.objdetect.HOGDescriptor;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.Trajectory.State;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
import frc4388.robot.subsystems.Arm;
import frc4388.robot.commands.AutoBalance;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.IHandController;
import frc4388.robot.commands.JoystickPlayback;
import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.commands.PlaybackChooser;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.XboxController;
@@ -56,6 +33,10 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
m_robotMap.rightFront,
m_robotMap.leftBack,
@@ -69,7 +50,7 @@ public class RobotContainer {
/* Autos */
public SendableChooser<Command> chooser = new SendableChooser<>();
private Command noAuto = new InstantCommand();
// private Command noAuto = new InstantCommand();
// private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
@@ -101,7 +82,6 @@ public class RobotContainer {
// chooser.addOption("Blue1Path", blue1Path);
// chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
// chooser.addOption("Red1Path", red1Path);
// chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
src/main/java/frc4388/robot/RobotMap.java
+37 -3
View File
@@ -7,11 +7,12 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import frc4388.robot.Constants.ArmConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveModule;
import frc4388.utility.RobotGyro;
@@ -30,10 +31,10 @@ public class RobotMap {
public SwerveModule leftBack;
public SwerveModule rightBack;
public RobotMap() {
configureLEDMotorControllers();
configureDriveMotors();
configArmMotors();
}
/* LED Subsystem */
@@ -60,7 +61,6 @@ public class RobotMap {
public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
void configureDriveMotors() {
// config factory default
leftFrontWheel.configFactoryDefault();
@@ -126,4 +126,38 @@ public class RobotMap {
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
}
// arm stuff
public WPI_TalonFX pivot = new WPI_TalonFX(-1); // TODO: Add real id
public WPI_TalonFX tele = new WPI_TalonFX(-1); // TODO: Add real id
public CANCoder pivotEncoder = new CANCoder(-1);
public void configArmMotors() {
// config factory default
pivot.configFactoryDefault();
tele.configFactoryDefault();
// config open loop ramp
pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config closed loop ramp
pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config neutral mode to brake
pivot.setNeutralMode(NeutralMode.Brake);
tele.setNeutralMode(NeutralMode.Brake);
// soft limits
pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
pivot.configForwardSoftLimitEnable(false);
pivot.configReverseSoftLimitEnable(false);
tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
tele.configForwardSoftLimitEnable(false);
tele.configReverseSoftLimitEnable(false);
}
}
src/main/java/frc4388/robot/subsystems/Arm.java
+92 -13
View File
@@ -1,39 +1,118 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.CANCoderConfiguration;
import frc4388.robot.Constants.ArmConstants;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Arm extends SubsystemBase {
private WPI_TalonFX m_armmotor;
private boolean m_debug;
// Moves arm to distence [dist] then returns new ang
public Arm(WPI_TalonFX armmotor, boolean debug) {
armmotor.configFactoryDefault();
m_armmotor = armmotor;
private WPI_TalonFX m_tele;
private WPI_TalonFX m_pivot;
private CANCoder m_pivotEncoder;
private boolean m_debug;
// Moves arm to distance [dist] then returns new ang
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) {
m_tele = tele;
m_pivot = pivot;
m_pivotEncoder = encoder;
TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
pivotConfig.slot0.kP = ArmConstants.kP;
pivotConfig.slot0.kI = ArmConstants.kI;
pivotConfig.slot0.kD = ArmConstants.kD;
pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
pivot.configAllSettings(pivotConfig);
CANCoderConfiguration config = new CANCoderConfiguration();
config.magnetOffsetDegrees = ArmConstants.OFFSET;
m_pivotEncoder.configAllSettings(config);
tele.configFactoryDefault();
pivot.configFactoryDefault();
}
public Arm(WPI_TalonFX armmoter) {
this(armmoter, false);
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
this(pivot, tele, encoder, false);
}
public void setRotVel(double vel) {
m_pivot.set(ControlMode.Velocity, vel);
}
public void setTeleVel(double vel) {
m_tele.set(ControlMode.Velocity, vel);
}
public void armSetRotation(double rot) {
if (rot > 1 || rot < 0) return;
// Move arm code
m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) +
ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
}
public void armSetLength(double len) {
if (len > 1 || len < 0) return;
// Move arm code
m_armmotor.set(ControlMode.Position, len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) +
ArmConstants.MIN_ARM_LEN);
m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) +
ArmConstants.TELE_FORWARD_SOFT_LIMIT);
if (m_tele.isRevLimitSwitchClosed() == 1) {
m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
} else if (m_tele.isFwdLimitSwitchClosed() == 1) {
m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
}
}
public double getArmLength() {
return (m_armmotor.getSelectedSensorPosition() - ArmConstants.MIN_ARM_LEN) /
(ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN);
return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) /
(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT);
}
public double getArmRotation() {
return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) /
(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
}
public void runPivotTele(double pivot, double tele) {
var rot = 0;
if (checkLimits(tele, rot)) {
armSetRotation(pivot);
armSetLength(tele);
}
}
/**
* Checks that an input is within bounds
* @param _len length from 0 to 1
* @param _theta radians from the zero (straight up)
* @return
*/
public static boolean checkLimits(double _len, double _theta) {
var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN;
var x = len * Math.cos(_theta);
var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
if (y < minHeight)
return false;
return true;
}
@Override
public void periodic() {
if (m_debug)
SmartDashboard.putNumber("Arm Motor", m_armmotor.getSelectedSensorPosition());
SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition());
}
}