TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-16 01:02:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

got the field centric drive back to normal, collector and deppositor work

Browse Source
This commit is contained in:
SpencerPiha
2023-11-11 11:56:51 -06:00
parent 0d35a20e5f
commit 4f55a49590
3 changed files with 111 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java
View File

@@ -72,10 +72,10 @@ public class DriveToCoordinatesState extends CyberarmState {
targetVelocityBR = currentVelocityX - currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity));
targetVelocityFR = currentVelocityX + currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity));
robot.frontLeft.setVelocity((targetVelocityFL / robot.R));
robot.backLeft.setVelocity((targetVelocityBL / robot.R));
robot.backRight.setVelocity((targetVelocityBR / robot.R));
robot.frontRight.setVelocity((targetVelocityFR / robot.R));
// robot.frontLeft.setVelocity((targetVelocityFL / robot.R));
// robot.backLeft.setVelocity((targetVelocityBL / robot.R));
// robot.backRight.setVelocity((targetVelocityBR / robot.R));
// robot.frontRight.setVelocity((targetVelocityFR / robot.R));
// setHasFinished(true)
}

110
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
View File

@@ -9,6 +9,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.Library.Robot;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
@@ -38,12 +39,14 @@ public class PrototypeRobot extends Robot {
public float currentSetPosShoulder;
public float currentSetPosElbow;
private HardwareMap hardwareMap;
public MotorEx frontLeft, frontRight, backLeft, backRight, lift;
public DcMotor frontLeft, frontRight, backLeft, backRight, lift;
public DcMotor odometerR, odometerL, odometerA;
public IMU imu;
public Servo depositorShoulder, depositorElbow, collectorShoulder, collectorElbow, depositor;
public Servo depositorShoulder, depositorElbow, collectorShoulder, collectorElbow, depositor, collector;
private HDrive xDrive;
private String string;
private double drivePower = 1;
public double xMultiplier = 1;
public double yMultiplier = 1;
public double positionX;
@@ -63,6 +66,7 @@ public class PrototypeRobot extends Robot {
public double localPositionX;
public double localPositionY;
public double localPositionH;
public final static double L = 23.425; // distance between left and right encoder in cm
final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
public final static double R = 4.5; // wheel radius in cm
@@ -71,6 +75,12 @@ public class PrototypeRobot extends Robot {
public final double MaxVelocityForward = 40;
public final double MaxStrafeVelocity = 34;
public final double MaxRotationalVelocity = 20;
private boolean lbsVar1;
private boolean lbsVar2;
private boolean rbsVar2;
public float depositorPos;
public float collectorPos;
@@ -107,28 +117,28 @@ public class PrototypeRobot extends Robot {
imu = engine.hardwareMap.get(IMU.class, "imu");
//MOTORS
frontRight = new MotorEx(hardwareMap, "frontRight");
frontLeft = new MotorEx(hardwareMap, "frontLeft");
backRight = new MotorEx(hardwareMap, "backRight");
backLeft = new MotorEx(hardwareMap, "backLeft");
lift = new MotorEx(hardwareMap, "Lift");
frontRight = engine.hardwareMap.dcMotor.get("frontRight");
frontLeft = engine.hardwareMap.dcMotor.get("frontLeft");
backRight = engine.hardwareMap.dcMotor.get("backRight");
backLeft = engine.hardwareMap.dcMotor.get("backLeft");
lift = engine.hardwareMap.dcMotor.get("Lift");
configuration = new TimeCraftersConfiguration("Blue Crab");
initConstants();
frontRight.motor.setDirection(DcMotorSimple.Direction.FORWARD);
backRight.motor.setDirection(DcMotorSimple.Direction.FORWARD);
backLeft.motor.setDirection(DcMotorSimple.Direction.FORWARD);
frontLeft.motor.setDirection(DcMotorSimple.Direction.FORWARD);
lift.motor.setDirection(DcMotorSimple.Direction.FORWARD);
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
backRight.setDirection(DcMotorSimple.Direction.FORWARD);
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
lift.setDirection(DcMotorSimple.Direction.FORWARD);
frontRight.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRight.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeft.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontLeft.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
lift.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
//IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
@@ -143,10 +153,13 @@ public class PrototypeRobot extends Robot {
collectorShoulder = hardwareMap.servo.get("collector_shoulder");
collectorElbow = hardwareMap.servo.get("collector_elbow");
depositor = hardwareMap.servo.get("depositor");
collector = hardwareMap.servo.get("collector");
depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN);
depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN);
// input motors exactly as shown below
xDrive = new HDrive(frontLeft, frontRight,
backLeft, backRight);
// depositorShoulder.setPosition(COLLECTOR_SHOULDER_IN);
// depositorElbow.setPosition(COLLECTOR_ELBOW_IN);
@@ -154,7 +167,38 @@ public class PrototypeRobot extends Robot {
}
public void driveTrainTeleOp() {
xDrive.driveRobotCentric(-engine.gamepad1.left_stick_x, engine.gamepad1.left_stick_y, -engine.gamepad1.right_stick_x);
boolean lbs1 = engine.gamepad1.left_stick_button;
if (lbs1 && !lbsVar1) {
if (drivePower == 1) {
drivePower = 0.5;
} else {
drivePower = 1;
}
}
lbsVar1 = lbs1;
double y = -engine.gamepad1.left_stick_y;
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = engine.gamepad1.right_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
// angle math to make things field oriented
double heading = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rotX = x * Math.cos(heading) - y * Math.sin(heading);
double rotY = x * Math.sin(heading) + y * Math.cos(heading);
// joystick math to find the approximate power across each wheel for a movement
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY + rotX - rx) / denominator;
double backRightPower = (rotY - rotX - rx) / denominator;
// setting each power determined previously from the math above
// as well as multiplying it by a drive power that can be changed.
backLeft.setPower(backLeftPower * drivePower);
backRight.setPower(backRightPower * drivePower);
frontLeft.setPower(frontLeftPower * drivePower);
frontRight.setPower(frontRightPower * drivePower);
}
public void ShoulderServoWaitTime(){
@@ -205,6 +249,28 @@ public class PrototypeRobot extends Robot {
}
public void CollectorToggle(){
boolean lbs2 = engine.gamepad2.left_stick_button;
if (lbs2 && !lbsVar2) {
if (collectorPos == 1F) {
collectorPos = 0F;
} else {
collectorPos = 1F;
}
}
lbsVar2 = lbs2;
}
public void DepositorToggle(){
boolean rbs2 = engine.gamepad2.right_stick_button;
if (rbs2 && !rbsVar2) {
if (depositorPos == 1F) {
depositorPos = 0F;
} else {
depositorPos = 1F;
}
}
rbsVar2 = rbs2;
}
public void ArmSequences(){
switch (armPosition) {
case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos
@@ -251,7 +317,7 @@ public class PrototypeRobot extends Robot {
}
break;
case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos
case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos
switch (oldArmPosition) {
case 0:
// transfer

31
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java
View File

@@ -21,23 +21,23 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
private void sliderTeleOp(){
if (engine.gamepad1.right_trigger != 0){
if (robot.lift.getCurrentPosition() >= maxExtension){
robot.lift.motor.setPower(0);
robot.lift.setPower(0);
} else if (robot.lift.getCurrentPosition() >= maxExtension - 200){
robot.lift.motor.setPower(0.35);
robot.lift.setPower(0.35);
}else {
robot.lift.motor.setPower(engine.gamepad1.right_trigger);
robot.lift.setPower(engine.gamepad1.right_trigger);
}
} else if (engine.gamepad1.left_trigger != 0){
if (robot.lift.getCurrentPosition() <= minExtension) {
robot.lift.motor.setPower(0);
robot.lift.setPower(0);
} else if (robot.lift.getCurrentPosition() < 350){
robot.lift.motor.setPower(-0.3);
robot.lift.setPower(-0.3);
}else {
robot.lift.motor.setPower(-engine.gamepad1.left_trigger);
robot.lift.setPower(-engine.gamepad1.left_trigger);
}
} else {
robot.lift.motor.setPower(0);
robot.lift.setPower(0);
}
}
@@ -53,23 +53,30 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
robot.armPosition = 3;
}
robot.depositor.setPosition(robot.depositorPos);
robot.collector.setPosition(robot.collectorPos);
// drivetrain
robot.driveTrainTeleOp();
// arm sequencer
robot.ArmSequences();
// lift
sliderTeleOp();
// collector depositor
robot.CollectorToggle();
// depositor toggle
robot.DepositorToggle();
}
@Override
public void telemetry() {
engine.telemetry.addData("Lift Encoder Pos", robot.lift.motor.getCurrentPosition());
engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition());
engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
// engine.telemetry.addData("Elbow Collect", robot.ELBOW_COLLECT);
// engine.telemetry.addData("Elbow Deposit", robot.ELBOW_DEPOSIT);
// engine.telemetry.addData("Shoulder Collect", robot.SHOULDER_COLLECT);
// engine.telemetry.addData("Shoulder Deposit", robot.SHOULDER_DEPOSIT);
engine.telemetry.addData("arm Pos", robot.armPosition);
engine.telemetry.addData("old arm pos", robot.oldArmPosition);
engine.telemetry.addData("depositor pos", robot.depositorPos);
engine.telemetry.addData("collector pos", robot.collectorPos);
}
}