Compare commits
10 Commits
eee5384f53
...
cbb95f40bd
Author | SHA1 | Date | |
---|---|---|---|
|
cbb95f40bd | ||
|
c390e47e5b | ||
|
41e0e47322 | ||
|
f9d9e4fe34 | ||
|
bb4bee0d96 | ||
|
e24e36486b | ||
|
752b8f440d | ||
|
b5ab5fa4bf | ||
|
a87c10beb5 | ||
|
c63f23a302 |
@ -1,108 +0,0 @@
|
||||
{
|
||||
"version": 1.0,
|
||||
"startingPose": {
|
||||
"position": {
|
||||
"x": 1.3,
|
||||
"y": 5.55
|
||||
},
|
||||
"rotation": 0
|
||||
},
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "lancer"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 0.5
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "deadline",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 3.25
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "10 avril reculer"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "balayer"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 0.5
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "10 avril avancer"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 0.2
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "lancer"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
@ -1,82 +0,0 @@
|
||||
{
|
||||
"version": 1.0,
|
||||
"startingPose": {
|
||||
"position": {
|
||||
"x": 1.27,
|
||||
"y": 5.566417514601411
|
||||
},
|
||||
"rotation": 0
|
||||
},
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "balayer"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "deadline",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 3.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "lancer"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 3.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "3 avril1"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "3 avril2"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
@ -1,58 +0,0 @@
|
||||
{
|
||||
"version": 1.0,
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.2,
|
||||
"y": 5.55
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.2,
|
||||
"y": 5.55
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.35,
|
||||
"y": 5.55
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.3500000000000001,
|
||||
"y": 5.55
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 0.5,
|
||||
"rotationDegrees": 0,
|
||||
"rotateFast": true
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.75,
|
||||
"maxAcceleration": 0.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0,
|
||||
"rotateFast": false
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"previewStartingState": {
|
||||
"rotation": 0,
|
||||
"velocity": 0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
@ -1,58 +0,0 @@
|
||||
{
|
||||
"version": 1.0,
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.3479282065452507,
|
||||
"y": 5.547612425391122
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.3479282065452507,
|
||||
"y": 5.547612425391122
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.20038226774791,
|
||||
"y": 5.547612425391122
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.20038226774791,
|
||||
"y": 5.547612425391122
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 0.8,
|
||||
"rotationDegrees": 0,
|
||||
"rotateFast": false
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.25,
|
||||
"maxAcceleration": 0.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0,
|
||||
"rotateFast": false
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"previewStartingState": {
|
||||
"rotation": 0,
|
||||
"velocity": 0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
@ -1,52 +0,0 @@
|
||||
{
|
||||
"version": 1.0,
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.27,
|
||||
"y": 5.566417514601411
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.2933883088848783,
|
||||
"y": 5.566417514601411
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.27,
|
||||
"y": 5.566417514601411
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.25830584555756,
|
||||
"y": 5.566417514601411
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 2.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0.0,
|
||||
"rotation": 0.0,
|
||||
"rotateFast": false
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"previewStartingState": {
|
||||
"rotation": 0,
|
||||
"velocity": 0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
@ -1,52 +0,0 @@
|
||||
{
|
||||
"version": 1.0,
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.265158439148787,
|
||||
"y": 5.57
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.3004233428739336,
|
||||
"y": 5.57
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1,
|
||||
"y": 5.57
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.090703297854801,
|
||||
"y": 5.57
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 2.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0,
|
||||
"rotateFast": false
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"previewStartingState": {
|
||||
"rotation": -0.3989133239630231,
|
||||
"velocity": 0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
@ -1,52 +0,0 @@
|
||||
{
|
||||
"version": 1.0,
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.2654580294915574,
|
||||
"y": 2.9352327650524255
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.2654580294915574,
|
||||
"y": 2.9352327650524255
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.276852593591227,
|
||||
"y": 2.9352327650524255
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.276852593591227,
|
||||
"y": 2.9352327650524255
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 2.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0,
|
||||
"rotateFast": false
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"previewStartingState": {
|
||||
"rotation": 0,
|
||||
"velocity": 0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
@ -14,10 +14,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
// Manettes
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.command.AvancerAuto;
|
||||
// Commands
|
||||
import frc.robot.command.Balayer;
|
||||
import frc.robot.command.GrimpeurDroit;
|
||||
@ -68,7 +69,7 @@ public class RobotContainer {
|
||||
GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftY);
|
||||
GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getRightY);
|
||||
Debalayer debalayer = new Debalayer(balayeuse, accumulateur);
|
||||
|
||||
AvancerAuto avancerAuto = new AvancerAuto(drive);
|
||||
public RobotContainer() {
|
||||
|
||||
dashboard.addCamera("limelight", "limelight","limelight.local:5800")
|
||||
@ -89,7 +90,7 @@ public class RobotContainer {
|
||||
manette.start().whileTrue(new RestGyro(drive));
|
||||
manette.a().whileTrue(new GuiderBas(guideur));
|
||||
manette.b().whileTrue(new GuiderHaut(guideur));
|
||||
|
||||
manette.x().whileTrue(avancerAuto);
|
||||
//joystick
|
||||
joystick.button(3).whileTrue(new Balayer(balayeuse, accumulateur));
|
||||
manette1.rightBumper().whileTrue(new LancerNote(lanceur, accumulateur));
|
||||
@ -115,6 +116,6 @@ public class RobotContainer {
|
||||
private void configureBindings() {}
|
||||
|
||||
public Command getAutonomousCommand(){
|
||||
return autoChooser.getSelected();
|
||||
return autoChooser.getSelected(); //new SequentialCommandGroup(lancer.withTimeout(2));
|
||||
}
|
||||
}
|
||||
|
40
src/main/java/frc/robot/command/AvancerAuto.java
Normal file
40
src/main/java/frc/robot/command/AvancerAuto.java
Normal file
@ -0,0 +1,40 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot.command;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc.robot.subsystem.Drive;
|
||||
|
||||
public class AvancerAuto extends Command {
|
||||
private Drive drive;
|
||||
/** Creates a new AvancerAuto. */
|
||||
public AvancerAuto(Drive drive) {
|
||||
this.drive = drive;
|
||||
addRequirements(drive);
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
drive.drive(0.5, 0, 0);
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
drive.drive(0, 0, 0);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
@ -58,9 +58,7 @@ public class Drive extends SubsystemBase {
|
||||
public SwerveModulePosition[] distance(){
|
||||
return swerveDrive.getModulePositions();
|
||||
}
|
||||
public void reset(){
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
Loading…
x
Reference in New Issue
Block a user