Skip to content

Commit

Permalink
Merge branch 'main' into testing-shooter-angle-math
Browse files Browse the repository at this point in the history
  • Loading branch information
@rflood07
rflood07 authored Feb 16, 2024
2 parents c184fae + 9f6eb6f commit 0038a65
Show file tree
Hide file tree
Showing 6 changed files with 114 additions and 98 deletions.
1 change: 1 addition & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,6 +128,7 @@ public class RobotContainer {
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
//preferences are initialized IF they don't already exist on the Rio
Preferences.initBoolean("Brushes", false);
Preferences.initBoolean("Intake", false);
Preferences.initBoolean("Climber", false);
Preferences.initBoolean("Shooter", false);
Expand Down
20 changes: 10 additions & 10 deletions src/main/java/frc/robot/settings/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -254,9 +254,9 @@ private DriveConstants() {
public static final double k_PICKUP_NOTE_tx_D = 0;
}
public static final class ShooterConstants{
public static final int SHOOTER_R_MOTORID = 2491;
public static final int SHOOTER_L_MOTORID = 2491;
public static final int PITCH_MOTOR_ID = 2491;
public static final int SHOOTER_R_MOTORID = 10;
public static final int SHOOTER_L_MOTORID = 9;
public static final int PITCH_MOTOR_ID = 22;
public static final double SHOOTER_MOTOR_POWER = 1;
public static final double SHOOTER_AMP_POWER = 0.3;
public static final double SHOOTING_SPEED_MPS = 10;
Expand All @@ -273,7 +273,7 @@ public static final class ShooterConstants{
public static final double SHOOTER_ANGLE_TOLERANCE = 0.5;
public static final double SHOOTER_HEIGHT = 0.1;
public static final double ANGLE_TICKS_PER_DEGREE = 2491;
public static final double DEGREES_PER_ROTATION = 2491;
public static final double DEGREES_PER_ROTATION = 360;
public static final double DISTANCE_MULTIPLIER = 0;
public static final double OFFSET_MULTIPLIER = 1;
public static final double MINIMUM_SHOOTER_ANGLE = 10;//still has to be found
Expand All @@ -282,9 +282,9 @@ public static final class ShooterConstants{


//PID coefficients for shooter:
public static final double kP = 0.1;
public static final double kI = 1e-4;
public static final double kD = 1;
public static final double kP = 1;
public static final double kI = 0;
public static final double kD = 0;
public static final double kIz = 0;
public static final double kFF = 0;
public static final double kMaxOutput = 1;
Expand Down Expand Up @@ -313,12 +313,12 @@ public static final class ClimberConstants{
public static final double CLIMBER_RPM = 0.1;
}
public static final class IndexerConstants{
public static final int INDEXER_MOTOR = 2491;
public static final int INDEXER_MOTOR = 23;
public static final double INDEXER_SPEED = 2491;
}
public static final class IntakeConstants{
public static final int INTAKE_1_MOTOR = 2491;
public static final int INTAKE_2_MOTOR = 2491;
public static final int INTAKE_1_MOTOR = 20;
public static final int INTAKE_2_MOTOR = 21;
public static final int BRUSH_1_MOTOR = 2491;
public static final int BRUSH_2_MOTOR = 2491;
public static final int BRUSH_3_MOTOR = 2491;
Expand Down
43 changes: 27 additions & 16 deletions src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,49 +13,56 @@

import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkAbsoluteEncoder;
import com.revrobotics.SparkPIDController;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.SparkAbsoluteEncoder.Type;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class AngleShooterSubsystem extends SubsystemBase {
CANSparkMax pitchMotor;
SparkPIDController pitchPID;
CANcoder angleEncoder;
SparkAbsoluteEncoder absoluteEncoder;
double shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS;
public static Pose2d dtvalues;
public static ChassisSpeeds DTChassisSpeeds;

public AngleShooterSubsystem() {
pitchMotor = new CANSparkMax(ShooterConstants.PITCH_MOTOR_ID, MotorType.kBrushless);

pitchMotor.setInverted(true);
pitchPID = pitchMotor.getPIDController();

pitchPID.setFF(ShooterConstants.pitchFeedForward);
}

absoluteEncoder = pitchMotor.getAbsoluteEncoder(Type.kDutyCycle);
absoluteEncoder.setPositionConversionFactor(1);
absoluteEncoder.setInverted(false);
SmartDashboard.putData("set zero offset", new InstantCommand(()->absoluteEncoder.setZeroOffset(SmartDashboard.getNumber("shooter encoder zero offset", absoluteEncoder.getZeroOffset()))));
}

public void pitchShooter(double pitchSpeed) {
pitchMotor.set(pitchSpeed);
}

public double getShooterAngle() {
return angleEncoder.getPosition().getValueAsDouble() * ShooterConstants.DEGREES_PER_ROTATION;
return absoluteEncoder.getPosition() * ShooterConstants.DEGREES_PER_ROTATION;
}

public static void setDTPose(Pose2d pose) {
dtvalues = pose;
}

public static void setDTChassisSpeeds(ChassisSpeeds speeds) {
DTChassisSpeeds = speeds;
}

public double calculateSpeakerAngleDifference() {
double deltaX;
shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS;
Expand All @@ -69,19 +76,19 @@ public double calculateSpeakerAngleDifference() {
double deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y);
double speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2));
SmartDashboard.putNumber("dist to speakre", speakerDist);

double shootingTime = speakerDist / shootingSpeed; // calculates how long the note will take to reach the target
double currentXSpeed = DTChassisSpeeds.vxMetersPerSecond;
double currentYSpeed = DTChassisSpeeds.vyMetersPerSecond;
Translation2d targetOffset = new Translation2d(currentXSpeed * shootingTime*OFFSET_MULTIPLIER, currentYSpeed * shootingTime*OFFSET_MULTIPLIER);
// line above calculates how much our current speed will affect the ending
// location of the note if it's in the air for ShootingTime

// next 3 lines set where we actually want to aim, given the offset our shooting
// will have based on our speed
double offsetSpeakerY = Field.SPEAKER_Y + targetOffset.getY();
double offsetSpeakerX;

if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) {
offsetSpeakerX = Field.RED_SPEAKER_X + targetOffset.getX();
} else {
Expand All @@ -92,7 +99,7 @@ public double calculateSpeakerAngleDifference() {
double offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaX, 2) + Math.pow(offsetDeltaY, 2));
SmartDashboard.putString("offset amount", targetOffset.toString());
SmartDashboard.putString("offset speaker location",
new Translation2d(offsetSpeakerX, offsetSpeakerY).toString());
new Translation2d(offsetSpeakerX, offsetSpeakerY).toString());
// getting desired robot angle
double totalDistToSpeaker = Math
.sqrt(Math.pow(offsetSpeakerdist, 2) + Math.pow(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT, 2));
Expand All @@ -106,12 +113,16 @@ public double calculateSpeakerAngleDifference() {
desiredShooterAngle = ShooterConstants.MAXIMUM_SHOOTER_ANGLE;
}
SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle);

double differenceAngle = (desiredShooterAngle - this.getShooterAngle());
SmartDashboard.putNumber("differenceAngleShooter", differenceAngle);

return differenceAngle;
}


