Tomer-arm

src/main/java/training/commands/MoveElbowToAngle.java

@@ -0,0 +1,9 @@
+package training.commands;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class MoveElbowToAngle {
+    public static void moveElbowToAngle(Rotation2d position){
+
+    }
+}

src/main/java/training/subsystems/RobotArm/ElbowConstants.java

@@ -0,0 +1,9 @@
+package training.subsystems.RobotArm;
+
+public class ElbowConstants {
+
+	public static final double BIGINNING_POSITION = 0, KP = 20, KI = 0, KD = 0, KS = 0, KA = 0, KV = 0, KG = 0,
+		ARE_FEED_FORWARDS = 0;
+	public static final int MOTOR_ID = 0, PID_SLOT = 0;
+
+}

src/main/java/training/subsystems/RobotArm/ElbowSubsystem.java

@@ -0,0 +1,52 @@
+package training.subsystems.RobotArm;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+import utils.GBSubsystem;
+
+public class ElbowSubsystem extends GBSubsystem {
+
+	private CANSparkMax motor;
+	private Rotation2d position;
+	private static ElbowSubsystem instance;
+
+
+	private ElbowSubsystem() {
+		this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		this.position = new Rotation2d(ElbowConstants.BIGINNING_POSITION);
+		motor.getPIDController().setP(ElbowConstants.KP);
+		motor.getPIDController().setI(ElbowConstants.KI);
+		motor.getPIDController().setD(ElbowConstants.KD);
+	}
+
+	public static ElbowSubsystem getInstance() {
+		if (instance != null)
+			instance = new ElbowSubsystem();
+		return instance;
+	}
+
+	public void goToPosition(Rotation2d position) {
+		motor.getPIDController()
+			.setReference(
+				position.getRadians(),
+				CANSparkBase.ControlType.kPosition,
+				ElbowConstants.PID_SLOT,
+				ElbowConstants.ARE_FEED_FORWARDS
+			);
+	}
+
+	public Rotation2d getPosition() {
+		return Rotation2d.fromDegrees(motor.getEncoder().getPosition());
+	}
+
+
+	protected String getLogPath() {
+		return "";
+	}
+
+	@Override
+	protected void subsystemPeriodic() {}
+
+}

src/main/java/training/subsystems/RobotArm/RolerConstens.java

@@ -0,0 +1,16 @@
+package training.subsystems.RobotArm;
+
+public class RolerConstens {
+
+	public static final double KP = 20,
+			KI = 0,
+			KD = 0,
+			KS = 0,
+			KA = 0,
+			KV = 0,
+			KG = 0,
+			ARB_FEED_FOWORDS = 0;
+	public static final int MOTOR_ID = 0,
+			PID_SLOT = 0;
+
+}

src/main/java/training/subsystems/RobotArm/Roller.java

