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

FOr senor bot

This commit is contained in:
ImposterVarunoverlord 2024-03-08 11:03:17 -06:00
parent d863843f94
commit 3dbbdeb30d
8 changed files with 119 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
View File

@ -22,18 +22,22 @@ public class Arm {
private double armTempPos;
private double doorPos;
private double wristPos;
public static double ARM_KP = 0.2;
public static double ARM_KP = 0.18;
public static double doorOpenPos = 0.36;
public static double doorClosePos = 0.61;
public static double armStart = 0.24;
public static double armScore = 0.95;
public static double armScore_highMacro = 0.95;
public static double wristStart = 0.29;
public static double wristScore = 0.5;
public static double wristScore_highMacro = 0.3;
public enum Position {
INTAKE, SCORE
INTAKE, SCORE, SCOREHI
}
public enum DoorPosition {
@ -81,6 +85,9 @@ public class Arm {
wristPos = wristStart;
} else if (pos == Position.SCORE) {
wristPos = wristScore;
} else if (pos == Position.SCOREHI) {
wristPos = wristScore_highMacro;
}
}

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java
View File

@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@ -12,6 +13,8 @@ import com.qualcomm.robotcore.hardware.Servo;
public class Intake {
private Servo rServo;
private Servo lServo;
private CRServo WheelServo;
private DcMotor dcMotor;
private Position pos = Position.UP;
@ -45,6 +48,7 @@ public class Intake {
lServo.setDirection(Servo.Direction.REVERSE);
rServo = hardwareMap.get(Servo.class, "Right Intake Servo");
dcMotor = hardwareMap.get(DcMotor.class, "Intakemotor");
WheelServo = hardwareMap.get(CRServo.class, "Wheel");
}
@ -92,6 +96,20 @@ public class Intake {
}
}
public void wheel_spit(){
WheelServo.setPower(1);
}
public void wheel_swallow(){
WheelServo.setPower(-1);
}
public void wheel_stop(){
WheelServo.setPower(0);
}
public double getPower() {
return dcMotor.getPower();
}

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

@ -106,6 +106,85 @@ public class Robot {
break;
}
}
public void extendMacro_auto(int slidePos, double runTime, int slidepos2 ) {
switch(macroState) {
case(0):
macroStartTime = runTime;
slides.setTarget(slidePos);
arm.setDoor(CLOSE);
macroState ++;
break;
case(1):
if (runTime > macroStartTime + slideWait || slides.atTarget()) {
macroState ++;
}
break;
case(2):
arm.setArmPos(Arm.Position.SCORE);
arm.setWristPos(Arm.Position.SCORE);
macroState++;
break;
case (3):
if(arm.armAtPosition()){
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
macroState++;
}
break;
case (4):
slides.setTarget(slidepos2);
break;
}
}
public void resetMacro_auto(int slidePos2, double runTime, int slidePos) {
switch(macroState) {
case(0):
macroStartTime = runTime;
arm.setDoor(OPEN);
macroState++;
break;
case(1):
if (runTime > macroStartTime + scoreWait) {
slides.setTarget(slidePos);
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.INTAKE);
arm.setWristPos(Arm.Position.INTAKE);
macroState++;
break;
case(3):
if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) {
macroState ++;
}
break;
case(4):
slides.setTarget(slidePos2);
macroState++;
break;
case (5):
if (slides.atTarget()){
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
}
break;
}
}
public void update(double runTime) {
drive.update();

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java
View File

@ -17,7 +17,7 @@ public class Slides {
// public static double d = 0;
// public static double f = 0.01;
//p was 0.0014
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.05,0,0.01);
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.04,0,0.01);
public static double pTolerance = 20;
private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d);
@ -25,6 +25,8 @@ public class Slides {
public static int targetMax = 830;
public static int down = 0;
public static int micro_tier1 = 50;
public static int mini_tier1 = 250;
public static int tier1 = 350;
public static int tier2 = 500;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java
View File

@ -96,7 +96,7 @@ public class BlueBackStageAuto extends AutoBase {
else {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro_auto(Slides.mini_tier1, getRuntime(), Slides.micro_tier1);
builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel
switch (teamPropLocation) {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java
View File

@ -97,7 +97,7 @@ public class RedBackStageAuto extends AutoBase {
else {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro_auto(0, getRuntime(),Slides.mini_tier1);
builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel
switch (teamPropLocation) {

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java
View File

@ -147,6 +147,13 @@ public class MainTeleOp extends OpMode {
this.robot.endGameMechs.hang_init_pos();
}
if (controller1.getX().isPressed()) {
this.robot.intake.wheel_spit();
} else if (controller1.getY().isPressed()) {
this.robot.intake.wheel_swallow();
} else {
this.robot.intake.wheel_stop();
}
// Driver 2

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

@ -43,7 +43,7 @@ public class Constants {
public static final String WHEEL_FRONT_RIGHT = "frontRight";
public static final String WHEEL_BACK_LEFT = "backLeft";
public static final String WHEEL_BACK_RIGHT = "backRight";
public static final String WEBCAM_NAME = "Targeting Webcam";
public static final String WEBCAM_NAME = "FrontWebcam";
public static final String ARM = "wobbler";
public static final String CLAW = "claw";
public static final String INTAKE = "intake";