Skip to content

Commit

Permalink
R: changes from tonight, new arm design progress
Browse files Browse the repository at this point in the history
  • Loading branch information
@JJTech0130
JJTech0130 committed Feb 15, 2024
1 parent c4daf65 commit 8ba6bdb
Show file tree
Hide file tree
Showing 5 changed files with 211 additions and 117 deletions.
165 changes: 106 additions & 59 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.control.PIDCoefficients;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
Expand Down Expand Up @@ -76,13 +76,23 @@ public static double rpmToVelocity(double rpm) {

private final AprilTagCamera[] cameras;

private final Servo purpleServo;
public final DcMotorEx armMotor;
public final Servo wristServo;
public final AxonServo intakeServo;
public final Servo droneServo;
// private final Servo purpleServo;
// public final DcMotorEx armMotor;
// public final Servo wristServo;
// public final AxonServo intakeServo;
// public final Servo droneServo;
// public final DcMotorEx hangMotor;
// public final DcMotor intakeMotor;

public final DcMotorEx shoulderMotor;
public final DcMotorEx hangMotor;
public final DcMotor intakeMotor;
public final AxonServo elbowServo;
public final AxonServo wristTwistServo;
public final Servo wristLiftServo;
public final Servo pinchServo;
public final Servo purpleServo;
public final Servo planeAngleServo;
public final CRServo planeReleaseServo;

public RoboticaBot(HardwareMap hardwareMap) {
super(hardwareMap);
Expand All @@ -92,15 +102,43 @@ public RoboticaBot(HardwareMap hardwareMap) {
cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 5.5, 5.5);


purpleServo = hardwareMap.get(Servo.class, "purple");
armMotor = hardwareMap.get(DcMotorEx.class, "arm");
armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
wristServo = hardwareMap.get(Servo.class, "cup_pivot");
intakeServo = new AxonServo("intake_servo", "intake_analog_2", hardwareMap);
droneServo = hardwareMap.get(Servo.class, "drone");
// plane_release crservo
// plane_angle servo

// shoulder motor
// hang motor
// elbow axon servo
// wrist_twist axon servo
// wrist_lift servo
// pinch axon servo
// purple servo

// axon_2 analog
// axon_3 analog

planeAngleServo = hardwareMap.get(Servo.class, "plane_angle");
planeReleaseServo = hardwareMap.get(CRServo.class, "plane_release");

shoulderMotor = hardwareMap.get(DcMotorEx.class, "shoulder");
hangMotor = hardwareMap.get(DcMotorEx.class, "hang");
intakeMotor = hardwareMap.get(DcMotor.class, "intake");
elbowServo = new AxonServo("elbow", "axon_2", hardwareMap);
wristTwistServo = new AxonServo("wrist_twist", "axon_3", hardwareMap);
wristLiftServo = hardwareMap.get(Servo.class, "wrist_lift");
//pinchServo = hardwareMap.get(AxonServo.class, "pinch");
//pinchServo = new AxonServo("pinch", null, hardwareMap); // TODO SWITCH TO NORMAL SERVO
pinchServo = hardwareMap.get(Servo.class, "pinch");
//pinchServo.setPower(0.1);
purpleServo = hardwareMap.get(Servo.class, "purple");

// purpleServo = hardwareMap.get(Servo.class, "purple");
// armMotor = hardwareMap.get(DcMotorEx.class, "arm");
// armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// wristServo = hardwareMap.get(Servo.class, "cup_pivot");
// intakeServo = new AxonServo("intake_servo", "intake_analog_2", hardwareMap);
// droneServo = hardwareMap.get(Servo.class, "drone");
// hangMotor = hardwareMap.get(DcMotorEx.class, "hang");
// intakeMotor = hardwareMap.get(DcMotor.class, "intake");

// TODO: Reverse Motors, encoders & such

Expand Down Expand Up @@ -171,9 +209,18 @@ public void dropPurplePixel(boolean state) {
}
}


public static double PLANE_START_ANGLE = 0.6;
public static double PLANE_LAUNCH_ANGLE = 0.80;
public static double PLANE_FOR_INCR = 0.001;
public static int PLANE_DELAY = 10;
@Override
public void launchPlane() {
// TODO
for (double i=PLANE_START_ANGLE; i<PLANE_LAUNCH_ANGLE; i+=PLANE_FOR_INCR) {
planeAngleServo.setPosition(i);
GlobalOpMode.opMode.sleep(PLANE_DELAY);
}
planeReleaseServo.setPower(0.1);
}

@Override
Expand All @@ -189,47 +236,47 @@ public TrajectoryDrive getDrive() {
// }
// }

public void initializeIntakeSystem() {
wristServo.setPosition(0.5);
GlobalOpMode.opMode.sleep(1000);
armMotor.setTargetPosition(100);
armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
armMotor.setPower(0.1);
while (armMotor.getCurrentPosition() < 90) {
GlobalOpMode.opMode.sleep(100);
}
//extendIntake(true);
//intakeServo.setAdjustedPosition(-1350, 0.1);
}

public void readyForIntake() {
intakeServo.setAdjustedPosition(-1350, 0.1);
armMotor.setTargetPosition(-240);
armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
armMotor.setPower(0.1);
while (armMotor.getCurrentPosition() > -230) {
GlobalOpMode.opMode.sleep(100);
}
wristServo.setPosition(0.85);
intakeServo.setAdjustedPosition(-950, 0.1);
}

public void dropOff() {
intakeServo.setAdjustedPosition(-1350, 0.1);
armMotor.setTargetPosition(100);
armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
armMotor.setPower(0.1);
while (armMotor.getCurrentPosition() < 90) {
GlobalOpMode.opMode.sleep(100);
}
wristServo.setPosition(0.5);
// Raise the arm the rest of the way around
armMotor.setTargetPosition(300);
armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
armMotor.setPower(0.1);
while (armMotor.getCurrentPosition() < 290) {
GlobalOpMode.opMode.sleep(100);
}
wristServo.setPosition(0);
}
// public void initializeIntakeSystem() {
// wristServo.setPosition(0.5);
// GlobalOpMode.opMode.sleep(1000);
// armMotor.setTargetPosition(100);
// armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// armMotor.setPower(0.1);
// while (armMotor.getCurrentPosition() < 90) {
// GlobalOpMode.opMode.sleep(100);
// }
// //extendIntake(true);
// //intakeServo.setAdjustedPosition(-1350, 0.1);
// }
//
// public void readyForIntake() {
// intakeServo.setAdjustedPosition(-1350, 0.1);
// armMotor.setTargetPosition(-240);
// armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// armMotor.setPower(0.1);
// while (armMotor.getCurrentPosition() > -230) {
// GlobalOpMode.opMode.sleep(100);
// }
// wristServo.setPosition(0.85);
// intakeServo.setAdjustedPosition(-950, 0.1);
// }
//
// public void dropOff() {
// intakeServo.setAdjustedPosition(-1350, 0.1);
// armMotor.setTargetPosition(100);
// armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// armMotor.setPower(0.1);
// while (armMotor.getCurrentPosition() < 90) {
// GlobalOpMode.opMode.sleep(100);
// }
// wristServo.setPosition(0.5);
// // Raise the arm the rest of the way around
// armMotor.setTargetPosition(300);
// armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// armMotor.setPower(0.1);
// while (armMotor.getCurrentPosition() < 290) {
// GlobalOpMode.opMode.sleep(100);
// }
// wristServo.setPosition(0);
// }
}
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AnalogTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public void runOpMode() throws InterruptedException {
servoTest.innerServo.setPower(gamepad1.left_stick_x);
//servoTest.innerAnalog.getVoltage();
//servoTest.getCurrentPosition();
telemetry.addData("Degrees", 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());
Expand Down
111 changes: 76 additions & 35 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AxonServo.java
View file
Original file line number Diff line number Diff line change
@@ -1,64 +1,105 @@
package org.firstinspires.ftc.teamcode.hardware;

import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
import org.firstinspires.ftc.teamcode.util.Timeout;

import java.util.ArrayList;

public class AxonServo {
public CRServo innerServo;
public AnalogInput innerAnalog;
public AnalogInput innerAnalog = null;

// Create a new background thread to count rotations
private final ServoCounter counter;

private class ServoCounter extends Thread {
private int count = 0;

public void run() {
try {
android.util.Log.w("AXON_THREAD", "Starting thread");
double lastPosition = getCurrentPosition();
while (!GlobalOpMode.opMode.isStopRequested()) {
//android.util.Log.w("AXON_THREAD", "LOOP");
Thread.yield();
double currentPosition = getCurrentPosition();
// Positive wrap-around from 0 -> 360
if (currentPosition > 180 && lastPosition < 180 && innerServo.getPower() > 0) {
count--;
}
// Negative wrap-around from 360 -> 0
if (currentPosition < 180 && lastPosition > 180 && innerServo.getPower() < 0) {
count++;
}
lastPosition = currentPosition;
}
} catch (Exception e) {
android.util.Log.e("AXON_THREAD", "Crashed with error: " + e);
}
private int count = 0;
//private final ServoCounter counter;

// private class ServoCounter extends Thread {
// private int count = 0;



// public void run() {
// try {
// android.util.Log.w("AXON_THREAD", "Starting thread");
// double lastPosition = getCurrentPosition();
// while (!GlobalOpMode.opMode.isStopRequested()) {
// //android.util.Log.w("AXON_THREAD", "LOOP");
// Thread.yield();
// double currentPosition = getCurrentPosition();
// // Positive wrap-around from 0 -> 360
// if (currentPosition > 180 && lastPosition < 180 && innerServo.getPower() > 0) {
// count--;
// }
// // Negative wrap-around from 360 -> 0
// if (currentPosition < 180 && lastPosition > 180 && innerServo.getPower() < 0) {
// count++;
// }
// lastPosition = currentPosition;
// }
// } catch (Exception e) {
// android.util.Log.e("AXON_THREAD", "Crashed with error: " + e);
// }
// }
//
// public int getCount() {
// return count;
// }
// }

private double lastPosition = -999999999;
private void update() {
if (this.innerAnalog == null) {
return;
}
if (lastPosition == -999999999) {
lastPosition = getCurrentPosition();
}
//double lastPosition = getCurrentPosition();

public int getCount() {
return count;
double currentPosition = getCurrentPosition();
// Positive wrap-around from 0 -> 360
if (currentPosition > 180 && lastPosition < 180 && innerServo.getPower() > 0) {
count--;
}
// Negative wrap-around from 360 -> 0
if (currentPosition < 180 && lastPosition > 180 && innerServo.getPower() < 0) {
count++;
}
lastPosition = currentPosition;
}

private static final ArrayList<AxonServo> allServos = new ArrayList<>();

public static void updateAll() {
for (AxonServo servo : allServos) {
servo.update();
}
}

public AxonServo(String servoName, String analogName, HardwareMap hwMap) {
this.innerServo = hwMap.get(CRServo.class, servoName);
this.innerAnalog = hwMap.get(AnalogInput.class, analogName);
if (analogName != null) {
this.innerAnalog = hwMap.get(AnalogInput.class, analogName);
allServos.add(this);
}

counter = new ServoCounter();
counter.start();
//counter = new ServoCounter();
//counter.start();
}

public double getCurrentPosition() {
private double getCurrentPosition() {
if (this.innerAnalog == null) {

}
return this.innerAnalog.getVoltage()/3.3 * 360;
}

public double getAdjustedPosition() {
return getCurrentPosition() + counter.getCount() * 360;
return getCurrentPosition() + this.count * 360;
}

public void setAdjustedPosition(double position, double power) {
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ public void runOpMode() throws InterruptedException {

// Set servo position right away so that it holds
if (robot.getClass() == RoboticaBot.class) {
((RoboticaBot) robot).wristServo.setPosition(0.2);
//((RoboticaBot) robot).wristServo.setPosition(0.2);
}

//Pose2d startPose = new Pose2d(12,-62, Math.toRadians(90)); // RED_BOARD
Expand Down
Loading

0 comments on commit 8ba6bdb

Please sign in to comment.