Maya arm

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

@@ -0,0 +1,27 @@
+package training.commands.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import training.subsystems.elbow.Elbow;
+
+public class MoveElbowToAngle extends Command {
+
+    private Rotation2d targetAngle;
+
+    public MoveElbowToAngle(Rotation2d targetAngle){
+       this.targetAngle = targetAngle;
+    }
+
+    @Override
+    public void initialize() {
+        Elbow.goToAngel(targetAngle);
+    }
+
+    public boolean isFinished(){
+        return Elbow.getInstance().isAtAngle(targetAngle);
+    }
+
+    public void end(boolean interrupted){
+        Elbow.getInstance().stop();
+    }
+}

src/main/java/training/commands/roller/MoveToSpeedRoller.java

@@ -0,0 +1,22 @@
+package training.commands.roller;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import training.subsystems.roller.Roller;
+
+public class MoveToSpeedRoller extends Command {
+
+    private double targetSpeed;
+
+    public void MoveToSpeedSpeed(double targetSpeed){
+        this.targetSpeed = targetSpeed;
+    }
+
+
+    public boolean isFinished(){
+        return Roller.getInstance().isAtVelocity(targetSpeed);
+    }
+
+    public void end(boolean interrupted){
+        Roller.getInstance().stop();
+    }
+}

src/main/java/training/commands/wrist/MoveToAngleWrist.java

@@ -0,0 +1,30 @@
+package training.commands.wrist;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import training.subsystems.wrist.Wrist;
+
+public class MoveToAngleWrist extends Command {
+
+    private Rotation2d targetAngle;
+
+    public MoveToAngleWrist(Rotation2d targetAngle){
+        this.targetAngle = targetAngle;
+    }
+
+    @Override
+    public void initialize() {
+        Wrist.goToAngel(targetAngle);
+    }
+
+    public boolean isFinished(){
+        return Wrist.getInstance().isAtAngle(targetAngle);
+    }
+
+    public void end(boolean interrupted){
+        Wrist.getInstance().stop();
+    }
+}
+
+
+

src/main/java/training/subsystems/elbow/Elbow.java

@@ -0,0 +1,53 @@
+package training.subsystems.elbow;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+import utils.GBSubsystem;
+
+public class Elbow extends GBSubsystem {
+
+	private static CANSparkMax motor;
+	private static Elbow instance;
+
+	private Elbow() {
+		motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		motor.getPIDController().setP(ElbowConstants.MOTOR_ID_P);
+		motor.getPIDController().setI(ElbowConstants.MOTOR_ID_I);
+		motor.getPIDController().setD(ElbowConstants.MOTOR_ID_D);
+	}
+
+	public static void init() {
+		if (instance == null) {
+			instance = new Elbow();
+		}
+	}
+
+	public static Elbow getInstance() {
+		init();
+		return instance;
+	}
+
+	public static void goToAngel(Rotation2d targetAngle) {
+		motor.getPIDController().setReference(targetAngle.getDegrees(), CANSparkBase.ControlType.kPosition);
+	}
+
+	public boolean isAtAngle(Rotation2d targetAngle) {
+		return Math.abs(targetAngle.getDegrees() - motor.getEncoder().getPosition()) == 0;
+	}
+
+	public void stop() {
+		motor.set(0);
+	}
+
+
+	@Override
+	protected String getLogPath() {
+		return "Elbow";
+	}
+
+	@Override
+	protected void subsystemPeriodic() {}
+
+}

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

@@ -0,0 +1,11 @@
+package training.subsystems.elbow;
+
+
+public class ElbowConstants {
+
+	public static final int MOTOR_ID = 5;
+	public static final int MOTOR_ID_P = 1;
+	public static final int MOTOR_ID_I = 0;
+	public static final int MOTOR_ID_D = 0;
+
+}

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

@@ -0,0 +1,62 @@
+package training.subsystems.roller;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+import training.subsystems.elbow.Elbow;
+import training.subsystems.elbow.ElbowConstants;
+import utils.GBSubsystem;
+
+public class Roller extends GBSubsystem {
+
+	private static CANSparkMax motor;
+	private static Roller instance;
+	private Roller() {
+		motor = new CANSparkMax(RollerConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		motor.getPIDController().setP(RollerConstants.MOTOR_ID_P);
+		motor.getPIDController().setI(RollerConstants.MOTOR_ID_I);
+		motor.getPIDController().setD(RollerConstants.MOTOR_ID_D);
+	}
+
+
+	public static void init() {
+		if (instance == null) {
+			instance = new Roller();
+		}
+	}
+	public boolean isAtVelocity(double targetVelocity) {
+		return Math.abs(targetVelocity - motor.getEncoder().getVelocity()) == 0.0;
+	}
+
+	public void goToSpeed(double targetSpeed) {
+		motor.getPIDController().setReference(targetSpeed, CANSparkBase.ControlType.kVelocity);
+	}
+
+	public static Roller getInstance() {
+		init();
+		return instance;
+	}
+	public void stop() {
+		motor.set(0);
+	}
+
+
+
+	@Override
+	protected String getLogPath() {
+		return null;
+	}
+
+	@Override
+	protected void subsystemPeriodic() {}
+
+}
+
+
+
+
+
+
+
+

src/main/java/training/subsystems/roller/RollerConstants.java

@@ -0,0 +1,10 @@
+package training.subsystems.roller;
+
+public class RollerConstants {
+    public static final int MOTOR_ID = 22;
+    public static final int MOTOR_ID_P = 1;
+    public static final int MOTOR_ID_I = 0;
+    public static final int MOTOR_ID_D = 0;
+
+
+}

src/main/java/training/subsystems/wrist/Wrist.java

@@ -0,0 +1,66 @@
+package training.subsystems.wrist;
+
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+import training.subsystems.elbow.ElbowConstants;
+import training.subsystems.roller.Roller;
+import training.subsystems.roller.RollerConstants;
+import utils.GBSubsystem;
+
+public class Wrist extends GBSubsystem {
+
+	private  static CANSparkMax motor;
+	private static Wrist instance;
+
+	private Wrist() {
+		motor = new CANSparkMax(WristConstant.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		motor.getPIDController().setP(WristConstant.MOTOR_ID_P);
+		motor.getPIDController().setI(WristConstant.MOTOR_ID_I);
+		motor.getPIDController().setD(WristConstant.MOTOR_ID_D);
+	}
+
+	public static void init() {
+		if (instance == null) {
+			instance = new Wrist();
+		}
+	}
+
+	public static void goToAngel(Rotation2d targetAngle) {
+		motor.getPIDController().setReference(targetAngle.getDegrees(), CANSparkBase.ControlType.kPosition);
+	}
+
+	public boolean isAtAngle(Rotation2d targetAngle) {
+		return Math.abs(targetAngle.getDegrees() - motor.getEncoder().getPosition()) == 0;
+	}
+
+	public static Wrist getInstance() {
+		init();
+		return instance;
+	}
+	public void stop() {
+		motor.set(0);
+	}
+
+
+
+	@Override
+	protected String getLogPath() {
+		return "Wrist";
+	}
+
+	@Override
+	protected void subsystemPeriodic() {}
+
+}
+
+
+
+
+
+
+
+
+

src/main/java/training/subsystems/wrist/WristConstant.java

@@ -0,0 +1,8 @@
+package training.subsystems.wrist;
+
+public class WristConstant {
+    public static final int MOTOR_ID = 11;
+    public static final int MOTOR_ID_P = 1;
+    public static final int MOTOR_ID_I = 0;
+    public static final int MOTOR_ID_D = 0;
+}