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

arm limits

Browse Source
This commit is contained in:
Connorppeach
2023-02-09 19:13:55 -07:00
parent f5b7e5ffef
commit dd4223e13e
4 changed files with 77 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -4
View File
@@ -106,13 +106,16 @@ public final class Constants {
}
public static final class ArmConstants {
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 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 = -1.0; // TODO: find actual value
public static final double TELE_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 class LEDConstants {
src/main/java/frc4388/robot/RobotContainer.java
+2 -3
View File
@@ -62,9 +62,8 @@ public class RobotContainer {
-getDriverController().getRightXAxis(), false), m_robotSwerveDrive)
);
m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.runPivotAndTele(
getOperatorController().getLeftYAxis(),
getOperatorController().getLeftXAxis()), m_robotArm)
m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.armSetLength(
getOperatorController().getLeftYAxis()), m_robotArm)
);
}
src/main/java/frc4388/robot/RobotMap.java
-1
View File
@@ -63,7 +63,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();
src/main/java/frc4388/robot/subsystems/Arm.java
+68 -52
View File
@@ -1,60 +1,76 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import frc4388.robot.Constants.ArmConstants;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
public class Arm extends SubsystemBase {
/** Creates a new Arm. */
private WPI_TalonFX pivot;
private WPI_TalonFX tele;
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) {
this.pivot = pivot;
this.tele = tele;
//Limit switches
pivot.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
pivot.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
tele.configForwardSoftLimitThreshold(Constants.ArmConstants.TELE_FORWARD_SOFT_LIMIT);
}
public void runPivot(double output) {
pivot.set(output);
}
public void runTele(double output) {
tele.set(output);
}
public void runPivotAndTele(double pOutput, double tOutput) {
pivot.set(pOutput);
tele.set(tOutput);
}
public double getPivotPos() {
return pivot.getSelectedSensorPosition();
}
public double getTelePos() {
return tele.getSelectedSensorPosition();
}
@Override
public void periodic() {
if (tele.isRevLimitSwitchClosed() == 1) {
tele.setSelectedSensorPosition(0);
private WPI_TalonFX m_tele;
private WPI_TalonFX m_pivot;
private boolean m_debug;
// Moves arm to distence [dist] then returns new ang
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, boolean debug) {
tele.configFactoryDefault();
m_tele = tele;
pivot.configFactoryDefault();
m_pivot = pivot;
}
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) {
this(pivot, tele, false);
}
public void armSetRotation(double rot) {
if (rot > 1 || rot < 0) return;
// Move arm code
m_pivot.set(ControlMode.Position, rot * (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_tele.set(ControlMode.Position, len * (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) +
ArmConstants.TELE_FORWARD_SOFT_LIMIT);
}
public double getArmLength() {
return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) /
(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_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_tele.getSelectedSensorPosition());
}
}
}