Skip to content

Commit

Permalink
merge
Browse files Browse the repository at this point in the history
  • Loading branch information
@amb2127
amb2127 committed Mar 3, 2024
2 parents 4db9b58 + ffe9c02 commit 29cabc6
Show file tree
Hide file tree
Showing 8 changed files with 146 additions and 29 deletions.
28 changes: 23 additions & 5 deletions src/main/java/org/codeorange/frc2024/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,9 @@
import org.codeorange.frc2024.utility.swerve.SwerveSetpointGenerator;
import org.codeorange.frc2024.utility.swerve.SecondOrderKinematics;
import org.jetbrains.annotations.NotNull;
import org.joml.Vector3d;

import java.io.File;
import java.io.ObjectInputStream;
import java.net.SocketException;
import java.nio.file.Files;

Expand All @@ -27,6 +27,7 @@ public final class Constants {
mac = MacAddressUtil.getMacAddress();
} catch (SocketException e) {
System.out.println("Failed to get MAC address");
mac = new byte[6];
}
}

Expand Down Expand Up @@ -99,14 +100,20 @@ public static class Ports {

public static final int INTAKE_MOTOR_ID = 31;
public static final int CLIMBER = 36;

//next two are rio dio ports
public static final int INTAKE_BEAM_BREAK = 0;
public static final int CLIMBER_LIMIT_SWITCH = 1;
//next two are rio pwm ports
public static final int SERVO_1 = 0;
public static final int SERVO_2 = 1;
}

public static final double FL_ABSOLUTE_ENCODER_OFFSET;
public static final double BL_ABSOLUTE_ENCODER_OFFSET;
public static final double FR_ABSOLUTE_ENCODER_OFFSET;
public static final double BR_ABSOLUTE_ENCODER_OFFSET;
public static final double WRIST_ABSOLUTE_ENCODER_OFFSET;
public static final double ARM_ABSOLUTE_ENCODER_OFFSET;

