Skip to content

Commit

Permalink
Kenneth first attempt at the button arm angle movements
Browse files Browse the repository at this point in the history
  • Loading branch information
@Kenneth-Choothakan
Kenneth-Choothakan committed Feb 20, 2024
1 parent bbeca40 commit 28ca8a5
Show file tree
Hide file tree
Showing 6 changed files with 114 additions and 35 deletions.
17 changes: 10 additions & 7 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Units.*;
import edu.wpi.first.wpilibj.XboxController.Button;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand All @@ -28,9 +29,11 @@ public static final class Arm {
public final static int RIGHT_MOTOR_PORT = 8;
//all angles in rot here
//TODO: finish understand why this is broken public static final Measure<Angle> INTAKE_ANGLE = Degrees.to(-1);
public static final double intakeAngle = Math.toDegrees(-0.1);
public static final double ampAngle = Math.toDegrees(0.3);
public static final double speakerAngle = Math.toDegrees(0.4);

// USE RADIANS FOR THE ARM
public static final double intakeAngle = Math.toRadians(0);
public static final double ampAngle = Math.toRadians(103);
public static final double speakerAngle = Math.toRadians(24);
//if needed
public static final double UPPER_ANGLE_LIMIT = Math.toRadians(70);
public static final double LOWER_ANGLE_LIMIT = Math.toRadians(0);
Expand All @@ -55,10 +58,10 @@ public static final class Arm {
public static final double MAX_FF_ACCEL = 1; // rot / s^2
public static TrapezoidProfile.Constraints trapConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);

//trapezoid profile numbers
public static final double kMaxV = 0;
public static final double kMaxA = 0;

//Arm buttons
public static final int raiseToSpeakerButton = Button.kY.value;
public static final int raiseToAmpButton = Button.kB.value;
public static final int raiseToGroundButton = Button.kA.value;
}
public static final class IntakeShooter {
//in set() speed
Expand Down
8 changes: 7 additions & 1 deletion src/main/java/org/carlmontrobotics/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,9 @@
import static org.carlmontrobotics.Constants.OI;

import org.carlmontrobotics.commands.ArmTeleop;
import org.carlmontrobotics.commands.armAmpPos;
import org.carlmontrobotics.commands.armGroundPos;
import org.carlmontrobotics.commands.armSpeakerPos;
import org.carlmontrobotics.subsystems.Arm;

import edu.wpi.first.wpilibj.GenericHID;
Expand Down Expand Up @@ -46,7 +49,7 @@ public RobotContainer() {

private void setDefaultCommands() {

arm.setDefaultCommand(new InstantCommand(() -> {arm.driveArm());
//arm.setDefaultCommand(new InstantCommand(() -> {arm.driveArm());

// drivetrain.setDefaultCommand(new TeleopDrive(
// drivetrain,
Expand All @@ -67,6 +70,9 @@ private void setBindingsManipulator() {
//3 setpositions of arm on letterpad
//Up is Speaker, down is ground, right is Amp
//right joystick used for manual arm control
new JoystickButton(manipulatorController, Constants.Arm.raiseToSpeakerButton).onTrue(new InstantCommand(() -> {arm.cancelArmCommand(); new armSpeakerPos(arm);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToAmpButton).onTrue(new InstantCommand(() -> {arm.cancelArmCommand(); new armAmpPos(arm);}));
new JoystickButton(manipulatorController, Constants.Arm.raiseToGroundButton).onTrue(new InstantCommand(() -> {arm.cancelArmCommand(); new armGroundPos(arm);}));
}

public Command getAutonomousCommand() {
Expand Down
25 changes: 20 additions & 5 deletions src/main/java/org/carlmontrobotics/commands/armAmpPos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,26 +3,41 @@
// the WPILib BSD license file in the root directory of this project.

package org.carlmontrobotics.commands;

import org.carlmontrobotics.subsystems.Arm;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;

public class armAmpPos extends Command {
/** Creates a new armPosAmp. */
public armAmpPos() {
private Arm arm;
private Timer armProfileTimer = new Timer();
public armAmpPos(Arm arm) {
// Use addRequirements() here to declare subsystem dependencies.
arm = this.arm;
addRequirements(arm);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
armProfileTimer.reset();
armProfileTimer.start();
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
public void execute() {
TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 1);
arm.COMBINE_PID_FF_TRAPEZOID(setpoint);
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
public void end(boolean interrupted) {
armProfileTimer.stop();
armProfileTimer.reset();
}

// Returns true when the command should end.
@Override
Expand Down
28 changes: 22 additions & 6 deletions src/main/java/org/carlmontrobotics/commands/armGroundPos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,26 +3,42 @@
// the WPILib BSD license file in the root directory of this project.

package org.carlmontrobotics.commands;

import org.carlmontrobotics.Constants;
import org.carlmontrobotics.subsystems.Arm;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;

public class armGroundPos extends Command {
/** Creates a new armGroundPos. */
public armGroundPos() {
/** Creates a new armPosAmp. */
private Arm arm;
private Timer armProfileTimer = new Timer();
public armGroundPos(Arm arm) {
// Use addRequirements() here to declare subsystem dependencies.
arm = this.arm;
addRequirements(arm);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
armProfileTimer.reset();
armProfileTimer.start();
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
public void execute() {
TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 0);
arm.COMBINE_PID_FF_TRAPEZOID(setpoint);
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
public void end(boolean interrupted) {
armProfileTimer.stop();
armProfileTimer.reset();
}

// Returns true when the command should end.
@Override
Expand Down
27 changes: 21 additions & 6 deletions src/main/java/org/carlmontrobotics/commands/armSpeakerPos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,26 +3,41 @@
// the WPILib BSD license file in the root directory of this project.

package org.carlmontrobotics.commands;

import org.carlmontrobotics.subsystems.Arm;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;

public class armSpeakerPos extends Command {
/** Creates a new armSpeakerPos. */
public armSpeakerPos() {
/** Creates a new armPosAmp. */
private Arm arm;
private Timer armProfileTimer = new Timer();
public armSpeakerPos(Arm arm) {
// Use addRequirements() here to declare subsystem dependencies.
arm = this.arm;
addRequirements(arm);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
armProfileTimer.reset();
armProfileTimer.start();
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
public void execute() {
TrapezoidProfile.State setpoint = arm.calculateSetPoint(armProfileTimer.get(), arm.getCurrentArmState(), 2);
arm.COMBINE_PID_FF_TRAPEZOID(setpoint);
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
public void end(boolean interrupted) {
armProfileTimer.stop();
armProfileTimer.reset();
}

// Returns true when the command should end.
@Override
Expand Down
44 changes: 34 additions & 10 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,19 @@
import org.carlmontrobotics.lib199.MotorControllerFactory;

import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.SparkAbsoluteEncoder;
import com.revrobotics.SparkPIDController;
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.SparkPIDController.AccelStrategy;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.XboxController;
Expand All @@ -40,14 +45,21 @@ public class Arm extends SubsystemBase {
//there is only one arm motor.
private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(kS, kV);
private final SparkAbsoluteEncoder armEncoder = armMotor1.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
private final PIDController armPID = new PIDController(kP, kI, kD);
public static TrapezoidProfile.State[] goalState = { new TrapezoidProfile.State(-Math.PI / 2, 0), new TrapezoidProfile.State(Math.toRadians(43), 0) };
private final SparkPIDController armPID = armMotor1.getPIDController();
public static TrapezoidProfile.State[] goalState = {
new TrapezoidProfile.State(Constants.Arm.intakeAngle, 0),
new TrapezoidProfile.State(Constants.Arm.ampAngle, 0),
new TrapezoidProfile.State(Constants.Arm.speakerAngle, 0),
};

TrapezoidProfile.Constraints constraints =new TrapezoidProfile.Constraints(kMaxV, kMaxA);

TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile profile = new TrapezoidProfile(Constants.Arm.trapConstraints);


public Arm() {
armPID.setP(Constants.Arm.kP);
armPID.setI(Constants.Arm.kI);
armPID.setD(Constants.Arm.kD);
}

//arm
/*
have 3 set positions
Expand All @@ -58,8 +70,12 @@ public class Arm extends SubsystemBase {
There will also be a manual control for the arm using the right joystick
*/
}


public TrapezoidProfile.State calculateSetPoint(double elapsedSeconds,TrapezoidProfile.State setPoint, int goalStateIndex) {
return profile.calculate(elapsedSeconds, setPoint, goalState[goalStateIndex]);
}

public void setArmGoal(double targetPosition, double targetVelocity) {
//Sets arm to the optimal angle for amp, speaker and Ground intake | used to score in amp
//these values are in constants
Expand All @@ -69,13 +85,21 @@ public void setArmGoal(double targetPosition, double targetVelocity) {

}

public void COMBINE_PID_FF_TRAPEZOID(TrapezoidProfile.State setPoint) {
// feed forward still needs the math part
double armFeedVolts = armFeed.calculate(setPoint.velocity, 0);
armPID.setReference(setPoint.position, CANSparkMax.ControlType.kPosition,0, armFeedVolts);
}

public double getArmPos() {
return MathUtil.inputModulus(armEncoder.getPosition(), Constants.Arm.ARM_DICONT_RAD,
Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI);
}

public double getKg() {
return 17; //This is a placeholder number

public void cancelArmCommand() {
Command currentArmCommand = getCurrentCommand();
currentArmCommand.cancel();
}

public Translation2d getCoM() {
Expand Down Expand Up @@ -114,8 +138,8 @@ public TrapezoidProfile.State getCurrentArmState() {
return new TrapezoidProfile.State(getArmPos(), getArmVel());
}


@Override
public void periodic() {

}
}

0 comments on commit 28ca8a5

Please sign in to comment.