Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-08 16:28:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'auto-programming' of https://github.com/Team4388/RiseOfRidgebotics2020 into auto-programming

Browse Source
This commit is contained in:
Aarav Shah
2021-02-11 17:53:07 -07:00
parent 060086c6bb c5a34f3708
commit eadc601ce9
16 changed files with 103 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
PathWeaver/Groups/FigureEight
+1
View File
@@ -0,0 +1 @@
FigureEight3
PathWeaver/Paths/DriveOffLineForward
+1 -1
View File
@@ -1,3 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
3.2,-2.3,0.5,0.0,true,
3.7,-2.3,0.5,0.0,true,
4.926159376619506,-2.306021520328549,0.5,0.0,true,
PathWeaver/Paths/FigureEight
+6
View File
@@ -0,0 +1,6 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
8.144025699763882,-4.085312310773085,0.0567858762907818,3.1232231959930674,true,
10.661532881988599,-4.142098187063867,-0.0567858762907818,-2.744650687387848,true,
8.162954325194141,-4.085312310773085,0.13250037801182657,2.915008316260198,true,
5.853662022702296,-4.142098187063867,0.0,-2.9717941925509805,true,
8.162954325194141,-4.142098187063867,0.0,2.650007560236543,true,
PathWeaver/Paths/FigureEight3
+3
View File
@@ -0,0 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
0.0,0.0,10.0,0.0,true,
10.0,-10.0,0.0,-10.0,true,
PathWeaver/pathweaver.json
-1
View File
@@ -1,6 +1,5 @@
{
"lengthUnit": "Meter",
"exportUnit": "Always Meters",
"maxVelocity": 1.7,
"maxAcceleration": 2.7,
"wheelBase": 0.648,
src/main/deploy/paths/DriveOffLineForward.wpilib.json
+1 -1
View File
@@ -1 +1 @@
[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.24673072577541286,"velocity":0.6661729595936147,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2821826120636555,"y":-2.300096658926172},"rotation":{"radians":-0.0022988506389506674}},"curvature":-0.017838043885837547},{"time":0.33725349511063807,"velocity":0.9105844367987228,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.353548563195771,"y":-2.300293663507997},"rotation":{"radians":-0.0030970284585935958}},"curvature":-0.0069332281418155},{"time":0.43198725573191693,"velocity":1.1663655904761758,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.4519266542203786,"y":-2.3006233214402596},"rotation":{"radians":-0.0035415696301389923}},"curvature":-0.0029369290271881927},{"time":0.5284273014978229,"velocity":1.4267537140441218,"acceleration":2.7,"pose":{"translation":{"x":3.576966078125889,"y":-2.3010839099521765},"rotation":{"radians":-0.0037936885562888066}},"curvature":-0.0013707072882595045},{"time":0.5763438583279685,"velocity":1.556128417485515,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.6484302726022984,"y":-2.3013581294794916},"rotation":{"radians":-0.0038756571162190407}},"curvature":-9.542625124576716E-4},{"time":0.6235805335204395,"velocity":1.6836674405051866,"acceleration":0.3371724938324549,"pose":{"translation":{"x":3.7249482805894383,"y":-2.3016571676734268},"rotation":{"radians":-0.003936728545992743}},"curvature":-6.608939907291656E-4},{"time":0.6712952623779239,"velocity":1.699755534626604,"acceleration":0.0018965134247352152,"pose":{"translation":{"x":3.805667202540378,"y":-2.3019768360475115},"rotation":{"radians":-0.0039808512652144586}},"curvature":-4.439007504459168E-4},{"time":0.7206906310513733,"velocity":1.699849213606413,"acceleration":0.0015545280583909033,"pose":{"translation":{"x":3.8896288968047106,"y":-2.302312429284039},"rotation":{"radians":-0.004010692104327395}},"curvature":-2.737831684939188E-4},{"time":0.771381063968029,"velocity":1.6999280133066739,"acceleration":0.001402028284918414,"pose":{"translation":{"x":3.9757962901544204,"y":-2.302658854441877},"rotation":{"radians":-0.0040279621163264315}},"curvature":-1.3070032995605277E-4},{"time":0.8227257437439109,"velocity":1.6999999999999997,"acceleration":-0.001402028284905696,"pose":{"translation":{"x":4.063079688309753,"y":-2.3030107601642746},"rotation":{"radians":-0.0040336164409507415}},"curvature":-5.668115247084055E-16},{"time":0.8740704235197836,"velocity":1.6999280133066745,"acceleration":-0.0015545280583857497,"pose":{"translation":{"x":4.150363086465086,"y":-2.303362665886672},"rotation":{"radians":-0.004027962116326537}},"curvature":1.3070032995489233E-4},{"time":0.9247608564364644,"velocity":1.6998492136064138,"acceleration":-0.0018965134247457959,"pose":{"translation":{"x":4.236530479814796,"y":-2.3037090910445097},"rotation":{"radians":-0.0040106921043276}},"curvature":2.7378316849265206E-4},{"time":0.9741562251098724,"velocity":1.6997555346266044,"acceleration":-0.33717249383246317,"pose":{"translation":{"x":4.320492174079129,"y":-2.3040446842810374},"rotation":{"radians":-0.003980851265214779}},"curvature":4.4390075044446666E-4},{"time":1.021870953967357,"velocity":1.6836674405051866,"acceleration":-2.699999999999997,"pose":{"translation":{"x":4.401211096030068,"y":-2.304364352655122},"rotation":{"radians":-0.003936728545993193}},"curvature":6.608939907274078E-4},{"time":1.069107629159828,"velocity":1.556128417485515,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":4.477729104017208,"y":-2.3046633908490572},"rotation":{"radians":-0.00387565711621964}},"curvature":9.542625124554455E-4},{"time":1.1170241859899734,"velocity":1.4267537140441227,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":4.549193298493617,"y":-2.3049376103763723},"rotation":{"radians":-0.0037936885562895833}},"curvature":0.001370707288256562},{"time":1.2134642317558797,"velocity":1.166365590476176,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":4.674232722399128,"y":-2.3053981988882892},"rotation":{"radians":-0.003541569630140284}},"curvature":0.0029369290271822348},{"time":1.3081979923771587,"velocity":0.9105844367987229,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":4.772610813423736,"y":-2.305727856820553},"rotation":{"radians":-0.003097028458595807}},"curvature":0.006933228141801077},{"time":1.3987207617123838,"velocity":0.6661729595936149,"acceleration":-2.7,"pose":{"translation":{"x":4.843976764555851,"y":-2.3059248614023775},"rotation":{"radians":-0.0022988506389546087}},"curvature":0.017838043885796937},{"time":1.6454514874877968,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":4.926159376619506,"y":-2.3060215203285503},"rotation":{"radians":-1.0658141036401503E-14}},"curvature":-5.6843418860808015E-14}]
src/main/deploy/paths/FigureEight.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/FigureEight3.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/java/frc4388/robot/Robot.java
+3 -2
View File
@@ -7,6 +7,7 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.TimedRobot;
@@ -71,7 +72,8 @@ public class Robot extends TimedRobot {
@Override
public void disabledPeriodic() {
}
m_robotContainer.resetOdometry(new Pose2d());
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
@@ -83,7 +85,6 @@ public class Robot extends TimedRobot {
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(true);
m_robotContainer.resetOdometry(new Pose2d());
//m_robotContainer.resetGyroYawRobotContainer(0);
src/main/java/frc4388/robot/RobotContainer.java
+42 -4
View File
@@ -28,15 +28,19 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.commands.auto.DriveOffLineBackward;
import frc4388.robot.commands.auto.DriveOffLineForward;
import frc4388.robot.commands.auto.EightBallAutoMiddle;
import frc4388.robot.commands.auto.FigureEight;
import frc4388.robot.commands.auto.FiveBallAutoMiddle;
import frc4388.robot.commands.auto.SixBallAutoMiddle;
import frc4388.robot.commands.auto.Slalom;
@@ -52,6 +56,7 @@ import frc4388.robot.commands.drive.DriveStraightAtVelocityPID;
import frc4388.robot.commands.drive.DriveStraightToPositionMM;
import frc4388.robot.commands.drive.DriveWithJoystick;
import frc4388.robot.commands.drive.TurnDegrees;
import frc4388.robot.commands.intake.RunIntake;
import frc4388.robot.commands.intake.RunIntakeWithTriggers;
import frc4388.robot.commands.shooter.CalibrateShooter;
import frc4388.robot.commands.shooter.TrackTarget;
@@ -111,6 +116,12 @@ public class RobotContainer {
SixBallAutoMiddle m_sixBallAutoMiddle;
SixBallAutoMiddle m_sixBallAuto0;
SixBallAutoMiddle m_sixBallAuto1;
FigureEight m_figureEight;
EightBallAutoMiddle m_eightBallAutoMiddle;
DriveOffLineForward m_driveOffLineForward;
@@ -164,8 +175,8 @@ public class RobotContainer {
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// runs the storage not
//m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
//m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
//m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
}
@@ -233,7 +244,7 @@ public class RobotContainer {
.whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
.whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
// safety for climber and leveler
// safety for climber and leveler
new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
.whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
.whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
@@ -294,6 +305,24 @@ public class RobotContainer {
m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
String[] sixBallAutoMiddle0 = new String[]{
"SixBallMid0"
};
m_sixBallAuto0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0));
String[] sixBallAutoMiddle1 = new String[]{
"SixBallMid1"
};
m_sixBallAuto1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1));
String[] figureEight = new String[]{
"FigureEight"
};
m_figureEight = new FigureEight(m_robotDrive, buildPaths(figureEight));
String[] eightBallAutoMiddlePaths = new String[]{
"EightBallMidComplete"
};
@@ -348,7 +377,16 @@ public class RobotContainer {
//return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
// return m_figureEight.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
return new SequentialCommandGroup(
m_sixBallAuto0,
new InstantCommand(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d()))),
m_sixBallAuto1
// /**new ParallelRaceGroup(
// m_sixBallAuto0,
// new RunIntake(m_robotIntake)
// )**/); //ParallelRaceGroup(m_sixBallAuto0, new RunIntake(m_robotIntake));
);
} catch (Exception e) {
System.err.println("ERROR");
}
src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java
+5 -1
View File
@@ -9,7 +9,9 @@ package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.intake.RunIntake;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.subsystems.Intake;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
@@ -21,9 +23,11 @@ public class DriveOffLineForward extends SequentialCommandGroup {
public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
Intake m_intake = new Intake();
addCommands(
paths[0]
paths[0],
new RunIntake(m_intake)
);
}
}
src/main/java/frc4388/robot/commands/auto/FigureEight.java
+28
View File
@@ -0,0 +1,28 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class FigureEight extends SequentialCommandGroup {
/**
* Creates a new FigureEight.
*/
public FigureEight(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0]
);
}
}
src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
+7 -1
View File
@@ -12,10 +12,14 @@ import java.nio.file.Path;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
import frc4388.robot.subsystems.Drive;
import frc4388.robot.RobotContainer;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
@@ -27,9 +31,11 @@ public class SixBallAutoMiddle extends SequentialCommandGroup {
public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
RobotContainer m_robotContainer = new RobotContainer();
addCommands(
paths[0]
paths[0],
new InstantCommand(() -> m_robotContainer.resetOdometry(new Pose2d(0, 0, new Rotation2d())))
);
}
}
src/main/java/frc4388/robot/commands/auto/Wait.java
+1
View File
@@ -8,6 +8,7 @@
package frc4388.robot.commands.auto;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Wait extends CommandBase {
src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java
+2 -2
View File
@@ -18,8 +18,8 @@ import frc4388.robot.subsystems.Intake;
public class RunExtenderOutIn extends CommandBase {
private Intake m_intake;
CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
//CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
//CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
CANDigitalInput m_extenderForwardLimit;
CANDigitalInput m_extenderReverseLimit;
src/main/java/frc4388/robot/subsystems/Drive.java
+1 -2
View File
@@ -833,8 +833,7 @@ public class Drive extends SubsystemBase {
SmartDashboard.putData("Pigeon Gyro", m_pigeonGyro);
SmartDashboard.putData("Drive Train", m_driveTrain);
//SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
//SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
//SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());