-
Notifications
You must be signed in to change notification settings - Fork 0
Ofir #8
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
ofirKAMInetsky
wants to merge
49
commits into
main
Choose a base branch
from
arm
base: main
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
Ofir #8
Changes from all commits
Commits
Show all changes
49 commits
Select commit
Hold shift + click to select a range
5ad3e66
Constants
ofirKAMInetsky 8e2b938
Update ArmConstants.java
ofirKAMInetsky 99e624e
Update ArmConstants.java
ofirKAMInetsky 8769ff4
updated ArmConstants
ofirKAMInetsky 7a39af8
updated ArmConstants
ofirKAMInetsky 8128bdf
updated ArmConstants.java
ofirKAMInetsky 2328764
Update ArmConstants.java
ofirKAMInetsky 28a7ab0
FIX
ofirKAMInetsky 82882a0
FIX
ofirKAMInetsky 99805bb
changed the names
ofirKAMInetsky 773f4f3
___FIXED___
ofirKAMInetsky 113172a
Update ArmConstants.java
ofirKAMInetsky cb29753
Update ArmConstants.java
ofirKAMInetsky b5f06cb
Update ArmConstants.java
ofirKAMInetsky b10639e
Create ArmSubsystem.java
ofirKAMInetsky 75998fa
Update ArmConstants.java
ofirKAMInetsky 50fbce0
____added___motors___
ofirKAMInetsky b75612e
made it static
ofirKAMInetsky 32bb477
Update ArmSubsystem.java
ofirKAMInetsky ca981c8
==!!!___updated___!!!==
ofirKAMInetsky 2385256
i dont know what it is
ofirKAMInetsky 1dcd1e4
added angle methods
ofirKAMInetsky 1372f92
I added the profile of the elevator
ofirKAMInetsky 656b3d8
corrected the last profile generate
ofirKAMInetsky 2227ae3
added and fixed things
ofirKAMInetsky f8d3f49
Update Arm.java
ofirKAMInetsky aa18345
Fixed .run folder
ofirKAMInetsky 41faad4
Fixed the .gitignore file
ofirKAMInetsky 097d24d
Revert "Fixed .run folder"
ofirKAMInetsky fc7840a
Fixed the .run folder
ofirKAMInetsky bb4e12b
Update Arm.java
ofirKAMInetsky 6586e6d
Update ArmConstants.java
ofirKAMInetsky 847ed6d
fixed something
ofirKAMInetsky b19c37d
fix
ofirKAMInetsky 14a9e97
!@#$%^&*()_FIX_)(*&^%$#@!
ofirKAMInetsky 2aa4458
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!--FIX--!!!!!!!!!!!!!!!!!!…
ofirKAMInetsky a35019b
FFFFFFFFFFFFFIXXXXXXXXXXXXXXX
ofirKAMInetsky 2f10641
Update Arm.java
ofirKAMInetsky ddae22a
Update Arm.java
ofirKAMInetsky a8023f9
Merge branch 'main' into arm
ofirKAMInetsky 77bd329
-=+=-__FIX__added the Deferred Command__FIX__-=+=-
ofirKAMInetsky 6e55d4c
updated and fixed
ofirKAMInetsky 7a8efe6
Update Arm.java
ofirKAMInetsky fc4b2b5
Update Arm.java
ofirKAMInetsky 71b1adb
Update Arm.java
ofirKAMInetsky a5dc6d7
Update Arm.java
ofirKAMInetsky aecf2a1
Update Arm.java
ofirKAMInetsky af693b1
Update Arm.java
ofirKAMInetsky 6f5dbc0
Moved all the commands ArmCommands class.
ofirKAMInetsky File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,180 @@ | ||
package frc.trigon.robot.subsystems.arm; | ||
|
||
import com.ctre.phoenix.motorcontrol.can.TalonSRX; | ||
import com.revrobotics.CANSparkMax; | ||
import edu.wpi.first.math.geometry.Rotation2d; | ||
import edu.wpi.first.math.trajectory.TrapezoidProfile; | ||
import edu.wpi.first.math.util.Units; | ||
import edu.wpi.first.wpilibj.Timer; | ||
import edu.wpi.first.wpilibj2.command.*; | ||
import frc.trigon.robot.utilities.Conversions; | ||
|
||
import java.util.Set; | ||
|
||
public class Arm extends SubsystemBase { | ||
ofirKAMInetsky marked this conversation as resolved.
Show resolved
Hide resolved
ofirKAMInetsky marked this conversation as resolved.
Show resolved
Hide resolved
ofirKAMInetsky marked this conversation as resolved.
Show resolved
Hide resolved
ofirKAMInetsky marked this conversation as resolved.
Show resolved
Hide resolved
ofirKAMInetsky marked this conversation as resolved.
Show resolved
Hide resolved
|
||
private final static Arm INSTANCE = new Arm(); | ||
|
||
private final CANSparkMax | ||
masterAngleMotor = ArmConstants.MASTER_ANGLE_MOTOR, | ||
followerAngleMotor = ArmConstants.FOLLOWER_ANGLE_MOTOR, | ||
masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR, | ||
followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR; | ||
private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER; | ||
|
||
private TrapezoidProfile | ||
angleMotorProfile = null, | ||
elevatorMotorProfile = null; | ||
private double | ||
lastAngleProfileGenerationTime, | ||
lastElevatorProfileGenerationTime; | ||
|
||
public static Arm getInstance() { | ||
return INSTANCE; | ||
} | ||
|
||
private Arm() { | ||
} | ||
ofirKAMInetsky marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
||
Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { | ||
if (isElevatorOpening(elevatorPosition)) { | ||
return new SequentialCommandGroup( | ||
getSetTargetAngleCommand(angle, angleSpeedPercentage), | ||
getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentage) | ||
); | ||
} | ||
return new SequentialCommandGroup( | ||
getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentage), | ||
getSetTargetAngleCommand(angle, angleSpeedPercentage) | ||
); | ||
} | ||
|
||
private Command getSetTargetElevatorPositionCommand(double targetElevatorPosition, double speedPercentage) { | ||
return new FunctionalCommand( | ||
() -> generateElevatorMotorProfile(targetElevatorPosition, speedPercentage), | ||
this::setTargetElevatorFromProfile, | ||
(interrupted) -> { | ||
}, | ||
() -> false, | ||
this | ||
); | ||
} | ||
|
||
private Command getSetTargetAngleCommand(Rotation2d angle, double angleSpeedPercentage) { | ||
return new FunctionalCommand( | ||
() -> generateAngleMotorProfile(angle, angleSpeedPercentage), | ||
this::setTargetAngleFromProfile, | ||
(interrupted) -> { | ||
}, | ||
() -> false, | ||
this | ||
); | ||
} | ||
|
||
private boolean isElevatorOpening(double targetElevatorPosition) { | ||
return targetElevatorPosition > getElevatorPositionRevolutions(); | ||
} | ||
|
||
private void generateAngleMotorProfile(Rotation2d targetAngle, double speedPercentage) { | ||
angleMotorProfile = new TrapezoidProfile( | ||
Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINS, speedPercentage), | ||
new TrapezoidProfile.State(targetAngle.getDegrees(), 0), | ||
new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSecond()) | ||
); | ||
lastAngleProfileGenerationTime = Timer.getFPGATimestamp(); | ||
} | ||
|
||
private void generateElevatorMotorProfile(double targetElevator, double speedPercentage) { | ||
elevatorMotorProfile = new TrapezoidProfile( | ||
Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINS, speedPercentage), | ||
new TrapezoidProfile.State(targetElevator, 0), | ||
new TrapezoidProfile.State(getElevatorPositionRevolutions(), getElevatorVelocityRevolutionsPerSecond()) | ||
); | ||
lastElevatorProfileGenerationTime = Timer.getFPGATimestamp(); | ||
} | ||
|
||
private void setTargetAngleFromProfile() { | ||
if (angleMotorProfile == null) { | ||
stopAngleMotors(); | ||
return; | ||
} | ||
TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); | ||
setAngleMotorsVoltage(calculateAngleMotorOutput(targetState)); | ||
} | ||
|
||
private void setTargetElevatorFromProfile() { | ||
if (elevatorMotorProfile == null) { | ||
stopElevatorMotors(); | ||
return; | ||
} | ||
TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime()); | ||
setElevatorMotorsVoltage(calculateElevatorMotorOutput(targetState)); | ||
} | ||
|
||
private double getAngleMotorProfileTime() { | ||
return Timer.getFPGATimestamp() - lastAngleProfileGenerationTime; | ||
} | ||
|
||
private double getElevatorMotorProfileTime() { | ||
return Timer.getFPGATimestamp() - lastElevatorProfileGenerationTime; | ||
} | ||
|
||
private double calculateAngleMotorOutput(TrapezoidProfile.State targetState) { | ||
double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate( | ||
getAnglePosition().getDegrees(), | ||
targetState.position | ||
); | ||
double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( | ||
Units.degreesToRadians(targetState.position), | ||
Units.degreesToRadians(targetState.velocity) | ||
ofirKAMInetsky marked this conversation as resolved.
Show resolved
Hide resolved
|
||
); | ||
return feedforward + pidOutput; | ||
} | ||
|
||
private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState) { | ||
double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate( | ||
getElevatorPositionRevolutions(), | ||
targetState.position | ||
); | ||
double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(targetState.velocity); | ||
return feedforward + pidOutput; | ||
} | ||
|
||
private double getAngleVelocityDegreesPerSecond() { | ||
double positionRevolutions = ArmConstants.ANGLE_VELOCITY_SIGNAL.refresh().getValue(); | ||
return Conversions.revolutionsToDegrees(positionRevolutions); | ||
} | ||
|
||
private double getElevatorVelocityRevolutionsPerSecond() { | ||
double magTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorEncoder.getSelectedSensorVelocity()); | ||
return Conversions.magTicksToRevolutions(magTicksPerSecond); | ||
} | ||
|
||
private Rotation2d getAnglePosition() { | ||
double positionRevolutions = ArmConstants.ANGLE_POSITION_SIGNAL.refresh().getValue(); | ||
return Rotation2d.fromRotations(positionRevolutions); | ||
} | ||
|
||
private double getElevatorPositionRevolutions() { | ||
return Conversions.magTicksToRevolutions(elevatorEncoder.getSelectedSensorPosition()); | ||
} | ||
|
||
private void stopAngleMotors() { | ||
masterAngleMotor.stopMotor(); | ||
followerAngleMotor.stopMotor(); | ||
} | ||
|
||
private void stopElevatorMotors() { | ||
masterElevatorMotor.stopMotor(); | ||
followerElevatorMotor.stopMotor(); | ||
} | ||
|
||
private void setAngleMotorsVoltage(double voltage) { | ||
masterAngleMotor.setVoltage(voltage); | ||
followerAngleMotor.setVoltage(voltage); | ||
} | ||
|
||
private void setElevatorMotorsVoltage(double voltage) { | ||
masterElevatorMotor.setVoltage(voltage); | ||
followerElevatorMotor.setVoltage(voltage); | ||
} | ||
} |
33 changes: 33 additions & 0 deletions
33
src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,33 @@ | ||
package frc.trigon.robot.subsystems.arm; | ||
|
||
import edu.wpi.first.math.geometry.Rotation2d; | ||
import edu.wpi.first.wpilibj2.command.Command; | ||
import edu.wpi.first.wpilibj2.command.DeferredCommand; | ||
|
||
import java.util.Set; | ||
|
||
public class ArmCommands { | ||
private static final Arm ARM = Arm.getInstance(); | ||
|
||
public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState) { | ||
return getSetTargetArmStateCommand(targetState.elevatorPositionMeters, targetState.angle, 100, 100); | ||
} | ||
|
||
public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState, double angleSpeedPercentage, double elevatorSpeedPercentage) { | ||
return getSetTargetArmStateCommand(targetState.elevatorPositionMeters, targetState.angle, angleSpeedPercentage, elevatorSpeedPercentage); | ||
} | ||
|
||
public Command getSetTargetArmStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { | ||
return new DeferredCommand( | ||
() -> ARM.getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage), | ||
Set.of(ARM) | ||
); | ||
} | ||
|
||
public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle) { | ||
return new DeferredCommand( | ||
() -> ARM.getCurrentSetTargetStateCommand(elevatorPosition, angle, 100, 100), | ||
Set.of(ARM) | ||
); | ||
} | ||
} |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.