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.

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

Tomer-arm #9

Open
wants to merge 33 commits into
base: on-robot
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
33 commits
Select commit Hold shift + click to select a range
f8c5528
Yoni - added joyysticks
itamaroryan Jul 17, 2024
ed95af0
tomer and guy - robot
itamaroryan Jul 17, 2024
23dd4a6
yotam and tomer - PID
itamaroryan Jul 21, 2024
12cd07f
guy and tomer robot code
tomer-hershman Jul 24, 2024
e0629d5
guy and tomer robot code
tomer-hershman Jul 24, 2024
6b1ef28
guy and tomer robot arm
tomer-hershman Jul 28, 2024
dc34dc6
tomer and guy sarted PID on arm
tomer-hershman Jul 31, 2024
d1a1c59
Tomer- finished PID Wrist and Roller
tomer-hershman Aug 4, 2024
012fdf1
tomer-finished
tomer-hershman Aug 4, 2024
0746e76
cr 1
tomer-hershman Aug 7, 2024
37b8586
cr 1
tomer-hershman Aug 7, 2024
7db3775
cr 1
tomer-hershman Aug 7, 2024
da02665
cr 1
tomer-hershman Aug 7, 2024
421e234
hilel and tomer spotless
tomer-hershman Aug 7, 2024
0b0b3ad
tomer - started moving stuf
tomer-hershman Aug 7, 2024
7c7b3ab
tomer-finised comends(or at least I think so)
tomer-hershman Aug 11, 2024
3e154de
tomer- worct on the hole arm manely elbow
tomer-hershman Aug 21, 2024
97414be
tomer- warked on the elbow
tomer-hershman Aug 21, 2024
a766980
tomer- warked on factory
tomer-hershman Aug 21, 2024
205a997
tomer- warked on factory
tomer-hershman Aug 21, 2024
4f8dfa9
tomer- warked on factory
tomer-hershman Aug 21, 2024
e96b5ef
tomer- warked on factory
tomer-hershman Aug 21, 2024
90b6b86
tomer- warked on factory
tomer-hershman Aug 21, 2024
09ff21b
tomer-finised shahar cr
tomer-hershman Aug 23, 2024
a512ee8
tomer-finished elbow simulation
tomer-hershman Aug 24, 2024
5f9a33b
tomer-finished elbow simulation
tomer-hershman Aug 24, 2024
0355a62
tomer-finished elbow simulation
tomer-hershman Aug 25, 2024
ce86a66
rtomer-request approval cr
tomer-hershman Aug 25, 2024
63a7ddf
rtomer-request approval cr
tomer-hershman Aug 25, 2024
d982715
rtomer-request approval cr
tomer-hershman Aug 25, 2024
ffdf1b2
rtomer-request approval cr
tomer-hershman Aug 25, 2024
3776f75
rtomer-request approval cr
tomer-hershman Aug 25, 2024
ead3740
rtomer-request approval cr
tomer-hershman Aug 25, 2024
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
7 changes: 7 additions & 0 deletions src/main/java/training/GeneralConstants.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
package training;

public class GeneralConstants {

public static RobotType ROBOT_TYPE = RobotType.SIMULATION;

}
8 changes: 8 additions & 0 deletions src/main/java/training/RobotType.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
package training;

public enum RobotType {

SIMULATION,
SYNCOPA;

}
11 changes: 11 additions & 0 deletions src/main/java/training/TrainingRobotManager.java
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
package training;

import edu.wpi.first.math.geometry.Rotation2d;
import training.commands.elbow.MoveElbowToAngle;
import training.subsystems.RobotArm.elbow.Elbow;
import utils.DefaultRobotManager;
import utils.joysticks.JoystickPorts;
import utils.joysticks.SmartJoystick;

public class TrainingRobotManager extends DefaultRobotManager {

Expand All @@ -19,6 +24,12 @@ public void trainingPeriodic() {
@Override
public void teleopInit() {
// schedule your command...
SmartJoystick smartJoystick = new SmartJoystick(JoystickPorts.MAIN);

smartJoystick.A.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(30), Elbow.getInstance()));
smartJoystick.Y.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(50), Elbow.getInstance()));
smartJoystick.B.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(80), Elbow.getInstance()));
smartJoystick.X.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(100), Elbow.getInstance()));
}


Expand Down
33 changes: 33 additions & 0 deletions src/main/java/training/commands/elbow/MoveElbowToAngle.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
package training.commands.elbow;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import training.subsystems.RobotArm.elbow.Elbow;

