Skip to content

Commit

Permalink
working
Browse files Browse the repository at this point in the history
  • Loading branch information
@david-safro
david-safro committed Nov 23, 2024
1 parent 43ab713 commit 1c93a61
Show file tree
Hide file tree
Showing 3 changed files with 87 additions and 64 deletions.
54 changes: 28 additions & 26 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,9 @@
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.robotcore.external.Const;
import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
import org.firstinspires.ftc.teamcode.robot.Constants;
import org.firstinspires.ftc.teamcode.robot.Robot;
import org.firstinspires.ftc.teamcode.wrappers.FCDrivingWrapper;
import org.firstinspires.ftc.teamcode.wrappers.JoystickWrapper;
Expand All @@ -16,47 +18,47 @@
public class DriveTest extends LinearOpMode {

public static class Params {
public static double CLAW_OPEN = 0.3;
public static double CLAW_CLOSE = 0.67;
public static double CLAW_OPEN = Constants.CLAW_OPEN;
public static double CLAW_CLOSE = Constants.CLAW_CLOSE;

public static double ROT_SERVO_DEFAULT = 0.86;
public static double ROT_SERVO_BACK = 0.18;
public static double ROT_SERVO_DEFAULT = Constants.ROT_SERVO_DEFAULT;
public static double ROT_SERVO_BACK = Constants.ROT_SERVO_BACK;

public static double CLAW_ANGLE_FORWARD = 0;
public static double CLAW_ANGLE_DOWN = 0.3;
public static double CLAW_ANGLE_BACK = 0.6;
public static double CLAW_ANGLE_FORWARD = Constants.CLAW_ANGLE_FORWARD;
public static double CLAW_ANGLE_DOWN = Constants.CLAW_ANGLE_DOWN;
public static double CLAW_ANGLE_BACK = Constants.CLAW_ANGLE_BACK;

public static int CLAW_SLIDER_FORWARD = 8000;
public static int CLAW_SLIDER_DOWN = 0;
public static int CLAW_SLIDER_BACK = -9300;
public static int CLAW_SLIDER_FORWARD = Constants.CLAW_SLIDER_FORWARD;
public static int CLAW_SLIDER_DOWN = Constants.CLAW_SLIDER_DOWN;
public static int CLAW_SLIDER_BACK = Constants.CLAW_SLIDER_BACK;

public static int HORIZONTAL_SLIDE_INTAKE_INITIAL = 200;
public static int HORIZONTAL_SLIDE_INTAKE_INITIAL = Constants.HORIZONTAL_SLIDE_INTAKE_INITIAL;

public static double INTAKE_ANGLE_TRANSFER = .48;
public static double INTAKE_ANGLE_DOWN = .51;
public static int VERTICAL_SLIDE_POSITION = 140;
public static double INTAKE_ANGLE_TRANSFER = Constants.INTAKE_ANGLE_TRANSFER;
public static double INTAKE_ANGLE_DOWN = Constants.INTAKE_ANGLE_DOWN;
public static int VERTICAL_SLIDE_POSITION = Constants.VERTICAL_SLIDE_POSITION;

public static int VERTICAL_SLIDE_TRANSFER_POSITION = 420;
public static int VERTICAL_SLIDE_TRANSFER_POSITION = Constants.VERTICAL_SLIDE_TRANSFER_POSITION;

public static int VERTICAL_SLIDE_WALL_POSITION = 20;
public static int VERTICAL_SLIDE_WALL_POSITION = Constants.VERTICAL_SLIDE_WALL_POSITION;

public static int VERTICAL_SLIDE_HANG_PREP_POSITION = 1450;
public static int VERTICAL_SLIDE_HANG_PREP_POSITION = Constants.VERTICAL_SLIDE_HANG_PREP_POSITION;

public static int VERTICAL_SLIDE_HANG_DROP_POSITION = 1400;
public static int VERTICAL_SLIDE_HANG_DROP_POSITION = Constants.VERTICAL_SLIDE_HANG_DROP_POSITION;

public static int VERTICAL_SLIDE_DROP_L1 = 1800;
public static int VERTICAL_SLIDE_DROP_L1 = Constants.VERTICAL_SLIDE_DROP_L1;

public static int VERTICAL_SLIDE_DROP_L2 = 1800;
public static int VERTICAL_SLIDE_DROP_L2 = Constants.VERTICAL_SLIDE_DROP_L2;

public static int HORIZONTAL_SLIDE_TRANSFER_POSITION = 100;
public static int HORIZONTAL_SLIDE_TRANSFER_POSITION = Constants.HORIZONTAL_SLIDE_TRANSFER_POSITION;

public static int HORIZONTAL_SLIDE_MAX_POSITION = 300;
public static int HORIZONTAL_SLIDE_MAX_POSITION = Constants.HORIZONTAL_SLIDE_MAX_POSITION;

public static double VERTICAL_SLIDE_PID_kp = 0.027;
public static double VERTICAL_SLIDE_PID_kd = 0.027;
public static double VERTICAL_SLIDE_PID_ki = 0.0001;
public static double VERTICAL_SLIDE_PID_kp = Constants.VERTICAL_SLIDE_PID_kp;
public static double VERTICAL_SLIDE_PID_kd = Constants.VERTICAL_SLIDE_PID_kd;
public static double VERTICAL_SLIDE_PID_ki = Constants.VERTICAL_SLIDE_PID_ki;

public static int VERTICAL_SLIDE_PID_target = 400;
public static int VERTICAL_SLIDE_PID_target = Constants.VERTICAL_SLIDE_PID_target;

}

Expand Down
44 changes: 36 additions & 8 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,45 @@
package org.firstinspires.ftc.teamcode.robot;

public class Constants {
public static boolean initialStateCalled = false;
public static double CLAW_OPEN = 0.3;
public static double CLAW_CLOSE = 0.67;

public static double ROT_SERVO_DEFAULT = 0.86;
public static double ROT_SERVO_BACK = 0.18;

private static final double CLAW_UP = 0.3;
private static final double CLAW_DOWN = 0.7;
private static final double ROT_SERVO = 0.5;
public static double CLAW_ANGLE_FORWARD = 0;
public static double CLAW_ANGLE_DOWN = 0.3;
public static double CLAW_ANGLE_BACK = 0.6;

// private static final double
public static int CLAW_SLIDER_FORWARD = 8000;
public static int CLAW_SLIDER_DOWN = 0;
public static int CLAW_SLIDER_BACK = -9300;

public Constants(boolean initialStateCalled){
this.initialStateCalled = initialStateCalled;
public static int HORIZONTAL_SLIDE_INTAKE_INITIAL = 200;

}
public static double INTAKE_ANGLE_TRANSFER = .48;
public static double INTAKE_ANGLE_DOWN = .51;
public static int VERTICAL_SLIDE_POSITION = 140;

public static int VERTICAL_SLIDE_TRANSFER_POSITION = 420;

public static int VERTICAL_SLIDE_WALL_POSITION = 20;

public static int VERTICAL_SLIDE_HANG_PREP_POSITION = 1450;

public static int VERTICAL_SLIDE_HANG_DROP_POSITION = 1400;

public static int VERTICAL_SLIDE_DROP_L1 = 1800;

public static int VERTICAL_SLIDE_DROP_L2 = 1800;

public static int HORIZONTAL_SLIDE_TRANSFER_POSITION = 100;

public static int HORIZONTAL_SLIDE_MAX_POSITION = 300;

public static double VERTICAL_SLIDE_PID_kp = 0.027;
public static double VERTICAL_SLIDE_PID_kd = 0.027;
public static double VERTICAL_SLIDE_PID_ki = 0.0001;

public static int VERTICAL_SLIDE_PID_target = 400;
}
53 changes: 23 additions & 30 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,25 +20,6 @@

public class Robot {

public static final double CLAW_OPEN = 0.3;
public static final double CLAW_CLOSE = 0.67;

public static final double ROT_SERVO_DEFAULT = 0.4;

public static final double CLAW_ANGLE_FORWARD = 1.0;
public static final double CLAW_ANGLE_DOWN = 0.5;
public static final double CLAW_ANGLE_BACK = 0.0;

public static final int CLAW_SLIDER_FORWARD = 0;
public static final int CLAW_SLIDER_DOWN = 2000;
public static final int CLAW_SLIDER_BACK = -21110;

public static final int HORIZONTAL_SLIDE_INTAKE_INITIAL = 200;

public static final double INTAKE_ANGLE_TRANSFER = .48;
public static final double INTAKE_ANGLE_DOWN = .51;
public static final int VERTICAL_SLIDE_POSITION = 140;

//private static Robot instance;
private IRobot currentState;
private final IDrive drive;
Expand Down Expand Up @@ -129,50 +110,62 @@ public void execute(Telemetry telemetry) {

}

public void setHorizontalSlideTargetPosition(int target) {
public Robot setHorizontalSlideTargetPosition(int target) {
horizontalSlideController.setTargetPosition(target);
return this;
}

public void setVerticalSlideTargetPosition(int target) {
public Robot setVerticalSlideTargetPosition(int target) {
verticalSlideController.setTargetPosition(target);
return this;
}

public void increseVerticalSlideTargetPosition(int target) {
public Robot increseVerticalSlideTargetPosition(int target) {
verticalSlideController.increaseTargetPosition(target);
return this;
}

public void increseHorizontalSlideTargetPosition(int target) {
public Robot increseHorizontalSlideTargetPosition(int target) {
horizontalSlideController.increaseTargetPosition(target);
return this;
}

public void setClawSlideTargetPosition(int target) {
public Robot setClawSlideTargetPosition(int target) {
clawSlideController.setTargetPosition(target);
return this;
}

public void setClawAnglePosition(double position) {
public Robot setClawAnglePosition(double position) {
clawAngleServo.setPosition(position);
return this;
}

public double getClawAnglePosition() {return clawAngleServo.getPosition();}
public double getClawAnglePosition() {
return clawAngleServo.getPosition();
}

public void setClawRotationPosition(double position) {
public Robot setClawRotationPosition(double position) {
clawRotationServo.setPosition(position);
return this;
}

public double getClawRotationPosition() {
return clawRotationServo.getPosition();
}

public void setClawPosition(double position) {
public Robot setClawPosition(double position) {
clawServo.setPosition(position);
return this;
}

public void setIntakePower(double power) {
public Robot setIntakePower(double power) {
intakeServo.setPower(power);
return this;
}

public void setIntakeAngleServoPosition(double position) {
public Robot setIntakeAngleServoPosition(double position) {
intakeAngleServo.setPosition(position);
return this;
}

public int getVerticalSlidePosition(){
Expand Down

0 comments on commit 1c93a61

Please sign in to comment.