eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Intake, drive and hang for prime on boiler plate.

This commit is contained in:
sihan 2023-11-01 20:50:46 -05:00
parent 3a062d75aa
commit 6f2ed777c2
8 changed files with 126 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/build.gradle
View File

@ -29,5 +29,6 @@ dependencies {
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
implementation 'org.openftc:easyopencv:1.7.0'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
}

27
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -1,27 +0,0 @@
package opmodes;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.hardware.Robot;
@TeleOp(name = "MainTeleOp", group = "Main")
public class MainTeleOp extends OpMode {
private Robot robot;
@Override
public void init() {
this.robot = new Robot(this.hardwareMap);
telemetry.addData("Status", "Initialized");
}
@Override
public void loop() {
double x = gamepad1.left_stick_x;
double y = -gamepad1.left_stick_y;
double z = gamepad1.right_stick_x;
this.robot.getDrive().setInput(x, y, z);
}
}

70
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -1,19 +1,71 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.qualcomm.robotcore.hardware.HardwareMap;
import static org.firstinspires.ftc.teamcode.util.Configurables.LOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.LOCKSPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCKSPEED;
import static org.firstinspires.ftc.teamcode.util.Constants.LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.RIGHT;
import lombok.Getter;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
public class Robot {
private MecanumDrive drive;
private Hang hang;
private Intake intake;
@Getter
private Drive drive;
public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
this.hang = new Hang().init(hardwareMap);
public Robot(HardwareMap hardwareMap) {
this.init(hardwareMap);
return this;
}
private void init(HardwareMap hardwareMap) {
this.drive = new Drive(hardwareMap);
public Intake getIntake() {return this.intake;}
public Hang getHang() {return this.hang;}
public MecanumDrive getDrive() {return this.drive;}
public static class Intake {
private DcMotor intake = null;
public Intake init(HardwareMap hardwareMap) {
this.intake = hardwareMap.dcMotor.get("intake");
this.intake.setDirection(DcMotorSimple.Direction.REVERSE);
this.intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
return this;
}
public void spinIn() {this.intake.setPower(0.7);}
public void spinOut() {this.intake.setPower(-0.7);}
}
}
public static class Hang {
private Servo hangLeft;
private Servo hangRight;
public Hang init(HardwareMap hardwareMap) {
this.hangLeft = hardwareMap.get(Servo.class, LEFT);
this.hangRight = hardwareMap.get(Servo.class, RIGHT);
return this;
}
public void lock() {
this.hangLeft.setPosition(LOCKSPEED);
this.hangRight.setPosition(LOCK);
}
public void release() {
this.hangLeft.setPosition(UNLOCKSPEED);
this.hangRight.setPosition(UNLOCK);
}
}
}

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -8,6 +8,9 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.TRACK_WIDTH;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.encoderTicksToInches;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
import androidx.annotation.NonNull;
@ -29,6 +32,7 @@ import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@ -40,9 +44,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceRunner;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LynxModuleUtil;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
@ -53,7 +55,7 @@ import java.util.List;
* Simple mecanum drive hardware implementation for REV hardware.
*/
@Config
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
@ -309,9 +311,14 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
return new ProfileAccelerationConstraint(maxAccel);
}
public void setInput(double x, double y, double z) {
Pose2d vel = new Pose2d(x, y, z);
setDrivePower(vel);
update();
public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
this.setWeightedDrivePower(
new Pose2d(
gamepad1.left_stick_y* -1,
gamepad1.left_stick_x*-1,
-gamepad1.right_stick_x
));
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java Normal file
View File

@ -0,0 +1,40 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "CenterStage", group = "Development")
public class MainTeleOp extends OpMode {
private Robot robot;
private double speedMultiplier = 1.0; // Start at 100% speed
private MecanumDrive drive;
@Override
public void init() {
this.robot = new Robot().init(hardwareMap);
this.robot.getHang().lock();
}
@Override
public void loop() {
//Drive
robot.getDrive().setInput(gamepad1, gamepad2);
//Hang
if (gamepad1.b) {
this.robot.getHang().release();
} else {
this.robot.getHang().lock();
}
//Intake
if (gamepad1.x) {
this.robot.getIntake().spinIn();
} else if (gamepad1.y) {
this.robot.getIntake().spinOut();
}
}
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
View File

@ -1,9 +1,5 @@
package org.firstinspires.ftc.teamcode.util;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Size;
public class Configurables {
// Robot Constants
public static double R_ARM_POWER = 0.2;
@ -40,4 +36,10 @@ public class Configurables {
public static double CV_MIN_GOAL_AREA = 0;
public static double CV_MAX_GOAL_AREA = 0.3;
//Servo Positions
public static double UNLOCKSPEED = .5;
public static double LOCKSPEED = .1;
public static double UNLOCK = .5;
public static double LOCK = .1;
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
View File

@ -51,4 +51,6 @@ public class Constants {
public static final String STACK_WEBCAM = "Stack Webcam";
public static final String TARGETING_WEBCAM = "Targeting Webcam";
public static final String IMU_SENSOR = "imu";
public static final String LEFT = "leftHang";
public static final String RIGHT = "rightHang";
}

2
build.gradle
View File

@ -11,7 +11,7 @@ buildscript {
}
dependencies {
// Note for FTC Teams: Do not modify this yourself.
classpath 'com.android.tools.build:gradle:7.2.0'
classpath 'com.android.tools.build:gradle:7.3.1'
}
}