Emily bottom arm

src/main/java/frc/robot/Config.java

@@ -35,6 +35,20 @@ public final class Config {
    */
   private static int robotId = -1;
 
+  /**
+   * CAN IDs, ports, channels, etc.
+   */
+  public static class CANID {
+    // Arm Subsystem
+    public static final int TOP_ARM_SPARK_CAN_ID = robotSpecific(5,0,0,0,0,18,18);
+    public static final int BOTTOM_ARM_SPARK_CAN_ID = robotSpecific(4,0,0,0,0,19,19);
+
+    //Arm Subsystem
+    public static final int BOTTOM_CANCODER_CAN_ID = 2;
+
+
+  }
+
   /**
    * Returns one of the values passed based on the robot ID
    *
@@ -94,12 +108,14 @@ public static int getRobotId() {
 
 
    /** ADD CONSTANTS BELOW THIS LINE */
-  // PCM Can ID
-  public static final int CTRE_PCM_CAN_ID = 1;
 
-  // Constants for arm pneumatics
-  public static final int ARMLOW_PNEUMATIC_FORWARD_CHANNEL = 0;
-  public static final int ARMLOW_PNEUMATIC_REVERSE_CHANNEL = 1;
+   //PCM Can ID 
+   public static final int CTRE_PCM_CAN_ID = 1;
+
+   //Constants for arm pneumatics 
+   public static final int ARMLOW_PNEUMATIC_FORWARD_CHANNEL=0;
+   public static final int ARMLOW_PNEUMATIC_REVERSE_CHANNEL = 1;
+
   public static final int ARMHIGH_PNEUMATIC_FORWARD_CHANNEL = 2;
   public static final int ARMHIGH_PNEUMATIC_REVERSE_CHANNEL = 3;
 

src/main/java/frc/robot/ErrorCheck.java

@@ -0,0 +1,25 @@
+package frc.robot;
+
+import com.ctre.phoenix.ErrorCode;
+import com.revrobotics.REVLibError;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+public class ErrorCheck {
+    public static int count = -1;
+    public static boolean errREV(REVLibError error) {
+        if (error == REVLibError.kOk) {
+            return true;
+        }
+    DriverStation.reportError("REV DEVICE Error" + error.toString(), true);
+    return false;
+}
+public static boolean errCTRE(ErrorCode error) {
+    if (error == ErrorCode.OK) {
+        return true;
+    }
+
+    DriverStation.reportError("CTRE Device Error: " + error.toString(), true);
+    return false;
+}
+}

src/main/java/frc/robot/Robot.java

@@ -8,6 +8,10 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.SubsystemChecker.SubsystemType;
+import frc.robot.commands.CheckArmEncodersSync;
+import frc.robot.commands.SyncArmEncoders;
 import frc.robot.robotcontainers.BeetleContainer;
 import frc.robot.robotcontainers.ClutchContainer;
 import frc.robot.robotcontainers.CosmobotContainer;
@@ -33,6 +37,13 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     createRobotContainer();
+
+    if (SubsystemChecker.canSubsystemConstruct(SubsystemType.ArmSubsystem)) {
+      System.out.println("*****robotInit*****");
+      new WaitCommand(5).andThen(new SyncArmEncoders()).schedule();
+      //new WaitCommand(10).andThen(new CheckArmEncodersSync()).schedule();
+    }
+
   }
 
   private void createRobotContainer() {

src/main/java/frc/lib/lib2706/SubsystemChecker.java → src/main/java/frc/robot/SubsystemChecker.java

@@ -2,9 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc.lib.lib2706;
-
-import frc.robot.Config;
+package frc.robot;
 
 /** Add your docs here. */
 public class SubsystemChecker {

src/main/java/frc/robot/commands/CheckArmEncodersSync.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ArmSubsystem;
+
+public class CheckArmEncodersSync extends CommandBase {
+  /** Creates a new CheckArmEncodersSync. */
+  public CheckArmEncodersSync() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(ArmSubsystem.getInstance());
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (ArmSubsystem.getInstance().areEncodersSynced()==false) {
+      DriverStation.reportWarning ("CheckArmEncoders: Arm encoders are not synced", false);
+      System.out.println("*****CheckArmEncoders: Arm encoders are not synced*****");
+    }
+    else {
+      DriverStation.reportWarning ("CheckArmEncoders: Arm encoders are synced", false);
+      System.out.println("*****CheckArmEncoders: Arm encoders are synced*****");
+    }
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+
+  @Override 
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

src/main/java/frc/robot/commands/SetBottomArm.java

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ArmPneumaticsSubsystem;
+import frc.robot.subsystems.ArmSubsystem;
+
+public class SetBottomArm extends CommandBase {
+  double bottomArmAngleRadians;
+  final double TIMEOUT_S=2;
+
+  Timer m_timer = new Timer();
+  /** Creates a new SetBottomArm. */
+  public SetBottomArm(double angleDegree) {
+    bottomArmAngleRadians = Math.toRadians(angleDegree);
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(ArmSubsystem.getInstance());
+    addRequirements(ArmPneumaticsSubsystem.getInstance());
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    //ArmSubsystem.getInstance().controlBottomArmBrake(false);
+    ArmPneumaticsSubsystem.getInstance().controlBottomBrake(false, false);
+    m_timer.stop();
+    m_timer.reset();
+    //todo: 
+    m_timer.start();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    ArmSubsystem.getInstance().setBottomJointAngle(bottomArmAngleRadians);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    ArmSubsystem.getInstance().stopMotors();
+    if (interrupted == false)
+     {
+      //ArmSubsystem.getInstance().controlBottomArmBrake(true);
+      ArmPneumaticsSubsystem.getInstance().controlBottomBrake(true, true);
+
+    }
+    m_timer.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    //todo: check if bottom arm reaches the position 
+    //time out, finished
+    return m_timer.hasElapsed(TIMEOUT_S);
+  }
+}

src/main/java/frc/robot/commands/SyncArmEncoders.java

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ArmSubsystem;
+
+public class SyncArmEncoders extends CommandBase {
+  int counter;
+  boolean bSynced;
+  /** Creates a new SyncArmEncoders. */
+  public SyncArmEncoders() {
+    counter = 0;
+    bSynced = false;
+    addRequirements(ArmSubsystem.getInstance());
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    counter ++;
+    if (ArmSubsystem.getInstance().areEncodersSynced()==false) {
+      DriverStation.reportWarning ("Arm encoders are not synced", false);
+      System.out.println("*****Arm encoders are not synced*****");
+    }
+    else {
+      DriverStation.reportWarning ("Arm encoders are synced", false);
+      System.out.println("*****Arm encoders are synced*****");
+      bSynced = true;
+      ArmSubsystem.getInstance().burnFlash();
+    }
+
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  @Override 
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (counter >= 10 || bSynced == true) {
+    return true;
+    }
+    else {
+
+      return false;
+    }
+  }
+}

src/main/java/frc/robot/robotcontainers/PoseidonContainer.java

@@ -8,6 +8,8 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Robot;
+import frc.robot.commands.SetBottomArm;
+import frc.robot.commands.SyncArmEncoders;
 import frc.robot.commands.ArmPneumaticsCommands.*;
 import frc.robot.subsystems.GripperSubsystem;
 
@@ -41,6 +43,8 @@ public PoseidonContainer() {
    * created via the {@link CommandXboxController} or other ways.
    */
   private void configureButtonBindings() {
+    CommandXboxController driver = new CommandXboxController(0);
+    CommandXboxController operator = new CommandXboxController (1);
     /* Driver Controls */
 
     //ArmPneumaticsSubsystem commands
@@ -63,7 +67,13 @@ private void configureButtonBindings() {
     driver.a().onTrue(GripperSubsystem.getInstance().lowPressureCommand());
     // B - High Pressure
     driver.b().onTrue(GripperSubsystem.getInstance().highPressureCommand());
+
+
     /* Operator Controls */
+    operator.a().onTrue(new SetBottomArm(90));
+    operator.b().onTrue(new SetBottomArm(80));
+    operator.y().onTrue(new SyncArmEncoders());
+
   }
 
   /**

src/main/java/frc/robot/subsystems/ArmConfig.java

@@ -0,0 +1,54 @@
+package frc.robot.subsystems;
+
+import frc.robot.Config;
+
+public class ArmConfig {
+    public static final boolean TOP_SET_INVERTED = true;
+    public static final boolean BOTTOM_SET_INVERTED = true;
+
+    public static final int CURRENT_LIMIT = 60;
+
+    //soft limit constants for top arm
+    public static final float top_arm_forward_limit = (float)Math.toRadians(190); //converts degree to radian
+    public static final float top_arm_reverse_limit = (float)Math.toRadians(7); 
+    public static final boolean TOP_SOFT_LIMIT_ENABLE = true;
+
+    //soft limit constant for bottom arm
+    public static final float bottom_arm_forward_limit = (float)Math.toRadians(135);
+    public static final float bottom_arm_reverse_limit = (float)Math.toRadians(40);
+    public static final boolean BOTTOM_SOFT_LIMIT_ENABLE = true;
+
+    //PID constants
+    public static final double bottom_arm_kP = 1.4;
+    public static final double bottom_arm_kI = 0.0003;
+    public static final double bottom_arm_kD = 0.9;
+    public static final double bottom_arm_kIz = 0.3;
+    public static final double bottom_arm_kFF = 0;
+    public static final double min_output = -1;
+    public static final double max_output = 1;
+
+
+
+
+    //duty cycle encoders
+    public static final int bottom_duty_cycle_channel = 7;
+
+    //arm offsets
+    public static final double bottom_arm_offset = 307.800000;
+
+    //syncing encoders
+    public static double ENCODER_SYNCING_TOLERANCE = 0.01; //radians
+
+    public static final double BOTTOM_NEO_GEAR_RATIO =  Config.robotSpecific(62.5,0.0,0.0,0.0);
+
+    public static final double bottomArmPositionConversionFactor = 2 * Math.PI / BOTTOM_NEO_GEAR_RATIO;
+    public static final double bottomArmVelocityConversionFactor = bottomArmPositionConversionFactor / 60.0;
+
+    public static final double BOTTOM_MAX_VEL = Math.PI * 3;
+    public static final double BOTTOM_MAX_ACCEL = Math.PI *3;
+
+
+
+
+
+}