Skip to content

Commit 9a33acf

Browse files
edsoneliedsoneli
committed
auto for 26
1 parent 2e061bf commit 9a33acf

File tree

5 files changed

+977
-0
lines changed

5 files changed

+977
-0
lines changed

‎TeamCode/src/main/java/org/mrpsvt/capital_robotics/auto/blue_close.java‎

Lines changed: 204 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,204 @@
1+
package org.mrpsvt.capital_robotics.auto;
2+
3+
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
4+
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
5+
import com.qualcomm.robotcore.hardware.DcMotor;
6+
import com.qualcomm.robotcore.hardware.DcMotorEx;
7+
import com.qualcomm.robotcore.hardware.Servo;
8+
9+
@Autonomous(name="blue far")
10+
public class blue_close extends LinearOpMode {
11+
// Flywheels
12+
private DcMotorEx flywheel;
13+
private DcMotorEx flywheel2;
14+
private DcMotorEx lodewheel;
15+
16+
// Drive motors
17+
private DcMotor frontLeft;
18+
private DcMotor frontRight;
19+
private DcMotor backLeft;
20+
private DcMotor backRight;
21+
22+
// Servos
23+
private Servo claw;
24+
25+
// Constants
26+
private static final int TARGET_FLYWHEEL = 6000;
27+
private static final int RAMP_RATE = 900;
28+
private static final double CLAW_CLOSED = 1.0;
29+
private static final double CLAW_OPEN = 0.25;
30+
private static final double LODEWHEEL_SPEED = 8;
31+
32+
@Override
33+
public void runOpMode() throws InterruptedException {
34+
// Initialize hardware
35+
initializeHardware();
36+
37+
telemetry.addData("Status", "Initialized");
38+
telemetry.addData("Ready", "Waiting for start");
39+
telemetry.update();
40+
41+
waitForStart();
42+
43+
if (opModeIsActive()) {
44+
// Execute autonomous sequence
45+
autonomousSequence();
46+
}
47+
}
48+
49+
private void initializeHardware() {
50+
// Map flywheel motors
51+
flywheel = hardwareMap.get(DcMotorEx.class, "flywheel");
52+
flywheel2 = hardwareMap.get(DcMotorEx.class, "flywheel2");
53+
lodewheel = hardwareMap.get(DcMotorEx.class, "lode");
54+
55+
flywheel.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
56+
flywheel2.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
57+
lodewheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
58+
59+
flywheel.setDirection(DcMotorEx.Direction.FORWARD);
60+
flywheel2.setDirection(DcMotorEx.Direction.REVERSE);
61+
62+
// Initialize servos
63+
claw = hardwareMap.get(Servo.class, "claw");
64+
// claw.setPosition(CLAW_OPEN);
65+
66+
// Initialize drive motors - adjust these names to match your robot configuration
67+
frontLeft = hardwareMap.get(DcMotor.class, "fl");
68+
frontRight = hardwareMap.get(DcMotor.class, "fr");
69+
backLeft = hardwareMap.get(DcMotor.class, "bl");
70+
backRight = hardwareMap.get(DcMotor.class, "br");
71+
72+
// Set motor directions for mecanum drive
73+
frontLeft.setDirection(DcMotor.Direction.FORWARD);
74+
backLeft.setDirection(DcMotor.Direction.FORWARD);
75+
frontRight.setDirection(DcMotor.Direction.REVERSE);
76+
backRight.setDirection(DcMotor.Direction.REVERSE);
77+
78+
// Set zero power behavior
79+
frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
80+
frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
81+
backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
82+
backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
83+
}
84+
85+
private void autonomousSequence() throws InterruptedException {
86+
// Example autonomous sequence - customize as needed
87+
88+
// Step 1: Close claw to grab preloaded game element
89+
90+
91+
// Step 2: Drive forward to position
92+
telemetry.addData("Step", "2: Driving forward");
93+
telemetry.update();
94+
driveDistance(-1, 0, 0, 1500);
95+
//-forwers +back weras
96+
telemetry.addData("Step", "2: Driving forward");
97+
telemetry.update();
98+
driveDistance(0, 0, -1, 250);
99+
//+ to the rigth - to the lefft
100+
// Step 3: Spin up flywheels
101+
telemetry.addData("Step", "3: Spinning up flywheels");
102+
telemetry.update();
103+
rampFlywheels(TARGET_FLYWHEEL, 3000);
104+
105+
106+
telemetry.addData("Step", "1: Closing claw");
107+
telemetry.update();
108+
setClaw(CLAW_OPEN);
109+
sleep(500);
110+
// Step 4: Run lodewheel to push ball into flywheels and launch
111+
// telemetry.addData("Step", "4: Pushing ball and launching");
112+
// telemetry.update();
113+
// lodewheel.setVelocity(LODEWHEEL_SPEED);
114+
// sleep(2000);
115+
// lodewheel.setVelocity(0);
116+
117+
// Step 5: Spin down flywheels
118+
telemetry.addData("Step", "5: Spinning down");
119+
telemetry.update();
120+
rampFlywheels(0, 2000);
121+
122+
// Step 6: Strafe right
123+
// telemetry.addData("Step", "6: Strafing right");
124+
// telemetry.update();
125+
// driveDistance(0, 0.5, 0, 1000);
126+
127+
// Step 7: Open claw
128+
telemetry.addData("Step", "7: Opening claw");
129+
telemetry.update();
130+
setClaw(CLAW_OPEN);
131+
sleep(500);
132+
133+
// Step 8: Drive backward
134+
// telemetry.addData("Step", "8: Driving backward");
135+
// telemetry.update();
136+
// driveDistance(-0.3, 0, 0, 800);
137+
138+
telemetry.addData("Status", "Autonomous Complete");
139+
telemetry.update();
140+
}
141+
142+
private void driveDistance(double forward, double strafe, double turn, long milliseconds) {
143+
// Mecanum drive calculations
144+
double frontLeftPower = forward + strafe + turn;
145+
double frontRightPower = forward - strafe - turn;
146+
double backLeftPower = forward - strafe + turn;
147+
double backRightPower = forward + strafe - turn;
148+
149+
// Normalize powers if any exceed 1.0
150+
double maxPower = Math.max(
151+
Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)),
152+
Math.max(Math.abs(backLeftPower), Math.abs(backRightPower))
153+
);
154+
155+
if (maxPower > 1.0) {
156+
frontLeftPower /= maxPower;
157+
frontRightPower /= maxPower;
158+
backLeftPower /= maxPower;
159+
backRightPower /= maxPower;
160+
}
161+
162+
// Set motor powers
163+
frontLeft.setPower(frontLeftPower);
164+
frontRight.setPower(frontRightPower);
165+
backLeft.setPower(backLeftPower);
166+
backRight.setPower(backRightPower);
167+
168+
sleep(milliseconds);
169+
170+
// Stop motors
171+
frontLeft.setPower(0);
172+
frontRight.setPower(0);
173+
backLeft.setPower(0);
174+
backRight.setPower(0);
175+
}
176+
177+
private void rampFlywheels(int targetVelocity, long rampTimeMs) {
178+
long startTime = System.currentTimeMillis();
179+
double currentVelocity = flywheel.getVelocity();
180+
181+
while (System.currentTimeMillis() - startTime < rampTimeMs && opModeIsActive()) {
182+
double progress = (double)(System.currentTimeMillis() - startTime) / rampTimeMs;
183+
double velocity = currentVelocity + (targetVelocity - currentVelocity) * progress;
184+
185+
flywheel.setVelocity(velocity);
186+
flywheel2.setVelocity(velocity);
187+
188+
telemetry.addData("Flywheel Target", targetVelocity);
189+
telemetry.addData("Flywheel Current", velocity);
190+
telemetry.addData("Flywheel1 Actual", flywheel.getVelocity());
191+
telemetry.addData("Flywheel2 Actual", flywheel2.getVelocity());
192+
telemetry.update();
193+
194+
sleep(50);
195+
}
196+
197+
flywheel.setVelocity(targetVelocity);
198+
flywheel2.setVelocity(targetVelocity);
199+
}
200+
201+
private void setClaw(double position) {
202+
claw.setPosition(position);
203+
}
204+
}

0 commit comments

Comments
 (0)