Skip to content

Commit ecdd2f3

Browse files
Rand0mAsianKidRand0mAsianKid
committed
Intakes using gyro and math
1 parent 9eee946 commit ecdd2f3

File tree

3 files changed

+320
-0
lines changed

3 files changed

+320
-0
lines changed

‎src/main/java/org/carlmontrobotics/RobotContainer.java‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -382,6 +382,11 @@ private void registerAutoCommands() {
382382
NamedCommands.registerCommand("StopBoth",
383383
new ParallelCommandGroup(new InstantCommand(intakeShooter::stopIntake),
384384
new InstantCommand(intakeShooter::stopOuttake)));
385+
386+
NamedCommands.registerCommand("Auto Intake",
387+
new SequentialCommandGroup(
388+
new AlignToNoteMath(drivetrain, intakeShooter),
389+
new DriveAndIntake(intakeShooter, drivetrain)));
385390
}
386391

387392
private void setupAutos() {

‎src/main/java/org/carlmontrobotics/commands/AlignToNoteMath.java‎

Lines changed: 237 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,237 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package org.carlmontrobotics.commands;
6+
7+
import java.util.Optional;
8+
9+
import org.carlmontrobotics.subsystems.Drivetrain;
10+
import org.carlmontrobotics.subsystems.IntakeShooter;
11+
import org.carlmontrobotics.commands.Intake;
12+
13+
14+
import edu.wpi.first.math.geometry.Rotation2d;
15+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
16+
import edu.wpi.first.wpilibj2.command.InstantCommand;
17+
import edu.wpi.first.wpilibj2.command.ProxyCommand;
18+
import edu.wpi.first.wpilibj.DriverStation;
19+
20+
/** Add your docs here. */
21+
public class AlignToNoteMath extends ProxyCommand {
22+
static double rednote1X = 13.67;
23+
static double rednote1Y = 6.99;
24+
25+
static double rednote2X = 13.67;
26+
static double rednote2Y = 5.51;
27+
28+
static double rednote3X = 13.67;
29+
static double rednote3Y = 4.10;
30+
31+
32+
static double bluenote1X = 2.90;
33+
static double bluenote1Y = 6.99;
34+
35+
static double bluenote2X = 2.90;
36+
static double bluenote2Y = 5.51;
37+
38+
static double bluenote3X = 2.90;
39+
static double bluenote3Y = 4.10;
40+
41+
42+
static double note4X = 8.28;
43+
static double note4Y = 7.43;
44+
45+
static double note5X = 8.28;
46+
static double note5Y = 5.77;
47+
48+
static double note6X = 8.28;
49+
static double note6Y = 4.11;
50+
51+
static double note7X = 8.28;
52+
static double note7Y = 2.43;
53+
54+
static double note8X = 8.28;
55+
static double note8Y = 0.77;
56+
57+
public AlignToNoteMath(Drivetrain dt, IntakeShooter intakeShooter) {
58+
59+
super(() -> {
60+
Optional<Alliance> allianceSide = DriverStation.getAlliance();
61+
62+
double redNote1distance =
63+
Math.hypot(dt.getPose().getX() - rednote1X,
64+
dt.getPose().getY() - rednote1Y);
65+
double redNote2distance =
66+
Math.hypot(dt.getPose().getX() - rednote2X,
67+
dt.getPose().getY() - rednote2Y);
68+
double redNote3distance =
69+
Math.hypot(dt.getPose().getX() - rednote3X,
70+
dt.getPose().getY() - rednote3Y);
71+
72+
double blueNote1distance =
73+
Math.hypot(dt.getPose().getX() - bluenote1X,
74+
dt.getPose().getY() - bluenote1Y);
75+
double blueNote2distance =
76+
Math.hypot(dt.getPose().getX() - bluenote2X,
77+
dt.getPose().getY() - bluenote2Y);
78+
double blueNote3distance =
79+
Math.hypot(dt.getPose().getX() - bluenote3X,
80+
dt.getPose().getY() - bluenote3Y);
81+
82+
double Note4distance = Math.hypot(dt.getPose().getX() - note4X,
83+
dt.getPose().getY() - note4Y);
84+
double Note5distance = Math.hypot(dt.getPose().getX() - note5X,
85+
dt.getPose().getY() - note5Y);
86+
double Note6distance = Math.hypot(dt.getPose().getX() - note6X,
87+
dt.getPose().getY() - note6Y);
88+
double Note7distance = Math.hypot(dt.getPose().getX() - note7X,
89+
dt.getPose().getY() - note7Y);
90+
double Note8distance = Math.hypot(dt.getPose().getX() - note8X,
91+
dt.getPose().getY() - note8Y);
92+
93+
94+
95+
double redNote1distance1 = Math.abs(redNote1distance);
96+
double redNote2distance2 = Math.abs(redNote2distance);
97+
double redNote3distance3 = Math.abs(redNote3distance);
98+
99+
double blueNote1distance1 = Math.abs(blueNote1distance);
100+
double blueNote2distance2 = Math.abs(blueNote2distance);
101+
double blueNote3distance3 = Math.abs(blueNote3distance);
102+
103+
double Note4distance4 = Math.abs(Note4distance);
104+
double Note5distance5 = Math.abs(Note5distance);
105+
double Note6distance6 = Math.abs(Note6distance);
106+
double Note7distance7 = Math.abs(Note7distance);
107+
double Note8distance8 = Math.abs(Note8distance);
108+
109+
110+
111+
// calculate the angle needed to align with note
112+
// after that, create a command that drives the drivetrain forward
113+
// once distance sensors detect a note, stop rollers and drivetrain
114+
// use a timer to stop the drivetrain just in case a note is not intaked
115+
// TODO: DONE RED
116+
if (redNote1distance1 < 2.0) {
117+
return new RotateToFieldRelativeAngle(
118+
new Rotation2d(dt.getPose().getX(),
119+
rednote1Y - dt.getPose().getY()),
120+
dt);
121+
}
122+
if (redNote2distance2 < 2.0) {
123+
return new RotateToFieldRelativeAngle(
124+
new Rotation2d(dt.getPose().getX(),
125+
rednote2Y - dt.getPose().getY()),
126+
dt);
127+
}
128+
if (redNote3distance3 < 2.0) {
129+
return new RotateToFieldRelativeAngle(
130+
new Rotation2d(dt.getPose().getX(),
131+
rednote3Y - dt.getPose().getY()),
132+
dt);
133+
}
134+
// TODO: DONE RED
135+
136+
137+
138+
// TODO: DONE BLUE
139+
if (blueNote1distance1 < 2.0) {
140+
return new RotateToFieldRelativeAngle(
141+
new Rotation2d(bluenote1X - dt.getPose().getX(),
142+
bluenote1Y - dt.getPose().getY()),
143+
dt);
144+
}
145+
if (blueNote2distance2 < 2.0) {
146+
return new RotateToFieldRelativeAngle(
147+
new Rotation2d(bluenote2X - dt.getPose().getX(),
148+
bluenote2Y - dt.getPose().getY()),
149+
dt);
150+
}
151+
if (blueNote3distance3 < 2.0) {
152+
return new RotateToFieldRelativeAngle(
153+
new Rotation2d(bluenote3X - dt.getPose().getX(),
154+
bluenote3Y - dt.getPose().getY()),
155+
dt);
156+
}
157+
// TODO: DONE BLUE
158+
159+
160+
161+
// TODO: DONE BLUE MIDDLE
162+
if (Note4distance4 < 2.0 && allianceSide.get() == Alliance.Red) {
163+
return new RotateToFieldRelativeAngle(new Rotation2d(
164+
dt.getPose().getX(), note4Y - dt.getPose().getY()), dt);
165+
}
166+
if (Note5distance5 < 2.0 && allianceSide.get() == Alliance.Red) {
167+
return new RotateToFieldRelativeAngle(new Rotation2d(
168+
dt.getPose().getX(), note5Y - dt.getPose().getY()), dt);
169+
}
170+
if (Note6distance6 < 2.0 && allianceSide.get() == Alliance.Red) {
171+
return new RotateToFieldRelativeAngle(new Rotation2d(
172+
dt.getPose().getX(), note6Y - dt.getPose().getY()), dt);
173+
}
174+
if (Note7distance7 < 2.0 && allianceSide.get() == Alliance.Red) {
175+
return new RotateToFieldRelativeAngle(new Rotation2d(
176+
dt.getPose().getX(), note7Y - dt.getPose().getY()), dt);
177+
}
178+
if (Note8distance8 < 2.0 && allianceSide.get() == Alliance.Red) {
179+
return new RotateToFieldRelativeAngle(new Rotation2d(
180+
dt.getPose().getX(), note8Y - dt.getPose().getY()), dt);
181+
}
182+
// TODO: DONE BLUE MIDDLE
183+
184+
185+
186+
if (Note4distance4 < 2.0 && allianceSide.get() == Alliance.Blue) {
187+
return new RotateToFieldRelativeAngle(
188+
new Rotation2d(note4X - dt.getPose().getX(),
189+
note4Y - dt.getPose().getY()),
190+
dt);
191+
}
192+
if (Note5distance5 < 2.0 && allianceSide.get() == Alliance.Blue) {
193+
return new RotateToFieldRelativeAngle(
194+
new Rotation2d(note5X - dt.getPose().getX(),
195+
note5Y - dt.getPose().getY()),
196+
dt);
197+
}
198+
if (Note6distance6 < 2.0 && allianceSide.get() == Alliance.Blue) {
199+
return new RotateToFieldRelativeAngle(
200+
new Rotation2d(note6X - dt.getPose().getX(),
201+
note6Y - dt.getPose().getY()),
202+
dt);
203+
}
204+
if (Note7distance7 < 2.0 && allianceSide.get() == Alliance.Blue) {
205+
return new RotateToFieldRelativeAngle(
206+
new Rotation2d(note7X - dt.getPose().getX(),
207+
note7Y - dt.getPose().getY()),
208+
dt);
209+
}
210+
if (Note8distance8 < 2.0 && allianceSide.get() == Alliance.Blue) {
211+
return new RotateToFieldRelativeAngle(
212+
new Rotation2d(note8X - dt.getPose().getX(),
213+
note8Y - dt.getPose().getY()),
214+
dt);
215+
}
216+
// return new RotateToFieldRelativeAngle(new Rotation2d(dt.getPose().getX(),
217+
// blueSpeakerY - dt.getPose().getY()), dt);
218+
// get pos and if it is in a something area, then do some tan^-1 math to align it, then
219+
// drive forward
220+
221+
/*
222+
* Optional<Alliance> allianceSide = DriverStation.getAlliance(); if (allianceSide.get()
223+
* == Alliance.Red) { // double redAngle = Math.atan2(redSpeakerY-dt.getPose().getY(),
224+
* // dt.getPose().getX()); return new RotateToFieldRelativeAngle( new
225+
* Rotation2d(dt.getPose().getX(), blueSpeakerY - dt.getPose().getY()), dt);
226+
*
227+
* } else if (allianceSide.get() == Alliance.Blue) { // double blueAngle =
228+
* Math.atan2(blueSpeakerY-dt.getPose().getY(), // dt.getPose().getX()); return new
229+
* RotateToFieldRelativeAngle( new Rotation2d(redSpeakerX - dt.getPose().getX(),
230+
* redSpeakerY - dt.getPose().getY()), dt);
231+
*
232+
* } // create an if statement based on alliance side
233+
*/
234+
return new InstantCommand();
235+
});
236+
}
237+
}

‎src/main/java/org/carlmontrobotics/commands/DriveAndIntake.java‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
package org.carlmontrobotics.commands;
2+
3+
import static org.carlmontrobotics.Constants.Effectorc.*;
4+
5+
import org.carlmontrobotics.subsystems.Drivetrain;
6+
import org.carlmontrobotics.subsystems.IntakeShooter;
7+
8+
import edu.wpi.first.wpilibj.Timer;
9+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
10+
import edu.wpi.first.wpilibj2.command.Command;
11+
12+
public class DriveAndIntake extends Command {
13+
// intake until sees game peice or 4sec has passed
14+
private final IntakeShooter intake;
15+
private Drivetrain Dt;
16+
private int index = 0;
17+
private double increaseSpeed = .007;
18+
private double initSpeed = .5;
19+
private double slowSpeed = .1;
20+
21+
public DriveAndIntake(IntakeShooter intake, Drivetrain Dt) {
22+
addRequirements(this.intake = intake);
23+
SmartDashboard.putNumber("Initial intake speed", initSpeed);
24+
SmartDashboard.putNumber("Slow intake speed", slowSpeed);
25+
SmartDashboard.putNumber("Increase speed", increaseSpeed);
26+
}
27+
// .-18
28+
29+
@Override
30+
public void initialize() {
31+
// TODO: Adjust speed or add in an index;
32+
// if (intake.intakeDetectsNote()) {
33+
// return;
34+
// }
35+
Dt.drive(0.3, 0, 0);
36+
37+
initSpeed = SmartDashboard.getNumber("Initial intake speed", 0);
38+
intake.motorSetIntake(initSpeed); // Fast intake speed
39+
// for initial
40+
// intake
41+
intake.resetCurrentLimit();
42+
index = 0;
43+
}
44+
45+
46+
// Called every time the scheduler runs while the command is scheduled.
47+
@Override
48+
public void execute() {
49+
// Intake Led
50+
increaseSpeed = SmartDashboard.getNumber("Increase speed", 0);
51+
slowSpeed = SmartDashboard.getNumber("Slow intake speed", 0);
52+
if ((intake.intakeDetectsNote())) {
53+
double intakeSpeed = slowSpeed + index * increaseSpeed;
54+
SmartDashboard.putNumber("Intake-current-speed", intakeSpeed);
55+
intake.motorSetIntake(intakeSpeed); // Slower intake
56+
// speed triggered
57+
// after intake ds
58+
// sees note
59+
++index;
60+
}
61+
}
62+
63+
// Called once the command ends or is interrupted.
64+
@Override
65+
public void end(boolean interrupted) {
66+
intake.stopIntake();
67+
Dt.drive(0, 0, 0);
68+
// intake.resetCurrentLimit();
69+
}
70+
71+
// Returns true when the command should end.
72+
@Override
73+
public boolean isFinished() {
74+
// return intake.intakeDetectsNote() && timer.get()>0.1;
75+
// || //timer.hasElapsed(MAX_SECONDS_OVERLOAD);
76+
return intake.outtakeDetectsNote();
77+
}
78+
}

0 commit comments

Comments
 (0)