@@ -0,0 +1,67 @@
+package training.subsystems.RobotArm;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+import utils.GBSubsystem;
+
+public class Roller extends GBSubsystem {
+
+	private CANSparkMax motor;
+	private static Roller instance;
+
+	private Roller() {
+		this.motor = new CANSparkMax(RolerConstens.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		motor.getPIDController().setP(RolerConstens.KP);
+		motor.getPIDController().setI(RolerConstens.KI);
+		motor.getPIDController().setD(RolerConstens.KD);
+	}
+
+	public static Roller getInstance() {
+		if (instance != null)
+			instance = new Roller();
+		return instance;
+	}
+
+	public void moveAtSpeed(double velocity) {
+		motor.getPIDController()
+			.setReference(
+				velocity,
+				CANSparkBase.ControlType.kVelocity,
+				RolerConstens.PID_SLOT,
+				RolerConstens.ARB_FEED_FOWORDS
+			);
+	}
+
+	public Rotation2d getPower() {
+		return Rotation2d.fromDegrees(motor.getEncoder().getPosition());
+	}
+
+	protected String getLogPath() {
+		return "";
+	}
+
+
+	protected void subsystemPeriodic() {}
+
+}
+
+
+//    public void setPosition(Rotation2d position){
+//        motor.getPIDController().setReference(position.getRotations(), CANSparkBase.ControlType.kPosition);
+//
+//    }
+
+
+//    @Override
+//    public boolean isFinished() {
+//        return Math.abs(module.getAngularPosition().getDegrees() - position.getDegrees()) <= ModuleConstants.ANGULAR_TOLERANCE.getDegrees();
+//    }
+//
+//    @Override
+//    public void end(boolean interrupted) {
+//        module.stopAngularMotor();
+//        Logger.recordOutput("Angular position of module",module.getAngularPosition());
+//    }
+

src/main/java/training/subsystems/RobotArm/WristConstans.java

@@ -0,0 +1,18 @@
+package training.subsystems.RobotArm;
+
+import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
+
+public class WristConstans {
+
+	public static final double KP = 20, KI = 0, KD = 0, KS = 0, KA = 0, KV = 0, KG = 0, ARB_FEED_FOWORDS = 0,
+		MAG_ENCODER_CONVERSION_FACTOR = 8192;
+	public static final int MOTOR_ID = 0, PID_SLOT = 0, TIME_OUT_FOR_CONFIGS_SET = 0;
+	public static final TalonSRXConfiguration TALON_SRX_CONFIGURATION = new TalonSRXConfiguration();
+
+	static {
+		TALON_SRX_CONFIGURATION.slot0.kP = KP;
+		TALON_SRX_CONFIGURATION.slot0.kI = KI;
+		TALON_SRX_CONFIGURATION.slot0.kD = KD;
+	}
+
+}

src/main/java/training/subsystems/RobotArm/WristSubsystem.java

@@ -0,0 +1,41 @@
+package training.subsystems.RobotArm;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import edu.wpi.first.math.geometry.Rotation2d;
+import utils.GBSubsystem;
+
+public class WristSubsystem extends GBSubsystem {
+
+	private TalonSRX motor;
+	private Rotation2d position;
+	private static WristSubsystem instance;
+
+	private WristSubsystem() {
+		this.motor = new TalonSRX(WristConstans.MOTOR_ID);
+		this.position = new Rotation2d();
+		motor.configAllSettings(WristConstans.TALON_SRX_CONFIGURATION);
+		motor.configSelectedFeedbackSensor(
+			FeedbackDevice.CTRE_MagEncoder_Relative,
+			WristConstans.PID_SLOT,
+			WristConstans.TIME_OUT_FOR_CONFIGS_SET
+		);
+	}
+
+
+	public void goToPosition(Rotation2d position) {
+		motor.selectProfileSlot(WristConstans.PID_SLOT, 0);
+		motor.set(ControlMode.Position, (position.getRotations() * WristConstans.MAG_ENCODER_CONVERSION_FACTOR));
+	}
+
+
+	@Override
+	protected String getLogPath() {
+		return "";
+	}
+
+	@Override
+	protected void subsystemPeriodic() {}
+
+}

src/main/java/utils/DefaultRobotManager.java

@@ -1,8 +1,8 @@
 package utils;
 
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import utils.logger.LoggerFactory;
 import org.littletonrobotics.junction.LoggedRobot;
+import utils.logger.LoggerFactory;
 import utils.simulation.SimulationManager;
 
 public abstract class DefaultRobotManager extends LoggedRobot {

src/main/java/utils/joysticks/Axis.java

@@ -0,0 +1,30 @@
+package utils.joysticks;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+public enum Axis {
+
+	LEFT_X(0, true),
+	LEFT_Y(1, true),
+	LEFT_TRIGGER(2, false),
+	RIGHT_TRIGGER(3, false),
+	RIGHT_X(4, false),
+	RIGHT_Y(5, true);
+
+	private final int id;
+	private final int invertedSign;
+
+	Axis(int id, boolean isInverted) {
+		this.id = id;
+		this.invertedSign = isInverted ? -1 : 1;
+	}
+
+	AxisButton getAsButton(Joystick joystick, double threshold) {
+		return new AxisButton(joystick, id, threshold);
+	}
+
+	double getValue(Joystick joystick) {
+		return invertedSign * joystick.getRawAxis(id);
+	}
+
+}

src/main/java/utils/joysticks/AxisButton.java

@@ -0,0 +1,12 @@
+package utils.joysticks;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+public class AxisButton extends Trigger {
+
+	public AxisButton(GenericHID joystick, int axis, double threshold) {
+		super(() -> Math.abs(joystick.getRawAxis(axis)) >= threshold);
+	}
+
+}

src/main/java/utils/joysticks/ButtonID.java

@@ -0,0 +1,34 @@
+package utils.joysticks;
+
+enum ButtonID {
+
+	A(1),
+	B(2),
+	X(3),
+	Y(4),
+
+	L1(5),
+	R1(6),
+
+	BACK(7),
+	START(8),
+
+	L3(9),
+	R3(10),
+
+	POV_UP(0),
+	POV_RIGHT(90),
+	POV_DOWN(180),
+	POV_LEFT(270);
+
+	private final int id;
+
+	ButtonID(int id) {
+		this.id = id;
+	}
+
+	public int getId() {
+		return id;
+	}
+
+}

src/main/java/utils/joysticks/JoystickPorts.java

@@ -0,0 +1,22 @@
+package utils.joysticks;
+
+public enum JoystickPorts {
+
+	MAIN(0),
+	SECOND(1),
+	THIRD(2),
+	FOURTH(3),
+	FIFTH(4),
+	SIXTH(5);
+
+	private final int port;
+
+	JoystickPorts(int port) {
+		this.port = port;
+	}
+
+	public int getPort() {
+		return port;
+	}
+
+}