public class MoveElbowToAngle extends Command {

private Rotation2d position;
private Elbow elbow;

public MoveElbowToAngle(Rotation2d position, Elbow elbow) {
this.position = position;
this.elbow = elbow;
}

@Override
public void execute() {
elbow.goToAngle(position);
}

tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved

tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved
@Override
public boolean isFinished() {
return elbow.isAtPosition(position);
}

tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved
@Override
public void end(boolean interrupted) {
elbow.stayInPlace(position);
}

}
28 changes: 28 additions & 0 deletions src/main/java/training/commands/roller/MoveRollerAtSpeed.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
package training.commands.roller;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import training.subsystems.RobotArm.roller.Roller;

public class MoveRollerAtSpeed extends Command {

private Roller roller;
private Rotation2d velocity;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

double


public MoveRollerAtSpeed(Rotation2d velocity) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same, double and not rotation2d

this.velocity = velocity;
}

public void initialize() {
Roller.getInstance().moveAtSpeed(velocity.getDegrees());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

.moveAtSpeed(velocity) change velocity to a double representing the rotation

}

public boolean isFinished() {
return roller.isAtSpeed(velocity);
}

public void end(boolean interrupted) {
roller.moveAtSpeed(0);
}

}
tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved
32 changes: 32 additions & 0 deletions src/main/java/training/commands/wrist/MoveWristToAngle.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
package training.commands.wrist;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import training.subsystems.RobotArm.wrist.Wrist;

public class MoveWristToAngle extends Command {

private Wrist wristSubsystem;
private Rotation2d wantedAngle;

public MoveWristToAngle(Rotation2d wantedAngle, Wrist wristSubsystem) {
this.wantedAngle = wantedAngle;
this.wristSubsystem = wristSubsystem;
}

@Override
public void execute() {
wristSubsystem.goToPosition(wantedAngle);
}

@Override
public boolean isFinished() {
return wristSubsystem.isAtPosition(wantedAngle);
}

@Override
public void end(boolean interrupted) {
wristSubsystem.stayInPlace();
}

}
tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved
58 changes: 58 additions & 0 deletions src/main/java/training/subsystems/RobotArm/elbow/Elbow.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
package training.subsystems.RobotArm.elbow;

import edu.wpi.first.math.geometry.Rotation2d;
import org.littletonrobotics.junction.Logger;
import utils.GBSubsystem;

public class Elbow extends GBSubsystem {

private ElbowInputsAutoLogged inputs;
private Rotation2d position;
private static Elbow instance;
private final IElbow iElbow;

private Elbow() {
this.iElbow = ElbowFactory.create();
this.position = new Rotation2d(ElbowConstants.BEGINNING_POSITION);
this.inputs = new ElbowInputsAutoLogged();
}

public static Elbow getInstance() {
if (instance == null)
instance = new Elbow();
Comment on lines +21 to +22
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

סוגריים מסולסלים בבקשה

return instance;
}

public void goToAngle(Rotation2d position) {
iElbow.goToAngle(position);
}

public Rotation2d getPosition() {
return inputs.position;
}

public Rotation2d getVelocity() {
return inputs.velocity;
}
Comment on lines +34 to +36
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think velocity should be a double


tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved
public boolean isAtPosition(Rotation2d position) {
return Math.abs(getPosition().minus(position).getDegrees()) <= ElbowConstants.TOLERANCE.getDegrees();
}

public void stayInPlace(Rotation2d position) {
iElbow.stayInPlace();
}

@Override
protected String getLogPath() {
return "Elbow/";
}

@Override
protected void subsystemPeriodic() {
iElbow.updateInputs(inputs);
Logger.processInputs(getLogPath(), inputs);
}

}

Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
package training.subsystems.RobotArm.elbow;

import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;

public class ElbowConstants {

public static final double KP = 30, KI = 0, KD = 15, KS = 0, KA = 0, KV = 0, KG = 0, ARE_FEED_FORWARDS = 0;
public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(3);
public static final double BEGINNING_POSITION = 0;

public enum PresetPositions {

SAFE(Rotation2d.fromDegrees(0)),
INTAKE(Rotation2d.fromDegrees(0)),
SCORE(Rotation2d.fromDegrees(0)),
STARTING(Rotation2d.fromDegrees(0)),
TRANSFER(Rotation2d.fromDegrees(0));

public final Rotation2d ANGLE;

PresetPositions(Rotation2d angle) {
this.ANGLE = angle;
}

}

protected static ArmFeedforward ARM_FEED_FORWARD = new ArmFeedforward(KS, KG, KV, KA);

public static final int MOTOR_ID = 0, PID_SLOT = 0;

public static final double ARM_LENGTH = 0.44;

public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(-81);

public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(90);

public static final double ARM_MASS_KG = 0.44;

}
14 changes: 14 additions & 0 deletions src/main/java/training/subsystems/RobotArm/elbow/ElbowFactory.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
package training.subsystems.RobotArm.elbow;

