Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/Robot…

…Code2024 into sofie-arm
DeepBlueRobotics · Mar 3, 2024 · 6dd5c89 · 6dd5c89
2 parents 83100c2 + 2072eeb
commit 6dd5c89
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 10 deletions.
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.1"
 }
 
 java {

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -76,14 +76,22 @@ public Arm() {
         armEncoder.setInverted(ENCODER_INVERTED);
 
         armMotorFollower.follow(armMotorMaster);
+        armPID1.setP(kP);
+        armPID1.setI(kI);
+        armPID1.setD(kD);
+        armPID2.setP(kP);
+        armPID2.setI(kI);
+        armPID2.setD(kD);
 
         //armEncoder1.setZeroOffset(offsetRad);
 
         //armPID.setTolerance(posToleranceRad, velToleranceRadPSec);
 
         SmartDashboard.putData("Arm", this);
 
-        //armProfileTimer.start(); <-- don't neeed timer anymore
+
+
+        goalState = getCurrentArmState();
 
         setArmTarget(goalState.position);
     }
@@ -95,12 +103,7 @@ public void periodic() {
 
         //ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = SmartDashboard.getNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
         // armConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL , MAX_FF_ACCEL );
-        armPID1.setP(kP);
-        armPID1.setI(kI);
-        armPID1.setD(kD);
-        armPID2.setP(kP);
-        armPID2.setI(kI);
-        armPID2.setD(kD);
+
 
         //smart dahsboard stuff
         //SmartDashboard.putBoolean("ArmPIDAtSetpoint", armPID1.atSetpoint());
@@ -116,7 +119,7 @@ public void periodic() {
 
        // SmartDashboard.putNumber("ArmPos", getArmPos());
 
-        //driveArm(armProfile.calculate(armProfileTimer.get()));
+
 
         autoCancelArmCommand();
     }
@@ -152,7 +155,7 @@ public void setArmTarget(double targetPos) {
         targetPos = getArmClampedGoal(targetPos);
 
         armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
-        armProfileTimer.reset();
+
 
         goalState.position = targetPos;
         goalState.velocity = 0;
@@ -169,6 +172,7 @@ public void resetGoal() {
     }
     public void driveMotor(Measure<Voltage> volts) {
        armMotorMaster.setVoltage(volts.in(Volts));
+       armMotorFollower.setVoltage(volts.in(Volts));
     }
     public void logMotor(SysIdRoutineLog log) {
         log.motor("armMotorMaster")