elbow

svhsrobotics · Feb 27, 2024 · 7439a59 · 7439a59
1 parent 0f8a56d
commit 7439a59
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 19 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java
@@ -88,7 +88,7 @@ public static double rpmToVelocity(double rpm) {
 
     public final DcMotorEx shoulderMotor;
     public final DcMotorEx hangMotor;
-    public final AxonServo elbowServo;
+    public final Servo elbowServo;
     //public final AxonServo wristTwistServo;
     public final Servo wristTwistServo;
     public final Servo wristLiftServo;
@@ -127,7 +127,8 @@ public RoboticaBot(HardwareMap hardwareMap) {
         shoulderMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         shoulderMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         hangMotor = hardwareMap.get(DcMotorEx.class, "hang");
-        elbowServo = new AxonServo("elbow", "axon_2", hardwareMap);
+        //elbowServo = new AxonServo("elbow", "axon_2", hardwareMap);
+        elbowServo = hardwareMap.get(Servo.class, "elbow");
         //wristTwistServo = new AxonServo("wrist_twist", "axon_3", hardwareMap);
         wristTwistServo = hardwareMap.get(Servo.class, "wrist_twist");
         wristLiftServo = hardwareMap.get(Servo.class, "wrist_lift");

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java
@@ -1,30 +1,29 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Servo;
 
 @TeleOp
-@Disabled
 public class AnalogTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
-        AxonServo servoTest = new AxonServo("servo", "a0", hardwareMap);
+        //AxonServo servoTest = new AxonServo("servo", "a0", hardwareMap);
+        Servo elbowServo = hardwareMap.get(Servo.class, "elbow");
+
         waitForStart();
+        double elbowPos = 0;
         while (opModeIsActive()) {
 //            if (gamepad1.a) {
 //                servoTest.innerServo.setPower(0.6);
 //            } else {
 //                servoTest.innerServo.setPower(0);
 //            }
-            servoTest.innerServo.setPower(gamepad1.left_stick_x);
-            //servoTest.innerAnalog.getVoltage();
-            //servoTest.getCurrentPosition();
-            //telemetry.addData("Degrees", servoTest.getCurrentPosition());
-            telemetry.addData("Power", gamepad1.left_stick_x);
-            //telemetry.addData("Last Pos", servoTest.getLastPosDebug());
-            telemetry.addData("Adjusted Degrees", servoTest.getAdjustedPosition());
-            telemetry.addData("Voltage", servoTest.innerAnalog.getVoltage());
+
+            elbowPos += gamepad1.left_stick_x * 0.001;
+            elbowServo.setPosition(elbowPos); // 0.2 down 0.55 up
+
+            telemetry.addData("Elbow Pos", elbowPos);
 
             telemetry.update();
         }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java
@@ -40,6 +40,7 @@ public void runOpMode() throws InterruptedException {
         Toggle pinchToggle = new Toggle();
         ArmState currentState = ArmState.MANUAL;
         int offset = 0;
+        double elbowPos=0.2;
         double pinchLocation = 0.0;
 
         while (opModeIsActive()) {
@@ -131,12 +132,13 @@ public void runOpMode() throws InterruptedException {
 
                 // ELBOW
                 if (gamepad1.dpad_left || gamepad2.dpad_left) {
-                    rrobot.elbowServo.innerServo.setPower(1.0);
+                    elbowPos -= 0.008; // UP
                 } else if (gamepad1.dpad_right || gamepad2.dpad_right) {
-                    rrobot.elbowServo.innerServo.setPower(-1.0);
-                } else {
-                    rrobot.elbowServo.innerServo.setPower(0.0);
+                    elbowPos += 0.008; // DOWN
                 }
+                if (elbowPos > 0.55) wristPos = 0.55;
+                if (wristPos < 0.2) wristPos = 0.2;
+                rrobot.elbowServo.setPosition(wristPos);
 
                 if (gamepad1.dpad_up || gamepad2.dpad_up) {
                     pinchLocation = pinchLocation + 0.008;

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java
@@ -51,9 +51,9 @@ public void drive() {
         trajC.back(5);
         getRobot().getDrive().followTrajectorySequence(trajC.build());
 
-        ((RoboticaBot) getRobot()).elbowServo.innerServo.setPower(-1.0);
+        //((RoboticaBot) getRobot()).elbowServo.innerServo.setPower(-1.0);
         GlobalOpMode.opMode.sleep(400);
-        ((RoboticaBot) getRobot()).elbowServo.innerServo.setPower(0.0);
+        //((RoboticaBot) getRobot()).elbowServo.innerServo.setPower(0.0);
         ((RoboticaBot) getRobot()).wristTwistServo.setPosition(0.5);
         ((RoboticaBot) getRobot()).wristLiftServo.setPosition(TestTeleOp.RAISED_WRIST);