Simulation for testing robot

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2022.3.1"
+    id "edu.wpi.first.GradleRIO" version "2022.4.1"
 
 }
 

src/main/deploy/pathplanner/New Path.path

@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":3.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.990738243528404,"y":3.499367196509424},"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":4.343691258457919},"prevControl":{"x":3.0,"y":3.306087794610263},"nextControl":null,"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false}]}

src/main/deploy/pathplanner/Path 1.1.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":7.53788108317873,"y":1.851409388850916},"prevControl":null,"nextControl":{"x":7.558226241297971,"y":2.380383499951179},"holonomicAngle":-90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.659952031894175,"y":2.8991850319918195},"prevControl":{"x":7.558226241297971,"y":2.360038341831937},"nextControl":null,"holonomicAngle":-110.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
+{"waypoints":[{"anchorPoint":{"x":7.53788108317873,"y":1.851409388850916},"prevControl":null,"nextControl":{"x":7.558226241297971,"y":2.380383499951179},"holonomicAngle":-90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.659952031894175,"y":2.8991850319918195},"prevControl":{"x":7.558226241297971,"y":2.360038341831937},"nextControl":null,"holonomicAngle":-110.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":null,"maxAcceleration":null,"isReversed":true}

src/main/deploy/pathplanner/Path 2.3.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":7.73116008531152,"y":2.90935761105144},"prevControl":null,"nextControl":{"x":6.520623177216689,"y":2.5329721858454843},"holonomicAngle":-110.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.767852326804778,"y":2.319348025593455},"prevControl":{"x":6.907181181482265,"y":2.624525397382069},"nextControl":null,"holonomicAngle":-142.65065095535948,"isReversal":false,"velOverride":null,"isLocked":false}]}
+{"waypoints":[{"anchorPoint":{"x":7.73116008531152,"y":2.90935761105144},"prevControl":null,"nextControl":{"x":6.520623177216689,"y":2.5329721858454843},"holonomicAngle":-110.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.767852326804778,"y":2.319348025593455},"prevControl":{"x":6.907181181482265,"y":2.624525397382069},"nextControl":null,"holonomicAngle":-142.65065095535948,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":null,"maxAcceleration":null,"isReversed":false}

src/main/java/frc/robot/Constants.java

@@ -25,7 +25,6 @@ public final class Constants {
     public final static int SPARK_BL = 4;
     public final static int SPARK_BR = 3;
 
-
     //wheel diameter
     public static final double kWheelDiameterMeters = 0.1524;
 
@@ -35,10 +34,6 @@ public final class Constants {
     //Distance between centers of front and back wheels on robot
     public static final double kWheelBase = 0.676275; 
 
-    /**
-     * 27 1/4 inches = 0.69125
-     * 33 = 0.8382
-     */
 
     public static final double kEncoderCPR = 1; 
     public static final double kGearReduction = 16;
@@ -48,13 +43,13 @@ public final class Constants {
         ((kWheelDiameterMeters * Math.PI)) / (kGearReduction);
 
     //Feedforward gains for system dynamics 
-    public static final double kS = 0.11104; 
-    public static final double kV = 4.1572;  
-    public static final double kA = 0.21354;
+    public static final double kS = 0.12187; 
+    public static final double kV = 4.1231;  
+    public static final double kA = 0.90139;
 
     //Angular gains
-    public static final double kV_Angular = 1; //
-    public static final double kA_Angular = 1; //
+    public static final double kV_Angular = 1; // do not touch
+    public static final double kA_Angular = 1; // do not touch
 
     //position controllers
 
@@ -65,10 +60,10 @@ public final class Constants {
 
 
     //Velocity controllers
-    public static final double kPFrontLeftVel = 3.8167; 
-    public static final double kPRearLeftVel = 3.8167;
-    public static final double kPFrontRightVel = 3.8167;
-    public static final double kPRearRightVel = 3.8167;
+    public static final double kPFrontLeftVel = 5.7094; 
+    public static final double kPRearLeftVel = 5.7094;
+    public static final double kPFrontRightVel = 5.7094;
+    public static final double kPRearRightVel = 5.7094;
 
     //Converting chassis velocity into individual wheel velocities
     public static final MecanumDriveKinematics kDriveKinematics =
@@ -95,6 +90,8 @@ public final class Constants {
 
     //climb
     public static int Talon_arm = 5;
+    public static double intakeArmSpeed = .5;//.38;
+
     public static int talon_pivot = 6;
     public static int PCM_2 = 7;
     public static int[] firstSolenoid = {1, 0}; // rev, fwd
@@ -105,10 +102,17 @@ public final class Constants {
     public static double setMotorSpeed = 0.69;
     public static int swings = 0;
 
+    public static int[] climb_servo = {0,1};
     public static int left_servo = 0;
     public static int right_servo = 1;
-    public static int left_intake_servo = 2;
-    public static int right_intake_servo = 3;
+
+    public static int[] bottom_servos = {2,3};
+
+    public static int[] ULTRASONIC_INTAKE =  {1,2};
+    public static int[] ULTRASONIC_OUTAKE =  {3,4};
+
+    public static final double ULTRASONIC_INTAKE_THRESH = 2.0;
+    public static final double ULTRASONIC_OUTTAKE_THRESH = 2.0;
 
     public static double height = 15.375;
     public static double length = 24.0;

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

@@ -6,9 +6,30 @@
 
 
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsControlModule;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.commands.Climb.MoveLeftStatic;
+import frc.robot.commands.Climb.MoveRightSolenoid;
+import frc.robot.commands.Climb.MoveRightStatic;
+import frc.robot.commands.Climb.MoveLeftSolenoid;
+import frc.robot.commands.Climb.MoveTalon;
+import frc.robot.commands.Intake.MoveIntakeTalon;
+import frc.robot.commands.Intake.MoveLeftServo;
+import frc.robot.commands.Intake.MoveRightServo;
+import frc.robot.controls.ControlMap;
+import frc.robot.subsystems.Climb.SimLeftServoClimb;
+import frc.robot.subsystems.Climb.SimLeftPiston;
+import frc.robot.subsystems.Climb.SimPivotMotor;
+import frc.robot.subsystems.Climb.SimRightPiston;
+import frc.robot.subsystems.Climb.SimRightServoClimb;
+import frc.robot.subsystems.Intake.SimIntakeArmMotor;
+import frc.robot.subsystems.Intake.SimLeftServoClaw;
+import frc.robot.subsystems.Intake.SimRightServoClaw;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -21,21 +42,46 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
+   private SimLeftPiston m_lPiston;
+  private SimLeftServoClaw m_lServo;
+  private SimPivotMotor m_climbMotor;
+  private SimLeftServoClimb m_staticLeftServo;
+  private SimRightPiston m_rPiston;
+  private SimRightServoClimb m_staticRightServo;
+  private SimRightServoClaw m_rServo;
+  private SimIntakeArmMotor m_intakeTalon;
+
+  //UsbCamera front_cam;
+  //UsbCamera rear_cam;
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
    */
 
-  //  PneumaticsControlModule pcm = new PneumaticsControlModule();
+   //PneumaticsControlModule pcm = new PneumaticsControlModule();
    //Compressor compressor = new Compressor(PneumaticsModuleType.CTREPCM);
   @Override
   public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     // pcm.clearAllStickyFaults();
     // System.out.println("your mom is a: " + pcm.checkSolenoidChannel(0));
-    CameraServer.startAutomaticCapture();
+    //CameraServer.startAutomaticCapture();
+    System.out.println("init finished.");
+    // rear_cam = CameraServer.startAutomaticCapture(1);
+
+    //front_cam.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
+    // rear_cam.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
+
     m_robotContainer = new RobotContainer();
+    m_lPiston = new SimLeftPiston();
+    m_rPiston = new SimRightPiston();
+    m_lServo = new SimLeftServoClaw();
+    m_climbMotor = new SimPivotMotor();
+    m_staticLeftServo = new SimLeftServoClimb();
+    m_staticRightServo = new SimRightServoClimb();
+    m_rServo = new SimRightServoClaw();
+    m_intakeTalon = new SimIntakeArmMotor();
   }
 
   /**
@@ -85,6 +131,16 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+
+    //Running commands through keyboard
+    ControlMap.m_z.whenPressed(new MoveLeftSolenoid(m_lPiston));
+    ControlMap.m_j.whenPressed(new MoveRightSolenoid(m_rPiston));
+    ControlMap.m_n.whenPressed(new MoveLeftServo(m_lServo));
+    ControlMap.m_m.whenPressed(new MoveTalon(m_climbMotor));
+    ControlMap.m_l.whenPressed(new MoveLeftStatic(m_staticLeftServo));
+    ControlMap.m_q.whenPressed(new MoveRightStatic(m_staticRightServo));
+    ControlMap.m_y.whenPressed(new MoveRightServo(m_rServo));
+    ControlMap.m_u.whenPressed(new MoveIntakeTalon(m_intakeTalon));
   }
 
   /** This function is called periodically during operator control. */