Skip to content
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.

Sign up for GitHub

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

Using Elbow IMU #33

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Using Elbow IMU #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
873737b
using imu with elbow
katiewang22 Apr 3, 2023
e48666c
elbow PID in periodic()
katiewang22 Apr 3, 2023
bac3a43
testing elbow changes
katiewang22 Apr 4, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 23 additions & 16 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ public static final class CAN_IDS {
public static final int RIGHT_ELBOW_MOTOR = 54;
public static final int INTAKE_MOTOR = 55;
public static final int CANdle = 34;
public static final int ELBOW_IMU = 6;
}

public static final class PORTS {
Expand Down Expand Up @@ -247,60 +248,66 @@ public static final class OUTTAKE_STATE {

public static final class GROUND_INTAKE {
public static final double SHOULDER_POSITION = 9500.0; //13500.0;
public static final double ELBOW_POSITION = 54500.0; // 59500.0;
public static final double ALLOWANCE = 100.0;
public static final double ELBOW_POSITION = 0.0; // TODO
public static final double SHOULDER_ALLOWANCE = 100.0;
public static final double ELBOW_ALLOWANCE = 0.0; // TODO
public static final double TIME = 0.3;
}

public static final class HIGH_SCORE_CONE {
public static final double SHOULDER_POSITION = 68500.0; // 71500.0;
public static final double ELBOW_POSITION = 90000.0; // 84300.0;
public static final double ALLOWANCE = 100.0;
public static final double ELBOW_POSITION = 0.0; // TODO
public static final double SHOULDER_ALLOWANCE = 100.0;
public static final double ELBOW_ALLOWANCE = 0.0; // TODO
public static final double TIME = 0.2;
}

public static final class HIGH_SCORE_CUBE {
public static final double SHOULDER_POSITION = 62000.0;
public static final double ELBOW_POSITION = 80000.0;
public static final double ALLOWANCE = 100.0;
public static final double TIME = 0.5;
public static final double ELBOW_POSITION = 0.0; // TODO
public static final double SHOULDER_ALLOWANCE = 100.0;
public static final double ELBOW_ALLOWANCE = 0.0; // TODO
public static final double TIME = 0.2;
}

public static final class MID_SCORE_CONE {
public static final double SHOULDER_POSITION = 75500.0; // 44000.0;
public static final double ELBOW_POSITION = 125000.0; // 57000.0;
public static final double ELBOW_POSITION = 0.0; // TODO
}

public static final class MID_SCORE_CUBE {
public static final double SHOULDER_POSITION = 20000; // 38400.0;
public static final double ELBOW_POSITION = 27000; // 66100.0;
public static final double ELBOW_POSITION = 0.0; // TODO
}

public static final class LOW_SCORE_CONE {
public static final double SHOULDER_POSITION = 12250.0;
public static final double ELBOW_POSITION = 51000.0;
public static final double ELBOW_POSITION = 0.0; // TODO
}

public static final class LOW_SCORE_CUBE {
public static final double SHOULDER_POSITION = 12250.0;
public static final double ELBOW_POSITION = 51000.0;
public static final double ELBOW_POSITION = 0.0; // TODO
}

public static final class SUBSTATION_INTAKE {
public static final double SHOULDER_POSITION = 65000.0;
public static final double ELBOW_POSITION = 92500.0;
public static final double ALLOWANCE = 100.0;
public static final double ELBOW_POSITION = 0.0; // TODO
public static final double SHOULDER_ALLOWANCE = 100.0;
public static final double ELBOW_ALLOWANCE = 0.0; // TODO
public static final double TIME = 0.5;
}

public static final class ELBOW_IDLE {
public static final double ELBOW_POSITION = 30000.0;
public static final double ALLOWANCE = 10000.0;
public static final double SHOULDER_ALLOWANCE = 100.0; // TODO
public static final double ELBOW_POSITION = 0.0; // TODO
public static final double ELBOW_ALLOWANCE = 0.0; // TODO
public static final double TIME = 0.2;
}

public static final class IDLE {
public static final double ALLOWANCE = 1000.0;
public static final double SHOULDER_ALLOWANCE = 1000.0;
public static final double ELBOW_ALLOWANCE = 0.0; // TODO
public static final double TIME = 0.2;
}

Expand Down
6 changes: 6 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
import frc.robot.lib.auto.AutoModeBase;
import frc.robot.lib.auto.AutoModeExecutor;
import frc.robot.lib.auto.AutoModeSelector;
import frc.robot.lib.interfaces.Arm;
import frc.robot.lib.interfaces.Intake;
import frc.robot.lib.interfaces.LED;
import frc.robot.lib.interfaces.Swerve;
Expand Down Expand Up @@ -70,11 +71,16 @@ public void robotPeriodic() {
RobotMap.swerve.updatePoses();
Swerve.periodic();
LED.periodic();
Arm.periodic();

// see robot pose on Glass
// RobotMap.Field2d.setRobotPose(Swerve.swerveOdometry.getEstimatedPosition());
FieldObject2d currentPoseDisplay = RobotMap.Field2d.getObject("currentPose");
currentPoseDisplay.setPose(Swerve.swerveOdometry.getEstimatedPosition());

SmartDashboard.putNumber("Elbow pitch", RobotMap.elbowIMU.getPitch());
// SmartDashboard.putNumber("Elbow roll", RobotMap.elbowIMU.getRoll());
// SmartDashboard.putNumber("Elbow yaw", RobotMap.elbowIMU.getYaw());

// SmartDashboard.putNumber("Integrated Encoder Shoulder (L)", RobotMap.leftShoulderMotor.getSelectedSensorPosition());
// SmartDashboard.putNumber("Integrated Encoder Elbow (L)", RobotMap.leftElbowMotor.getSelectedSensorPosition());
Expand Down
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/RobotMap.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ public class RobotMap {
public static WPI_TalonFX leftShoulderMotor;
public static WPI_TalonFX rightShoulderMotor;
public static VictorSPX intakeMotor;
public static WPI_Pigeon2 elbowIMU;

/* Smart Dashboard Instances */
public static Field2d Field2d;
Expand All @@ -53,13 +54,15 @@ public static void init() {
leftElbowMotor = new WPI_TalonFX(Constants.CAN_IDS.LEFT_ELBOW_MOTOR);
rightElbowMotor = new WPI_TalonFX(Constants.CAN_IDS.RIGHT_ELBOW_MOTOR);
intakeMotor = new VictorSPX(Constants.CAN_IDS.INTAKE_MOTOR);
elbowIMU = new WPI_Pigeon2(Constants.CAN_IDS.ELBOW_IMU);

gyro.configFactoryDefault();
leftShoulderMotor.configFactoryDefault();
rightShoulderMotor.configFactoryDefault();
leftElbowMotor.configFactoryDefault();
rightElbowMotor.configFactoryDefault();
intakeMotor.configFactoryDefault();
elbowIMU.configFactoryDefault();

swerve = new Swerve();
/* By pausing init for a second before setting module offsets, we avoid a bug with inverting motors.
Expand Down
73 changes: 60 additions & 13 deletions src/main/java/frc/robot/lib/interfaces/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,11 @@
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Constants;
import frc.robot.RobotMap;
import frc.robot.lib.statemachine.State;
Expand All @@ -22,6 +26,22 @@ public class Arm {
private static double shoulderTarget = 0.0;
private static double elbowTarget = 0.0;

private static ProfiledPIDController controller1 = new ProfiledPIDController(
0.006,
0.0,
0.0,
new TrapezoidProfile.Constraints(100.0, 1600.0)
);

private static ProfiledPIDController controller2 = new ProfiledPIDController(
0.006,
0.0,
0.0,
new TrapezoidProfile.Constraints(100.0, 1600.0)
);

private static double elbowCommand = 0.0;

public Arm() {
RobotMap.rightShoulderMotor.setInverted(true);
RobotMap.rightElbowMotor.setInverted(true);
Expand All @@ -45,14 +65,14 @@ public Arm() {

RobotMap.leftElbowMotor.configNominalOutputForward(0, 10);
RobotMap.leftElbowMotor.configNominalOutputReverse(0, 10);
RobotMap.leftElbowMotor.configPeakOutputForward(0.17, 10);
RobotMap.leftElbowMotor.configPeakOutputReverse(-0.25, 10);
RobotMap.leftElbowMotor.configPeakOutputForward(1.0, 10);
RobotMap.leftElbowMotor.configPeakOutputReverse(-1.0, 10);
RobotMap.leftElbowMotor.configNeutralDeadband(0.001, 10);

RobotMap.rightElbowMotor.configNominalOutputForward(0, 10);
RobotMap.rightElbowMotor.configNominalOutputReverse(0, 10);
RobotMap.rightElbowMotor.configPeakOutputForward(0.17, 10);
RobotMap.rightElbowMotor.configPeakOutputReverse(-0.25, 10);
RobotMap.rightElbowMotor.configPeakOutputForward(1.0, 10);
RobotMap.rightElbowMotor.configPeakOutputReverse(-1.0, 10);
RobotMap.rightElbowMotor.configNeutralDeadband(0.001, 10);

RobotMap.leftShoulderMotor.configAllowableClosedloopError(0, 0, 10);
Expand All @@ -75,31 +95,58 @@ public Arm() {
RobotMap.leftElbowMotor.configReverseSoftLimitThreshold(Constants.ELBOW_MOTOR.B_TRAVEL_LIMIT);
RobotMap.rightElbowMotor.configReverseSoftLimitThreshold(Constants.ELBOW_MOTOR.B_TRAVEL_LIMIT);

// TODO: config mount pose of IMU
RobotMap.elbowIMU.configMountPose(-90.0, -90.0, 0);

RobotMap.rightShoulderMotor.set(ControlMode.Follower, Constants.CAN_IDS.LEFT_SHOULDER_MOTOR);
RobotMap.rightElbowMotor.set(ControlMode.Follower, Constants.CAN_IDS.LEFT_ELBOW_MOTOR);
//RobotMap.rightElbowMotor.set(ControlMode.Follower, Constants.CAN_IDS.LEFT_ELBOW_MOTOR);
}

public static void periodic() {
if (RobotMap.elbowIMU.getPitch() < -80.0) {
RobotMap.leftElbowMotor.set(0.1);
RobotMap.rightElbowMotor.set(0.1);
} else if (RobotMap.elbowIMU.getPitch() > 80.0) {
RobotMap.leftElbowMotor.set(-0.1);
RobotMap.rightElbowMotor.set(-0.1);
} else if (RobotMap.armStateMachine.getCurrentState() != ArmStateMachine.manualMoveState) {
RobotMap.leftElbowMotor.set(controller1.calculate(RobotMap.elbowIMU.getPitch()));
RobotMap.rightElbowMotor.set(controller2.calculate(RobotMap.elbowIMU.getPitch()));
}

SmartDashboard.putNumber("Left elbow motor power", RobotMap.leftElbowMotor.getMotorOutputPercent());
SmartDashboard.putNumber("Right elbow motor power", RobotMap.rightElbowMotor.getMotorOutputPercent());


SmartDashboard.putNumber("Left setpoint position", controller1.getSetpoint().position);
SmartDashboard.putNumber("Right setpoint velocity", controller2.getSetpoint().velocity);
}

public void setIdle() {
moveArmPosition(100.0, 0.0);
// TODO: IMU degree
moveArmPosition(100.0, -70.0);
}

public void setElbowIdle() {
RobotMap.leftShoulderMotor.getSelectedSensorPosition();
RobotMap.leftElbowMotor.set(TalonFXControlMode.Position, Constants.ELBOW_IDLE.ELBOW_POSITION);
elbowTarget = Constants.ELBOW_IDLE.ELBOW_POSITION;
moveArmPosition(shoulderTarget, Constants.ELBOW_IDLE.ELBOW_POSITION);
}

// encoder position for the shoulder, IMU degree for the elbow
public void moveArmPosition(double shoulder, double elbow) {
RobotMap.leftShoulderMotor.set(TalonFXControlMode.Position, shoulder);
RobotMap.leftElbowMotor.set(TalonFXControlMode.Position, elbow);
shoulderTarget = shoulder;
elbowTarget = elbow;
System.out.println("Setting shoulder target to: " + shoulder);
System.out.println("Setting elbow target to: " + elbow);
controller1.setGoal(elbowTarget + 2.0);
controller2.setGoal(elbowTarget - 2.0);
}

// method to check if arm has arrived at its position
public static Boolean getArrived(double allowance, double time) {
if (Math.abs(RobotMap.leftShoulderMotor.getSelectedSensorPosition() - shoulderTarget) <= Math.abs(allowance) &&
Math.abs(RobotMap.leftElbowMotor.getSelectedSensorPosition() - elbowTarget) <= Math.abs(allowance)) {
public static Boolean getArrived(double shoulderAllowance, double elbowAllowance, double time) {
if (Math.abs(RobotMap.leftShoulderMotor.getSelectedSensorPosition() - shoulderTarget) <= Math.abs(shoulderAllowance) &&
// TODO: IMU reading
Math.abs(RobotMap.elbowIMU.getPitch() - elbowTarget) <= Math.abs(elbowAllowance)) {

if (!timerStarted) {
timer.start();
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/subsystems/arm/ElbowIdleState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,8 @@ public class ElbowIdleState extends State {
@Override
public void build() {
transitions.add(new Transition(() -> {
return Arm.getArrived(Constants.ELBOW_IDLE.ALLOWANCE, Constants.ELBOW_IDLE.TIME);
return true;
// return Arm.getArrived(Constants.ELBOW_IDLE.SHOULDER_ALLOWANCE, Constants.ELBOW_IDLE.ELBOW_ALLOWANCE, Constants.ELBOW_IDLE.TIME);
}, ArmStateMachine.idleState));

transitions.add(new Transition(() -> {
Expand Down
42 changes: 21 additions & 21 deletions src/main/java/frc/robot/subsystems/drive/autos/BotSideLink.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,9 +102,9 @@ protected void routine() throws AutoModeEndedException {
runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoreHighState)));

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CONE.ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
}));
// runAction(new ConditionAction(() -> {
// return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CONE.ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
// }));

// then, score the piece
// runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoringState)));
Expand All @@ -121,9 +121,9 @@ protected void routine() throws AutoModeEndedException {
runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.groundPickupState)));

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return RobotMap.arm.getArrived(Constants.GROUND_INTAKE.ALLOWANCE, Constants.GROUND_INTAKE.TIME);
}));
// runAction(new ConditionAction(() -> {
// return RobotMap.arm.getArrived(Constants.GROUND_INTAKE.ALLOWANCE, Constants.GROUND_INTAKE.TIME);
// }));

// then, close on the cube
// runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ClawStateMachine.closingCube)));
Expand All @@ -143,9 +143,9 @@ protected void routine() throws AutoModeEndedException {
runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoreHighState)));

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CUBE.ALLOWANCE, Constants.HIGH_SCORE_CUBE.TIME);
}));
// runAction(new ConditionAction(() -> {
// return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CUBE.ALLOWANCE, Constants.HIGH_SCORE_CUBE.TIME);
// }));

// // then, score the piece
// runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoringState)));
Expand All @@ -159,9 +159,9 @@ protected void routine() throws AutoModeEndedException {
runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoreHighState)));

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CONE.ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
}));
// runAction(new ConditionAction(() -> {
// return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CONE.ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
// }));

// then, score the piece
// runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoringState)));
Expand All @@ -178,9 +178,9 @@ protected void routine() throws AutoModeEndedException {
runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.groundPickupState)));

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return RobotMap.arm.getArrived(Constants.GROUND_INTAKE.ALLOWANCE, Constants.GROUND_INTAKE.TIME);
}));
// runAction(new ConditionAction(() -> {
// return RobotMap.arm.getArrived(Constants.GROUND_INTAKE.ALLOWANCE, Constants.GROUND_INTAKE.TIME);
// }));

// then, close on the cone
// runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ClawStateMachine.closingCone)));
Expand All @@ -200,9 +200,9 @@ protected void routine() throws AutoModeEndedException {
runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoreHighState)));

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CUBE.ALLOWANCE, Constants.HIGH_SCORE_CUBE.TIME);
}));
// runAction(new ConditionAction(() -> {
// return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CUBE.ALLOWANCE, Constants.HIGH_SCORE_CUBE.TIME);
// }));

// then, score the piece
// runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoringState)));
Expand All @@ -216,9 +216,9 @@ protected void routine() throws AutoModeEndedException {
runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoreHighState)));

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CONE.ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
}));
// runAction(new ConditionAction(() -> {
// return RobotMap.arm.getArrived(Constants.HIGH_SCORE_CONE.ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
// }));

// then, score the piece
// runAction(new LambdaAction(() -> RobotMap.armStateMachine.setCurrentState(ArmStateMachine.scoringState)));
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/drive/autos/BotSimpleScore.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ protected void routine() throws AutoModeEndedException {

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return Arm.getArrived(Constants.HIGH_SCORE_CONE.ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
return Arm.getArrived(Constants.HIGH_SCORE_CONE.SHOULDER_ALLOWANCE, Constants.HIGH_SCORE_CONE.ELBOW_ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
}));

// then, score the piece
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/drive/autos/MidScoreBalance.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ protected void routine() throws AutoModeEndedException {

// wait for arm to arrive in position
runAction(new ConditionAction(() -> {
return Arm.getArrived(Constants.HIGH_SCORE_CONE.ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
return Arm.getArrived(Constants.HIGH_SCORE_CONE.SHOULDER_ALLOWANCE, Constants.HIGH_SCORE_CONE.ELBOW_ALLOWANCE, Constants.HIGH_SCORE_CONE.TIME);
}));

// then, score the piece
Expand Down
Loading