static {
switch(robotIdentity) {
Expand All @@ -115,24 +122,33 @@ public static class Ports {
BL_ABSOLUTE_ENCODER_OFFSET = -0.10107421875+0.5;
FR_ABSOLUTE_ENCODER_OFFSET = -0.33251953125;
BR_ABSOLUTE_ENCODER_OFFSET = 0.4794921875;
WRIST_ABSOLUTE_ENCODER_OFFSET = -0.389404296875;
ARM_ABSOLUTE_ENCODER_OFFSET = -0.36279296875;
}
case PRACTICE_BOT -> {
FL_ABSOLUTE_ENCODER_OFFSET = -0.130615234375;
BL_ABSOLUTE_ENCODER_OFFSET = -0.69091796875;
FR_ABSOLUTE_ENCODER_OFFSET = -0.94091796875;
BR_ABSOLUTE_ENCODER_OFFSET = -0.8701171875;
WRIST_ABSOLUTE_ENCODER_OFFSET = 0.336669921875;
ARM_ABSOLUTE_ENCODER_OFFSET = -0.36279296875;

}
case COMPETITION_BOT -> {
FL_ABSOLUTE_ENCODER_OFFSET = -0.962158203125;
BL_ABSOLUTE_ENCODER_OFFSET = -0.38037109375;
FR_ABSOLUTE_ENCODER_OFFSET = -0.038330078125;
BR_ABSOLUTE_ENCODER_OFFSET = -0.699462890625;
WRIST_ABSOLUTE_ENCODER_OFFSET = -0.10498046875;
ARM_ABSOLUTE_ENCODER_OFFSET = 0.45068359375;
}
default -> {
FL_ABSOLUTE_ENCODER_OFFSET = 0;
BL_ABSOLUTE_ENCODER_OFFSET = 0;
FR_ABSOLUTE_ENCODER_OFFSET = 0;
BR_ABSOLUTE_ENCODER_OFFSET = 0;
WRIST_ABSOLUTE_ENCODER_OFFSET = 0;
ARM_ABSOLUTE_ENCODER_OFFSET = 0;
}
}
}
Expand All @@ -146,19 +162,21 @@ public static class Ports {
public static final double CLIMBER_LOWER_LIMIT_ROTATIONS = 0;
public static final double CLIMBER_HANG_POSITION = 2;
public static final double CLIMBER_UPPER_LIMIT_ROTATIONS = 5;//TODO: find this
public static final double CLIMBER_HOME_VOLTAGE = -1.0;
public static final double NOMINAL_DT = 0.02;
public static final double ELEVATOR_HOME_VOLTAGE = -1.5;
public static final double ELEVATOR_STALLING_CURRENT = 30;
public static final double MIN_ELEVATOR_HOME_TIME = 0.2;


//positions for the superstructure
//TODO: find all these for the robots
public static final double SS_REST_ELEVATOR = 0;
public static final double SS_REST_ARM = 0;
public static final double SS_REST_WRIST = 0;
public static final double SS_REST_CLIMBER = 0;
public static final double SS_STOW_ELEVATOR = 0;
public static final double SS_STOW_ARM = -0.01;
public static final double SS_STOW_ARM = 0; //used to be -0.01
public static final double SS_STOW_WRIST = 0;
public static final double SS_STOW_CLIMBER = 0;
public static final double SS_GENINTERMEDIATE_ELEVATOR = isPrototype() ? 3.5 : 0;
Expand Down Expand Up @@ -208,15 +226,15 @@ public static class Ports {
public static final double SS_HOMING_WRIST = isPrototype() ? 0 : 0;
public static final double SS_HOMING_CLIMBER = isPrototype() ? 0 : 0;

public static final double SWERVE_DRIVE_P = 100;
public static final double SWERVE_DRIVE_P = 50;
public static final double SWERVE_DRIVE_D = 0;
public static final double SWERVE_DRIVE_I = 0;

public static final double TURN_P = isPrototype() ? 2 : 5;
public static final double TURN_I = isPrototype() ? 0 : 0;
public static final double TURN_D = 0.3;

public static final double ARM_P = 200;
public static final double ARM_P = isCompetition() ? 100 : 200;
public static final double ARM_I = 0;
public static final double ARM_D = isPrototype() ? 5 : 0;
public static final double ARM_RTS = isPrototype() ? 144.0 : 36.0 * 3;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/codeorange/frc2024/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -408,7 +408,7 @@ private ControllerDriveInputs getControllerDriveInputs() {
inputs = new ControllerDriveInputs(xbox.getRawAxis(1), xbox.getRawAxis(0), xbox.getRawAxis(4));
}

if (xbox.getRawButton(Controller.XboxButtons.X)) {
if (xbox.getRawButton(XboxButtons.X)) {
// Apply a larger deadzone when the button is pressed
inputs.applyDeadZone(0.2, 0.2, 0.2, 0.2);
} else {
Expand Down
14 changes: 8 additions & 6 deletions src/main/java/org/codeorange/frc2024/subsystem/arm/ArmIOTalonFX.java
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,10 @@
package org.codeorange.frc2024.subsystem.arm;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.MagnetSensorConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.*;
import com.ctre.phoenix6.controls.*;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.*;

Expand Down Expand Up @@ -58,6 +54,12 @@ public ArmIOTalonFX() {
armMotionMagicConfig.MotionMagicAcceleration = 3.2; //TODO change motion magic values
armMotionMagicConfig.MotionMagicJerk = 16;


/*var armCurrentLimitConfigs = talonFXConfigs.CurrentLimits;
armCurrentLimitConfigs.SupplyCurrentLimit = ELEVATOR_STALLING_CURRENT;
armCurrentLimitConfigs.SupplyCurrentLimitEnable = true;
armCurrentLimitConfigs.StatorCurrentLimitEnable = false;*/

Slot0Configs slot0 = talonFXConfigs.Slot0;
slot0.kP = ARM_P;
slot0.kI = ARM_I;
Expand All @@ -74,7 +76,7 @@ public ArmIOTalonFX() {
absoluteEncoder.getConfigurator().apply(new CANcoderConfiguration()
.withMagnetSensor(new MagnetSensorConfigs()
.withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
.withMagnetOffset(-0.36279296875)
.withMagnetOffset(ARM_ABSOLUTE_ENCODER_OFFSET)
.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Signed_PlusMinusHalf)
)
);
Expand Down
63 changes: 59 additions & 4 deletions src/main/java/org/codeorange/frc2024/subsystem/climber/Climber.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package org.codeorange.frc2024.subsystem.climber;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.DriverStation;
import org.codeorange.frc2024.subsystem.AbstractSubsystem;
import org.littletonrobotics.junction.Logger;

Expand All @@ -15,6 +16,8 @@
public class Climber extends AbstractSubsystem {
private final ClimberIO climberIO;
private final ClimberInputsAutoLogged climberInputs = new ClimberInputsAutoLogged();
private boolean servosOpen = false;
public boolean homing = false;

public Climber(ClimberIO climberIO){
super();
Expand All @@ -28,6 +31,21 @@ public void setPosition(double positionInRotations) {
public void update() {
climberIO.updateInputs(climberInputs);
Logger.processInputs("Climber", climberInputs);
if(servosOpen) {
climberIO.open();
} else {
climberIO.close();
}

if (homing) {
if (DriverStation.isEnabled()) {
climberIO.setVoltage(CLIMBER_HOME_VOLTAGE);
if (limitSwitchPushed()) {
homing = false;
climberIO.setEncoderToZero();
}
}
}
}

public double getPositionInRotations() {
Expand All @@ -38,11 +56,48 @@ public void zeroEncoder() {
climberIO.setEncoderToZero();
}

public void disengageRatchet() {
climberIO.disengageRatchet();
//public void disengageRatchet() {
// climberIO.disengageRatchet();
//}

//public void engageRatchet() {
// climberIO.engageRatchet();
//}

public void openServos() {
servosOpen = true;
}

public void closeServos() {
servosOpen = false;
}

public void home() {
homing = true;
}

public boolean limitSwitchPushed() {
return climberInputs.limitSwitchPushed;
}

public void stop() {
climberIO.stop();
}

public boolean areServosOpen() {
return servosOpen;
}

public void engageRatchet() {
climberIO.engageRatchet();
public void runVoltage(double voltage) {
if(voltage > 0) {
//voltage is positive, should always be allowed to go up
climberIO.setVoltage(voltage);
} else if(limitSwitchPushed()) {
//voltage is negative and down too far, should stop
stop();
} else {
//voltage is negative and not to limit switch, should be allowed to go down
climberIO.setVoltage(voltage);
}
}
}
13 changes: 10 additions & 3 deletions src/main/java/org/codeorange/frc2024/subsystem/climber/ClimberIO.java
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
package org.codeorange.frc2024.subsystem.climber;
import com.ctre.phoenix6.StatusSignal;
import org.littletonrobotics.junction.AutoLog;

public interface ClimberIO {
Expand All @@ -9,7 +10,8 @@ class ClimberInputs {
double climberCurrent = 0.0;
double climberTemp = 0.0;
double climberVoltage = 0.0;
String relayValue = "hi";
boolean limitSwitchPushed = false;
//String relayValue = "hi";

}

Expand All @@ -20,6 +22,11 @@ default void setPosition(double position) {}

default void setBrakeMode(boolean braked) {}

default void disengageRatchet() {}
default void engageRatchet() {}
//default void disengageRatchet() {}
//default void engageRatchet() {}

default void open() {}
default void close() {}
default void stop() {}
default void setVoltage(double voltage) {}
}
51 changes: 43 additions & 8 deletions src/main/java/org/codeorange/frc2024/subsystem/climber/ClimberIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,14 @@
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.*;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Servo;
import org.codeorange.frc2024.robot.Constants;

import static edu.wpi.first.wpilibj.Relay.Value.*;
Expand All @@ -24,11 +27,18 @@ public class ClimberIOTalonFX implements ClimberIO {
private final StatusSignal<Double> climberVoltage;

private final TalonFX motor;
private final Relay spikeRelay;
private final Servo servo1;
private final Servo servo2;
private final DigitalInput limitSwitch;

//private final Relay spikeRelay;

public ClimberIOTalonFX() {
motor = new TalonFX(Constants.Ports.CLIMBER);
spikeRelay = new Relay(Constants.CLIMBER_PWM_RELAY_CHANNEL);
servo1 = new Servo(Constants.Ports.SERVO_1);
servo2 = new Servo(Constants.Ports.SERVO_2);
limitSwitch = new DigitalInput(Ports.CLIMBER_LIMIT_SWITCH);
//spikeRelay = new Relay(Constants.CLIMBER_PWM_RELAY_CHANNEL);

TalonFXConfiguration motorConfig = new TalonFXConfiguration()
.withMotionMagic(new MotionMagicConfigs()
Expand Down Expand Up @@ -66,7 +76,11 @@ public ClimberIOTalonFX() {
}
private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0).withEnableFOC(true).withOverrideBrakeDurNeutral(true);
public void setPosition(double targetPosition) {
motor.setControl(motionMagicRequest.withPosition(targetPosition));
if (limitSwitch.get()) {
stop();
} else {
motor.setControl(motionMagicRequest.withPosition(targetPosition));
}
}

public void updateInputs(ClimberIO.ClimberInputs inputs) {
Expand All @@ -77,7 +91,8 @@ public void updateInputs(ClimberIO.ClimberInputs inputs) {
inputs.climberVoltage = climberVoltage.getValue();
inputs.climberCurrent = climberCurrent.getValue();
inputs.climberTemp = climberTemp.getValue();
inputs.relayValue = spikeRelay.get().getPrettyValue();
inputs.limitSwitchPushed = !limitSwitch.get();
//inputs.relayValue = spikeRelay.get().getPrettyValue();
}

public void setEncoderToZero() {
Expand All @@ -87,10 +102,30 @@ public void setEncoderToZero() {
public void setBrakeMode(boolean braked) {
motor.setNeutralMode(braked ? NeutralModeValue.Brake : NeutralModeValue.Coast);
}
public void disengageRatchet() {
spikeRelay.set(kOn);
//public void disengageRatchet() {
// spikeRelay.set(kOn);
//}
//public void engageRatchet() {
// spikeRelay.set(kOff);
//}

public void open() {
servo1.setPosition(0.5); //to compensate for bad servo programming :) opens left when facing robot-relative
servo2.setPosition(0.2); //opens to the right when facing robot-relative
}

public void close() {
servo1.setPosition(0.0); //should be about 90 degrees
servo2.setPosition(1.0); //should be about 90 degrees
}

public void stop() {
motor.stopMotor();
}
public void engageRatchet() {
spikeRelay.set(kOff);


private final VoltageOut withVoltage = new VoltageOut(0).withEnableFOC(true).withOverrideBrakeDurNeutral(true);
public void setVoltage(double voltage) {
motor.setControl(withVoltage.withOutput(voltage));
}
}
2 changes: 1 addition & 1 deletion src/main/java/org/codeorange/frc2024/subsystem/wrist/WristIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ public WristIOTalonFX() {
absoluteEncoder.getConfigurator().apply(new CANcoderConfiguration()
.withMagnetSensor(new MagnetSensorConfigs()
.withSensorDirection(SensorDirectionValue.Clockwise_Positive)
.withMagnetOffset(0.336669921875)));
.withMagnetOffset(WRIST_ABSOLUTE_ENCODER_OFFSET)));

wristAbsolutePosition = absoluteEncoder.getAbsolutePosition();
wristRelativePosition = wristMotor.getPosition();
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/codeorange/frc2024/utility/MacAddressUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public class MacAddressUtil {
(byte) 0x00, (byte) 0x80, (byte) 0x2f, (byte) 0x34, (byte) 0x8f, (byte) 0xb9
};
public static final byte[] COMPETITION_BOT = new byte[]{
(byte) 0x00, (byte) 0x20, (byte) 0x2f, (byte) 0x39, (byte) 0x0f, (byte) 0xc1
(byte) 0x00, (byte) 0x80, (byte) 0x2f, (byte) 0x39, (byte) 0x0f, (byte) 0xc1
};

/**
Expand Down

0 comments on commit 29cabc6

Please sign in to comment.