import training.GeneralConstants;

public class ElbowFactory {

public static IElbow create() {
return switch (GeneralConstants.ROBOT_TYPE) {
case SYNCOPA -> new ElbowNEO();
case SIMULATION -> new ElbowSimulation();
};
}

}
13 changes: 13 additions & 0 deletions src/main/java/training/subsystems/RobotArm/elbow/ElbowInputs.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
package training.subsystems.RobotArm.elbow;

import edu.wpi.first.math.geometry.Rotation2d;
import org.littletonrobotics.junction.AutoLog;

@AutoLog
public class ElbowInputs {

public Rotation2d position;

public Rotation2d velocity;

}
50 changes: 50 additions & 0 deletions src/main/java/training/subsystems/RobotArm/elbow/ElbowNEO.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,50 @@
package training.subsystems.RobotArm.elbow;

import com.revrobotics.CANSparkBase;
import com.revrobotics.CANSparkLowLevel;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.math.geometry.Rotation2d;

public class ElbowNEO implements IElbow {

private final CANSparkMax motor;
private Rotation2d position;

public ElbowNEO() {
this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
this.position = new Rotation2d(ElbowConstants.BEGINNING_POSITION);
motor.getPIDController().setP(ElbowConstants.KP);
motor.getPIDController().setI(ElbowConstants.KI);
motor.getPIDController().setD(ElbowConstants.KD);
}

@Override
public void goToAngle(Rotation2d position) {
motor.getPIDController()
.setReference(
position.getDegrees(),
CANSparkBase.ControlType.kPosition,
ElbowConstants.PID_SLOT,
ElbowConstants.ARM_FEED_FORWARD.calculate(getPosition().getDegrees(), getVelocity().getDegrees())
);
}

public Rotation2d getPosition() {
return Rotation2d.fromDegrees(motor.getEncoder().getPosition());
}

public Rotation2d getVelocity() {
return Rotation2d.fromDegrees(motor.getEncoder().getVelocity());
}
tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved

tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved
@Override
public void stayInPlace() {
goToAngle(position);
}

@Override
public void updateInputs(ElbowInputsAutoLogged inputs) {
inputs.position = getPosition();
}

}
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
package training.subsystems.RobotArm.elbow;

import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.PositionVoltage;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
import org.littletonrobotics.junction.Logger;
import utils.simulation.SingleJointedArmSimulation;

public class ElbowSimulation implements IElbow {

private SingleJointedArmSimulation motor;
private Rotation2d position;
private PIDController controller;

public ElbowSimulation() {
SingleJointedArmSim armSim = new SingleJointedArmSim(
DCMotor.getFalcon500(ElbowSimulationConstants.NUMBER_OF_MOTORS),
ElbowSimulationConstants.GEAR_RATIO,
SingleJointedArmSim.estimateMOI(ElbowConstants.ARM_LENGTH, ElbowConstants.ARM_MASS_KG),
ElbowConstants.ARM_LENGTH,
ElbowConstants.BACKWARD_ANGLE_LIMIT.getRadians(),
ElbowConstants.FORWARD_ANGLE_LIMIT.getRadians(),
false,
ElbowConstants.PresetPositions.STARTING.ANGLE.getRadians()
) {

};
this.motor = new SingleJointedArmSimulation(armSim);
TalonFXConfiguration config = new TalonFXConfiguration();
config.Slot0.kP = ElbowConstants.KP;
config.Slot0.kI = ElbowConstants.KI;
config.Slot0.kD = ElbowConstants.KD;
motor.applyConfiguration(config);
}

public Rotation2d getPosition() {
return motor.getPosition();
}

tomer-hershman marked this conversation as resolved.
Show resolved Hide resolved
public void goToAngle(Rotation2d targetAngle) {
PositionVoltage pos = new PositionVoltage(targetAngle.getRotations());
motor.setControl(pos);
}

public void stop() {
goToAngle(getPosition());
}

@Override
public void stayInPlace() {
goToAngle(getPosition());
}

@Override
public void updateInputs(ElbowInputsAutoLogged inputs) {
inputs.position = getPosition();
}

}
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
package training.subsystems.RobotArm.elbow;

public class ElbowSimulationConstants {

public static final int NUMBER_OF_MOTORS = 1;

public static final double GEAR_RATIO = 1;
final static double P = 5, I = 0, D = 0;

}
Loading
Loading