3/16 testing with new arm

FRC5190 · Mar 17, 2024 · 9939bfa · 9939bfa
1 parent 6d9ae9b
commit 9939bfa
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 21 deletions.
diff --git a/src/main/java/org/ghrobotics/frc2024/Robot.java b/src/main/java/org/ghrobotics/frc2024/Robot.java
@@ -66,7 +66,9 @@ public void robotInit() {
     SmartDashboard.putData("field", field_);
     setupTeleopControls();
     // drive_.setBrakeMode(true);
-    robot_state_.reset(auto_selector_.getStartingPose());
+    // robot_state_.reset(auto_selector_.getStartingPose());
+
+    robot_state_.reset(new Pose2d(1.363, 5.517, Rotation2d.fromDegrees(0)));
   }
 
   @Override
@@ -86,7 +88,7 @@ public void robotPeriodic() {
     SmartDashboard.putNumber("Vision y", limelight_.getBotPose2d().getY());
     SmartDashboard.putNumber("Vision Degrees", limelight_.getBotPose2d().getRotation().getDegrees());
 
-    field_.setRobotPose(limelight_.getBotPose2d());
+    field_.setRobotPose(robot_state_.getPosition());
   }
 
 
@@ -111,7 +113,7 @@ public void autonomousPeriodic() {}
   @Override
   public void teleopInit() {
     isAuto = false;
-    robot_state_.reset(limelight_.getBotPose2d());
+    // robot_state_.reset(limelight_.getBotPose2d());
     drive_.setBrakeMode(true);
     arm_.setBrakeMode(true);
   }
@@ -159,23 +161,27 @@ public void simulationPeriodic() {}
   private void setupTeleopControls() {
 
     // Driver Control
-    driver_controller_.rightTrigger().whileTrue(superstructure_.setShooter(-0.75));
+    driver_controller_.rightTrigger().whileTrue(superstructure_.setShooter(0.75));
+
+    driver_controller_.rightBumper().whileTrue(superstructure_.setShooter(0.65));
+
+    driver_controller_.pov(180).whileTrue(superstructure_.setShooter(0.55));
 
-    driver_controller_.rightBumper().whileTrue(superstructure_.setShooter(-0.5));
+    driver_controller_.pov(270).whileTrue(superstructure_.setShooter(85));
 
-    driver_controller_.leftTrigger().whileTrue(superstructure_.setIntake(-0.25));
+    driver_controller_.leftTrigger().whileTrue(superstructure_.setIntake(0.25));
 
-    driver_controller_.leftBumper().whileTrue(superstructure_.setIntake(0.15));
+    driver_controller_.leftBumper().whileTrue(superstructure_.setIntake(-0.15));
 
     driver_controller_.pov(0).whileTrue(superstructure_.setArmPercent(0.056));
 
     // driver_controller_.pov(180).whileTrue(superstructure_.setShooter(0.3));
 
     driver_controller_.b().whileTrue(superstructure_.shoot());
 
-    driver_controller_.a().whileTrue(superstructure_.setFeeder(0.5));
+    driver_controller_.a().whileTrue(superstructure_.setFeeder(0.85));
 
-    driver_controller_.pov(90).onTrue(new RotateToSpeaker(drive_, robot_state_, limelight_));
+    driver_controller_.pov(90).whileTrue(superstructure_.setShooter(90));
 
     // Useful for shooting while moving
     driver_controller_.pov(270).whileTrue(
@@ -189,13 +195,13 @@ private void setupTeleopControls() {
 
     operator_controller_.leftTrigger().whileTrue(superstructure_.setArmPercent(brake_value_));
 
-    operator_controller_.b().onTrue(new ArmPID(arm_, 20));
+    operator_controller_.b().onTrue(new ArmPID(arm_, 16.5));
 
     operator_controller_.a().onTrue(new ArmPID(arm_, 2));
 
-    operator_controller_.y().onTrue(new ArmPID(arm_, 36));
+    operator_controller_.y().onTrue(new ArmPID(arm_, 25));
 
-    operator_controller_.x().onTrue(new ArmPID(arm_, 60));
+    operator_controller_.x().onTrue(new ArmPID(arm_, 55));
 
     operator_controller_.pov(0).whileTrue(superstructure_.setArmPercent(0.1));
 

diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Arm.java
@@ -207,7 +207,7 @@ public void setBrake(double brake_angle) {
   public void setAnglePID(double angle) {
     pid_.setSetpoint(angle);
     // SmartDashboard.putNumber("Angle Calculate", pid_.calculate(Math.toDegrees(getAngle())));
-    double output = MathUtil.clamp(pid_.calculate(Math.toDegrees(getAngle())), -0.2, 0.55);
+    double output = MathUtil.clamp(pid_.calculate(Math.toDegrees(getAngle())), -0.15, 0.35);
     SmartDashboard.putNumber("Arm output", output);
 
     if (Math.abs(output) < 0.01){

diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Drive.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Drive.java
@@ -60,10 +60,10 @@ public void periodic() {
     for (int i = 0; i < modules_.length; i++) {
       modules_[i].setDesiredState(module_states[i], output_type_);
 
-      SmartDashboard.putNumber(String.format("Module [%d] Speed", i),
-        modules_[i].getDriveVelocity());
-      SmartDashboard.putNumber(String.format("Module [%d] Angle", i),
-        modules_[i].getSteerPosition().getDegrees());
+      // SmartDashboard.putNumber(String.format("Module [%d] Speed", i),
+      //   modules_[i].getDriveVelocity());
+      // SmartDashboard.putNumber(String.format("Module [%d] Angle", i),
+      //   modules_[i].getSteerPosition().getDegrees());
     }
   }
 

diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java b/src/main/java/org/ghrobotics/frc2024/subsystems/Intake.java
@@ -54,8 +54,8 @@ public void periodic() {
   // }
 
   public void setPercent(double value) {
-    io_.left_demand = value;
-    io_.right_demand = value - 0.1;
+    io_.left_demand = value + 0.2;
+    io_.right_demand = value;
   }
   public void stopMotor() {
     io_.left_demand = 0;

diff --git a/src/main/java/org/ghrobotics/frc2024/subsystems/SwerveModule.java b/src/main/java/org/ghrobotics/frc2024/subsystems/SwerveModule.java
@@ -140,8 +140,8 @@ public void setDesiredState(SwerveModuleState state, Drive.OutputType output_typ
     // Set drive output
     switch (output_type) {
       case OPEN_LOOP:
-        SmartDashboard.putNumber("11Drive Motor Set Speed", state.speedMetersPerSecond / Constants.kMaxModuleSpeed);
-        SmartDashboard.putNumber("11max speed", state.speedMetersPerSecond);
+        // SmartDashboard.putNumber("11Drive Motor Set Speed", state.speedMetersPerSecond / Constants.kMaxModuleSpeed);
+        // SmartDashboard.putNumber("11max speed", state.speedMetersPerSecond);
         if(SmartDashboard.getBoolean("Auto", false)) {
           drive_motor_.set(state.speedMetersPerSecond / Constants.kMaxAutoSpeed);
         } else {