Skip to content
This repository has been archived by the owner on Aug 31, 2024. It is now read-only.

Commit

Permalink
Merge pull request #68 from rh-robotics/teo-servo-test
Browse files Browse the repository at this point in the history
  • Loading branch information
@DragonDev07
DragonDev07 authored Nov 18, 2023
2 parents 101973f + b8b5d86 commit b89b9cf
Show file tree
Hide file tree
Showing 6 changed files with 147 additions and 91 deletions.
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/RRAuton.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,9 +107,9 @@ public void init_loop() {
public void loop() {
// --------------- Run Auton --------------- //
if (parkOnly) {
switch(alliance) {
switch (alliance) {
case RED:
switch(startPosition) {
switch (startPosition) {
case LEFT:
robot.drive.followTrajectory(LINESSIDE_park);
break;
Expand Down
74 changes: 34 additions & 40 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,13 @@

import androidx.annotation.NonNull;

import com.arcrobotics.ftclib.kinematics.Odometry;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.TouchSensor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.TouchSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcontroller.external.samples.SensorTouch;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.subsystems.roadrunner.drive.SampleMecanumDrive;
Expand All @@ -26,17 +23,14 @@
public class HWC {
// --------------- Declare Empty Hardware --------------- //
public DcMotorEx leftFront, rightFront, leftRear, rightRear, rightPulley, leftPulley, intakeMotor;
public Servo intakeL, intakeR, elbowL, elbowR, clawR, clawL;
public Servo intakeL, intakeR, wristL, wristR, clawR, clawL;
public TouchSensor buttonL, buttonR;
public Servo passoverArmLeft, passoverArmRight;
public WebcamName webcam;


// Other Variables
Telemetry telemetry;
public ElapsedTime time = new ElapsedTime();
public SampleMecanumDrive drive;

// Other Variables
Telemetry telemetry;
// Declare Position Variables
//TODO: UPDATE WITH REAL NUMBERS ONCE TESTED
double intakePos = 5; //made up number, needs to be tested and actually found
Expand Down Expand Up @@ -77,6 +71,8 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
clawR = hardwareMap.get(Servo.class, "clawR");
passoverArmLeft = hardwareMap.get(Servo.class, "passoverArmLeft");
passoverArmRight = hardwareMap.get(Servo.class, "passoverArmRight");
wristL = hardwareMap.get(Servo.class, "wristL");
wristR = hardwareMap.get(Servo.class, "wristR");

// Declare Sensors
buttonL = hardwareMap.get(TouchSensor.class, "buttonL");
Expand Down Expand Up @@ -111,6 +107,26 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
intakeMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
}

/**
* Initialize the AprilTag processor.
*/
public static void initAprilTag(AprilTagProcessor aprilTag, VisionPortal visionPortal, HWC robot) {

aprilTag = new AprilTagProcessor.Builder()
.setDrawTagOutline(true)
//.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
.build();

// Create the vision portal by using a builder.
VisionPortal.Builder builder = new VisionPortal.Builder();

builder.setCamera(robot.webcam); // Set Camera to webcam
builder.setCameraResolution(new Size(640, 480)); // Set Camera Resolution
builder.enableLiveView(true); // Enable preview on Robot Controller (Driver Station)
builder.addProcessor(aprilTag); // Set and enable the processor
visionPortal = builder.build(); // Build the Vision Portal, using the above settings
}

public void runIntake(double pwr) {
intakeMotor.setPower(pwr);
}
Expand Down Expand Up @@ -163,41 +179,19 @@ public void fullIntake() {

}

public void moveArmToDelivery(){
elbowL.setPosition(elbowDeliveryPos);
elbowR.setPosition(elbowDeliveryPos);
public void moveArmToDelivery() {
wristL.setPosition(elbowDeliveryPos);
wristR.setPosition(elbowDeliveryPos);
rightPulley.setTargetPosition(armPos);
leftPulley.setTargetPosition(armPos);
}

/**
* Initialize the AprilTag processor.
*/
public static void initAprilTag(AprilTagProcessor aprilTag, VisionPortal visionPortal, HWC robot) {

aprilTag = new AprilTagProcessor.Builder()
.setDrawTagOutline(true)
//.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
.build();

// Create the vision portal by using a builder.
VisionPortal.Builder builder = new VisionPortal.Builder();

builder.setCamera(robot.webcam); // Set Camera to webcam
builder.setCameraResolution(new Size(640, 480)); // Set Camera Resolution
builder.enableLiveView(true); // Enable preview on Robot Controller (Driver Station)
builder.addProcessor(aprilTag); // Set and enable the processor
visionPortal = builder.build(); // Build the Vision Portal, using the above settings
}

public void deliver(char claw){
if (claw == 'L'){
public void deliver(char claw) {
if (claw == 'L') {
toggleClaw('L');
}
else if (claw == 'R'){
} else if (claw == 'R') {
toggleClaw('R');
}
else{
} else {
toggleClaw('O');
}

Expand Down
11 changes: 5 additions & 6 deletions ...pires/ftc/teamcode/subsystems/roadrunner/trajectorysequence/TrajectorySequenceRunner.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,18 +49,17 @@ public class TrajectorySequenceRunner {
private final NanoClock clock;
private final FtcDashboard dashboard;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
private final VoltageSensor voltageSensor;
private final List<Integer> lastDriveEncPositions;
private final List<Integer> lastDriveEncVels;
private final List<Integer> lastTrackingEncPositions;
private final List<Integer> lastTrackingEncVels;
List<TrajectoryMarker> remainingMarkers = new ArrayList<>();
private TrajectorySequence currentTrajectorySequence;
private double currentSegmentStartTime;
private int currentSegmentIndex;
private int lastSegmentIndex;
private Pose2d lastPoseError = new Pose2d();
private final VoltageSensor voltageSensor;

private final List<Integer> lastDriveEncPositions;
private final List<Integer> lastDriveEncVels;
private final List<Integer> lastTrackingEncPositions;
private final List<Integer> lastTrackingEncVels;

public TrajectorySequenceRunner(
TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients, VoltageSensor voltageSensor,
Expand Down
4 changes: 3 additions & 1 deletion ...Code/src/main/java/org/firstinspires/ftc/teamcode/subsystems/roadrunner/util/Encoder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,12 @@ public class Encoder {
private final static int CPS_STEP = 0x10000;
private final DcMotorEx motor;
private final NanoClock clock;
private final double[] velocityEstimates;
private Direction direction;
private int lastPosition;
private int velocityEstimateIdx;
private final double[] velocityEstimates;
private double lastUpdateTime;

public Encoder(DcMotorEx motor, NanoClock clock) {
this.motor = motor;
this.clock = clock;
Expand All @@ -27,6 +28,7 @@ public Encoder(DcMotorEx motor, NanoClock clock) {
this.velocityEstimates = new double[3];
this.lastUpdateTime = clock.seconds();
}

public Encoder(DcMotorEx motor) {
this(motor, NanoClock.system());
}
Expand Down
127 changes: 94 additions & 33 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ServoTesting.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,22 +2,28 @@

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.teamcode.subsystems.HWC;

/***
* TeleOp for simply running the passover servos, will be deleted
*/

enum ServoControl {
LEFT,
RIGHT
PASSOVER_LEFT,
PASSOVER_RIGHT,
WRIST_LEFT,
WRIST_RIGHT,
CLAW_LEFT,
CLAW_RIGHT
}

/***
* TeleOp for simply running the passover servos, will be deleted
*/
@TeleOp(name = "ServoTesting", group = "Testing")
public class ServoTesting extends OpMode {
HWC robot; // Robot Hardware Class - Contains all hardware devices & related methods
ServoControl servoControl = ServoControl.LEFT;
HWC robot;
ServoControl servoControl = ServoControl.PASSOVER_LEFT;
final double SERVO_SPEED = 0.001;
double servoPosition;

// init() Runs ONCE after the driver hits initialize
@Override
Expand All @@ -32,58 +38,113 @@ public void init() {
// Move servos to position 0
robot.passoverArmLeft.setPosition(0);
robot.passoverArmRight.setPosition(0);
robot.clawL.setPosition(0);
robot.clawR.setPosition(0);
robot.wristL.setPosition(0);
robot.wristR.setPosition(0);

// Tell the driver the robot is ready
telemetry.addData("Status", "Initialized");
telemetry.update();
}

// init_loop() - Runs continuously until the driver hits play
@Override
public void init_loop() {}

// Start() - Runs ONCE when the driver presses play
@Override
public void start() { robot.time.reset(); }
public void start() {
robot.time.reset();
}

// loop() - Runs continuously while the OpMode is active
@Override
public void loop() {
double passoverPositionLeft = robot.passoverArmLeft.getPosition();
double passoverPositionRight = robot.passoverArmRight.getPosition();
switch (servoControl) {
case PASSOVER_LEFT:
servoPosition = robot.passoverArmLeft.getPosition();
break;
case PASSOVER_RIGHT:
servoPosition = robot.passoverArmRight.getPosition();
break;
case WRIST_LEFT:
servoPosition = robot.wristL.getPosition();
break;
case WRIST_RIGHT:
servoPosition = robot.wristR.getPosition();
break;
case CLAW_LEFT:
servoPosition = robot.clawL.getPosition();
break;
case CLAW_RIGHT:
servoPosition = robot.clawR.getPosition();
break;
}


// --------------- Update Which Servo Is Being Controlled --------------- //
if(gamepad1.a) { servoControl = ServoControl.LEFT; }
else if(gamepad1.b) { servoControl = ServoControl.RIGHT; }
if (gamepad1.left_bumper && gamepad1.a)
servoControl = ServoControl.PASSOVER_LEFT;
else if (gamepad1.right_bumper && gamepad1.a)
servoControl = ServoControl.PASSOVER_RIGHT;
else if (gamepad1.left_bumper && gamepad1.b)
servoControl = ServoControl.WRIST_LEFT;
else if (gamepad1.right_bumper && gamepad1.b)
servoControl = ServoControl.WRIST_RIGHT;
else if (gamepad1.left_bumper && gamepad1.x)
servoControl = ServoControl.CLAW_LEFT;
else if (gamepad1.right_bumper && gamepad1.x)
servoControl = ServoControl.CLAW_RIGHT;

if (gamepad1.left_bumper || gamepad1.right_bumper || gamepad1.a || gamepad1.b || gamepad1.x || gamepad1.y)
servoPosition = servoPosition = 0;

// --------------- Update Servo Position Values --------------- //
switch(servoControl) {
case LEFT:
if(gamepad1.dpad_up) { passoverPositionLeft += 0.01; }
else if(gamepad1.dpad_down) { passoverPositionLeft -= 0.01; }
break;
case RIGHT:
if(gamepad1.dpad_up) { passoverPositionRight += 0.01; }
else if(gamepad1.dpad_down) { passoverPositionRight -= 0.01; }
break;
if (gamepad1.dpad_up) {
servoPosition += SERVO_SPEED;
} else if (gamepad1.dpad_down) {
servoPosition -= SERVO_SPEED;
}

// --------------- Move Servos --------------- //
robot.passoverArmLeft.setPosition(passoverPositionLeft);
robot.passoverArmRight.setPosition(passoverPositionRight);
switch (servoControl) {
case PASSOVER_LEFT:
robot.passoverArmLeft.setPosition(servoPosition);
break;
case PASSOVER_RIGHT:
robot.passoverArmRight.setPosition(servoPosition);
break;
case WRIST_LEFT:
robot.wristL.setPosition(servoPosition);
break;
case WRIST_RIGHT:
robot.wristR.setPosition(servoPosition);
break;
case CLAW_LEFT:
robot.clawL.setPosition(servoPosition);
break;
case CLAW_RIGHT:
robot.clawR.setPosition(servoPosition);
break;
}

// --------------- Telemetry Updates --------------- //
telemetry.addData("Status", "Running");
telemetry.addLine();
telemetry.addData("Press 'A' to control left servo", "");
telemetry.addData("Press 'B' to control right servo", "");
telemetry.addData("Press 'DPAD_UP'/'DPAD_DOWN' to increase/decrease servo target position", "");
telemetry.addData("Press 'Left Bumper' and 'A' to control PASSOVER_LEFT", "");
telemetry.addData("Press 'Right Bumper' and 'A' to control PASSOVER_RIGHT", "");
telemetry.addData("Press 'Left Bumper' and 'B' to control WRIST_LEFT", "");
telemetry.addData("Press 'Right Bumper' and 'B' to control WRIST_RIGHT", "");
telemetry.addData("Press 'Left Bumper' and 'X' to control CLAW_LEFT", "");
telemetry.addData("Press 'Right Bumper' and 'X' to control CLAW_RIGHT", "");
telemetry.addData("Press 'DPAD_UP'/'DPAD_DOWN' to increase/decrease servo(s) target position", "");
telemetry.addLine();
telemetry.addData("Controlling Servo", servoControl);
telemetry.addData("Passover Left Target Position", passoverPositionLeft);
telemetry.addData("Passover Right Target Position", passoverPositionRight);
telemetry.addData("Servo Target Position", servoPosition);
telemetry.addLine();
telemetry.addData("Passover Left Actual Position", robot.passoverArmLeft.getPosition());
telemetry.addData("Passover Right Actual Position", robot.passoverArmRight.getPosition());
telemetry.addData("Wrist Left Actual Position", robot.wristL.getPosition());
telemetry.addData("Wrist Right Actual Position", robot.wristR.getPosition());
telemetry.addData("Claw Left Actual Position", robot.clawL.getPosition());
telemetry.addData("Claw Right Actual Position", robot.clawR.getPosition());
telemetry.update();
}
}
18 changes: 9 additions & 9 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOpMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,14 +21,6 @@ enum MultiplierSelection {
@TeleOp(name = "TeleOp", group = "Iterative OpMode")
public class TeleOpMode extends OpMode {
HWC robot; // Declare the object for HWC, will allow us to access all the motors declared there!

public enum RobotState {
DRIVING,
INTAKING,
DELIVERYING,
RESTING,
UNKNOWN
}
RobotState state;
double turnSpeed = 0.6; // Speed multiplier for turning
double driveSpeed = 0.8; // Speed multiplier for driving
Expand Down Expand Up @@ -153,7 +145,7 @@ public void loop() {
robot.runIntake(gamepad1.right_trigger);
state = RobotState.INTAKING;
}
if (gamepad1.left_trigger != 0){
if (gamepad1.left_trigger != 0) {
robot.runIntake(-gamepad1.left_trigger);
state = RobotState.INTAKING;
}
Expand Down Expand Up @@ -196,4 +188,12 @@ public void loop() {
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBPower, rightBPower);
telemetry.update();
}

public enum RobotState {
DRIVING,
INTAKING,
DELIVERYING,
RESTING,
UNKNOWN
}
}

0 comments on commit b89b9cf

Please sign in to comment.