@Override
public void periodic() {
SmartDashboard.putNumber("shooter angle encoder position", absoluteEncoder.getPosition());
SmartDashboard.putNumber("shooter encoder zero offset", absoluteEncoder.getZeroOffset());
}
}
13 changes: 8 additions & 5 deletions src/main/java/frc/robot/subsystems/IndexerSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.robot.subsystems;

import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkAnalogSensor;
import com.revrobotics.CANSparkLowLevel.MotorType;
Expand All @@ -15,12 +16,10 @@ public class IndexerSubsystem extends SubsystemBase {

public IndexerSubsystem() {
m_IndexerMotor = new CANSparkMax(IndexerConstants.INDEXER_MOTOR, MotorType.kBrushless);
m_IndexerMotor.setInverted(false);
m_IndexerMotor.burnFlash();
m_DistanceSensor = m_IndexerMotor.getAnalog(Mode.kAbsolute);
}

public boolean isNoteIn() {
return m_DistanceSensor.getVoltage()>2;
}

public void on() {
m_IndexerMotor.set(IndexerConstants.INDEXER_SPEED);
Expand All @@ -33,8 +32,12 @@ public void off() {
public void reverse() {
m_IndexerMotor.set(-IndexerConstants.INDEXER_SPEED);
}
public boolean isNoteIn() {
return m_DistanceSensor.getVoltage()>2;
}

@Override
public void periodic() {
SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage());
SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage());
}
}
49 changes: 29 additions & 20 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,9 @@
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;

import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.settings.Constants.IntakeConstants;
public class IntakeSubsystem extends SubsystemBase {
Expand All @@ -24,40 +27,46 @@ public class IntakeSubsystem extends SubsystemBase {
double intakeRunSpeed;
public IntakeSubsystem() {
intake1 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless);
intake2 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless);
brush1 = new CANSparkMax(IntakeConstants.BRUSH_1_MOTOR, MotorType.kBrushless);
brush2 = new CANSparkMax(IntakeConstants.BRUSH_2_MOTOR, MotorType.kBrushless);
brush3 = new CANSparkMax(IntakeConstants.BRUSH_3_MOTOR, MotorType.kBrushless);

intake2 = new CANSparkMax(IntakeConstants.INTAKE_2_MOTOR, MotorType.kBrushless);
if(Preferences.getBoolean("Brushes", false)) {
brush1 = new CANSparkMax(IntakeConstants.BRUSH_1_MOTOR, MotorType.kBrushless);
brush2 = new CANSparkMax(IntakeConstants.BRUSH_2_MOTOR, MotorType.kBrushless);
brush3 = new CANSparkMax(IntakeConstants.BRUSH_3_MOTOR, MotorType.kBrushless);
brush1.setIdleMode(IdleMode.kCoast);
brush2.setIdleMode(IdleMode.kCoast);
brush3.setIdleMode(IdleMode.kCoast);
}
intake2.follow(intake1);
intake2.setInverted(true);
intake1.setIdleMode(IdleMode.kCoast);
intake2.setIdleMode(IdleMode.kCoast);
brush1.setIdleMode(IdleMode.kCoast);
brush2.setIdleMode(IdleMode.kCoast);
brush3.setIdleMode(IdleMode.kCoast);
intake1.burnFlash();
intake2.burnFlash();
}

public void intakeYes(double intakeRunSpeed) {
intake1.set(intakeRunSpeed);
brush1.set(intakeRunSpeed);
brush2.set(intakeRunSpeed);
brush3.set(intakeRunSpeed);
if(Preferences.getBoolean("Brushes", false)) {
brush1.set(intakeRunSpeed);
brush2.set(intakeRunSpeed);
brush3.set(intakeRunSpeed);
}

}
public void intakeNo(double intakeRunSpeed) {
intake1.set(-intakeRunSpeed);
brush1.set(-intakeRunSpeed);
brush2.set(-intakeRunSpeed);
brush3.set(-intakeRunSpeed);


if(Preferences.getBoolean("Brushes", false)) {
brush1.set(-intakeRunSpeed);
brush2.set(-intakeRunSpeed);
brush3.set(-intakeRunSpeed);
}
}
public void intakeOff() {
intake1.set(0);
brush2.set(0);
brush1.set(0);
brush3.set(0);
if(Preferences.getBoolean("Brushes", false)) {
brush2.set(0);
brush1.set(0);
brush3.set(0);
}
}

}
Loading

0 comments on commit 0038a65

Please sign in to comment.