Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
1e962b904f | ||
|
|
1297645917 |
@ -1,5 +0,0 @@
|
|||||||
{
|
|
||||||
"Transitory Values": {
|
|
||||||
"open": false
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1 +0,0 @@
|
|||||||
{}
|
|
||||||
File diff suppressed because one or more lines are too long
@ -1,97 +0,0 @@
|
|||||||
{
|
|
||||||
"System Joysticks": {
|
|
||||||
"window": {
|
|
||||||
"enabled": false
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"keyboardJoysticks": [
|
|
||||||
{
|
|
||||||
"axisConfig": [
|
|
||||||
{
|
|
||||||
"decKey": 65,
|
|
||||||
"incKey": 68
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"decKey": 87,
|
|
||||||
"incKey": 83
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"decKey": 69,
|
|
||||||
"decayRate": 0.0,
|
|
||||||
"incKey": 82,
|
|
||||||
"keyRate": 0.009999999776482582
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"axisCount": 3,
|
|
||||||
"buttonCount": 4,
|
|
||||||
"buttonKeys": [
|
|
||||||
90,
|
|
||||||
88,
|
|
||||||
67,
|
|
||||||
86
|
|
||||||
],
|
|
||||||
"povConfig": [
|
|
||||||
{
|
|
||||||
"key0": 328,
|
|
||||||
"key135": 323,
|
|
||||||
"key180": 322,
|
|
||||||
"key225": 321,
|
|
||||||
"key270": 324,
|
|
||||||
"key315": 327,
|
|
||||||
"key45": 329,
|
|
||||||
"key90": 326
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"povCount": 1
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"axisConfig": [
|
|
||||||
{
|
|
||||||
"decKey": 74,
|
|
||||||
"incKey": 76
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"decKey": 73,
|
|
||||||
"incKey": 75
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"axisCount": 2,
|
|
||||||
"buttonCount": 4,
|
|
||||||
"buttonKeys": [
|
|
||||||
77,
|
|
||||||
44,
|
|
||||||
46,
|
|
||||||
47
|
|
||||||
],
|
|
||||||
"povCount": 0
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"axisConfig": [
|
|
||||||
{
|
|
||||||
"decKey": 263,
|
|
||||||
"incKey": 262
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"decKey": 265,
|
|
||||||
"incKey": 264
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"axisCount": 2,
|
|
||||||
"buttonCount": 6,
|
|
||||||
"buttonKeys": [
|
|
||||||
260,
|
|
||||||
268,
|
|
||||||
266,
|
|
||||||
261,
|
|
||||||
269,
|
|
||||||
267
|
|
||||||
],
|
|
||||||
"povCount": 0
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"axisCount": 0,
|
|
||||||
"buttonCount": 0,
|
|
||||||
"povCount": 0
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
@ -1,126 +0,0 @@
|
|||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "parallel",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "BlueBasStart"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "AprilTag"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 1.0
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "parallel",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "BlueBasChercher"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "AprilTag"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "parallel",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "AprilTag"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "BlueBasPorter"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 1.0
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "parallel",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "BlueBasChercher2"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "AprilTag"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "parallel",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "BlueBasPorter2"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "AprilTag"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"resetOdom": true,
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
@ -1,19 +0,0 @@
|
|||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "New Path"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"resetOdom": true,
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
@ -4,12 +4,6 @@
|
|||||||
"type": "sequential",
|
"type": "sequential",
|
||||||
"data": {
|
"data": {
|
||||||
"commands": [
|
"commands": [
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 3.0
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"type": "path",
|
"type": "path",
|
||||||
"data": {
|
"data": {
|
||||||
|
|||||||
@ -1,63 +0,0 @@
|
|||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"command": {
|
|
||||||
"type": "sequential",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "path",
|
|
||||||
"data": {
|
|
||||||
"pathName": "New Path"
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 2.0
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "deadline",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 2.0
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "baleeuse"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "deadline",
|
|
||||||
"data": {
|
|
||||||
"commands": [
|
|
||||||
{
|
|
||||||
"type": "wait",
|
|
||||||
"data": {
|
|
||||||
"waitTime": 2.0
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "named",
|
|
||||||
"data": {
|
|
||||||
"name": "baleeuse sort"
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
},
|
|
||||||
"resetOdom": true,
|
|
||||||
"folder": null,
|
|
||||||
"choreoAuto": false
|
|
||||||
}
|
|
||||||
File diff suppressed because one or more lines are too long
@ -9,7 +9,7 @@
|
|||||||
"prevControl": null,
|
"prevControl": null,
|
||||||
"nextControl": {
|
"nextControl": {
|
||||||
"x": 3.392520491803279,
|
"x": 3.392520491803279,
|
||||||
"y": 1.7293545081967205
|
"y": 1.7293545081967203
|
||||||
},
|
},
|
||||||
"isLocked": false,
|
"isLocked": false,
|
||||||
"linkedName": null
|
"linkedName": null
|
||||||
|
|||||||
@ -8,8 +8,8 @@
|
|||||||
},
|
},
|
||||||
"prevControl": null,
|
"prevControl": null,
|
||||||
"nextControl": {
|
"nextControl": {
|
||||||
"x": 6.40062663880838,
|
"x": 6.400623816881145,
|
||||||
"y": 1.8077964923366423
|
"y": 1.8077934662846444
|
||||||
},
|
},
|
||||||
"isLocked": false,
|
"isLocked": false,
|
||||||
"linkedName": null
|
"linkedName": null
|
||||||
@ -33,8 +33,8 @@
|
|||||||
"pointTowardsZones": [],
|
"pointTowardsZones": [],
|
||||||
"eventMarkers": [],
|
"eventMarkers": [],
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 1.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 1.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 540.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 720.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
@ -50,5 +50,5 @@
|
|||||||
"velocity": 0,
|
"velocity": 0,
|
||||||
"rotation": 119.99999999999999
|
"rotation": 119.99999999999999
|
||||||
},
|
},
|
||||||
"useDefaultConstraints": false
|
"useDefaultConstraints": true
|
||||||
}
|
}
|
||||||
@ -1,54 +0,0 @@
|
|||||||
{
|
|
||||||
"version": "2025.0",
|
|
||||||
"waypoints": [
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 7.564241803278687,
|
|
||||||
"y": 3.899129098360655
|
|
||||||
},
|
|
||||||
"prevControl": null,
|
|
||||||
"nextControl": {
|
|
||||||
"x": 6.506996649367605,
|
|
||||||
"y": 3.898163120386143
|
|
||||||
},
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"anchor": {
|
|
||||||
"x": 6.269569672131147,
|
|
||||||
"y": 3.899129098360655
|
|
||||||
},
|
|
||||||
"prevControl": {
|
|
||||||
"x": 7.259410179761819,
|
|
||||||
"y": 3.9002985318003183
|
|
||||||
},
|
|
||||||
"nextControl": null,
|
|
||||||
"isLocked": false,
|
|
||||||
"linkedName": null
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"rotationTargets": [],
|
|
||||||
"constraintZones": [],
|
|
||||||
"pointTowardsZones": [],
|
|
||||||
"eventMarkers": [],
|
|
||||||
"globalConstraints": {
|
|
||||||
"maxVelocity": 3.0,
|
|
||||||
"maxAcceleration": 3.0,
|
|
||||||
"maxAngularVelocity": 0.1,
|
|
||||||
"maxAngularAcceleration": 0.1,
|
|
||||||
"nominalVoltage": 12.0,
|
|
||||||
"unlimited": false
|
|
||||||
},
|
|
||||||
"goalEndState": {
|
|
||||||
"velocity": 0.0,
|
|
||||||
"rotation": 0.0
|
|
||||||
},
|
|
||||||
"reversed": false,
|
|
||||||
"folder": null,
|
|
||||||
"idealStartingState": {
|
|
||||||
"velocity": 0,
|
|
||||||
"rotation": 0.0
|
|
||||||
},
|
|
||||||
"useDefaultConstraints": false
|
|
||||||
}
|
|
||||||
@ -8,20 +8,20 @@
|
|||||||
},
|
},
|
||||||
"prevControl": null,
|
"prevControl": null,
|
||||||
"nextControl": {
|
"nextControl": {
|
||||||
"x": 7.119564732833961,
|
"x": 7.084733606557376,
|
||||||
"y": 6.478684258786044
|
"y": 6.488473360655737
|
||||||
},
|
},
|
||||||
"isLocked": false,
|
"isLocked": false,
|
||||||
"linkedName": null
|
"linkedName": null
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"anchor": {
|
"anchor": {
|
||||||
"x": 5.815652951699465,
|
"x": 5.849999999999999,
|
||||||
"y": 6.464497950819672
|
"y": 6.464497950819672
|
||||||
},
|
},
|
||||||
"prevControl": {
|
"prevControl": {
|
||||||
"x": 6.633711863494241,
|
"x": 6.32950819672131,
|
||||||
"y": 6.455611583097
|
"y": 6.464497950819672
|
||||||
},
|
},
|
||||||
"nextControl": null,
|
"nextControl": null,
|
||||||
"isLocked": false,
|
"isLocked": false,
|
||||||
@ -33,10 +33,10 @@
|
|||||||
"pointTowardsZones": [],
|
"pointTowardsZones": [],
|
||||||
"eventMarkers": [],
|
"eventMarkers": [],
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 1.0,
|
"maxVelocity": 3.0,
|
||||||
"maxAcceleration": 1.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 50.0,
|
"maxAngularVelocity": 540.0,
|
||||||
"maxAngularAcceleration": 100.0,
|
"maxAngularAcceleration": 720.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 12.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
@ -50,5 +50,5 @@
|
|||||||
"velocity": 0,
|
"velocity": 0,
|
||||||
"rotation": 180.0
|
"rotation": 180.0
|
||||||
},
|
},
|
||||||
"useDefaultConstraints": false
|
"useDefaultConstraints": true
|
||||||
}
|
}
|
||||||
@ -9,12 +9,12 @@
|
|||||||
"defaultMaxAngVel": 540.0,
|
"defaultMaxAngVel": 540.0,
|
||||||
"defaultMaxAngAccel": 720.0,
|
"defaultMaxAngAccel": 720.0,
|
||||||
"defaultNominalVoltage": 12.0,
|
"defaultNominalVoltage": 12.0,
|
||||||
"robotMass": 45.3592,
|
"robotMass": 74.088,
|
||||||
"robotMOI": 6.883,
|
"robotMOI": 6.883,
|
||||||
"robotTrackwidth": 0.546,
|
"robotTrackwidth": 0.546,
|
||||||
"driveWheelRadius": 0.048,
|
"driveWheelRadius": 0.048,
|
||||||
"driveGearing": 5.143,
|
"driveGearing": 5.143,
|
||||||
"maxDriveSpeed": 5.261,
|
"maxDriveSpeed": 5.45,
|
||||||
"driveMotorType": "krakenX60",
|
"driveMotorType": "krakenX60",
|
||||||
"driveCurrentLimit": 60.0,
|
"driveCurrentLimit": 60.0,
|
||||||
"wheelCOF": 1.2,
|
"wheelCOF": 1.2,
|
||||||
|
|||||||
37
src/main/java/frc/robot/Bougie.java
Normal file
37
src/main/java/frc/robot/Bougie.java
Normal file
@ -0,0 +1,37 @@
|
|||||||
|
// 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;
|
||||||
|
|
||||||
|
import com.ctre.phoenix.led.CANdle;
|
||||||
|
import com.ctre.phoenix.led.CANdleConfiguration;
|
||||||
|
import com.ctre.phoenix.led.RainbowAnimation;
|
||||||
|
|
||||||
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
|
||||||
|
public class Bougie extends SubsystemBase {
|
||||||
|
CANdle candle = new CANdle(5);
|
||||||
|
CANdleConfiguration config = new CANdleConfiguration();
|
||||||
|
RainbowAnimation rainbowAnim = new RainbowAnimation(1, 0.5, 64);
|
||||||
|
/** Creates a new Bougie. */
|
||||||
|
public Bougie() {
|
||||||
|
config.brightnessScalar = 0.5;
|
||||||
|
candle.configAllSettings(config);
|
||||||
|
}
|
||||||
|
public void Rouge() {
|
||||||
|
candle.setLEDs(255, 0, 0);
|
||||||
|
}
|
||||||
|
public void Vert() {
|
||||||
|
candle.setLEDs(0, 255, 0);
|
||||||
|
}
|
||||||
|
public void Bleu() {
|
||||||
|
candle.setLEDs(0, 0, 255);
|
||||||
|
}
|
||||||
|
public void RainBow(){candle.animate(rainbowAnim);}
|
||||||
|
@Override
|
||||||
|
public void periodic() {
|
||||||
|
// This method will be called once per scheduler run
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
File diff suppressed because it is too large
Load Diff
@ -6,185 +6,44 @@ package frc.robot;
|
|||||||
|
|
||||||
import static edu.wpi.first.units.Units.*;
|
import static edu.wpi.first.units.Units.*;
|
||||||
|
|
||||||
import java.util.Map;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
||||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt;
|
||||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||||
|
|
||||||
|
import com.pathplanner.lib.auto.AutoBuilder;
|
||||||
|
import com.pathplanner.lib.auto.NamedCommands;
|
||||||
|
|
||||||
import edu.wpi.first.cameraserver.CameraServer;
|
import edu.wpi.first.cameraserver.CameraServer;
|
||||||
import edu.wpi.first.math.MathUtil;
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||||
import edu.wpi.first.networktables.GenericEntry;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
import edu.wpi.first.wpilibj2.command.Commands;
|
||||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
|
||||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||||
import frc.robot.TunerConstants.TunerConstants;
|
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
|
||||||
import frc.robot.commands.RainBow;
|
|
||||||
import frc.robot.commands.reset;
|
|
||||||
import frc.robot.commands.Elevateur.Depart;
|
|
||||||
import frc.robot.commands.Elevateur.ElevateurManuel;
|
|
||||||
import frc.robot.commands.Elevateur.L2;
|
|
||||||
import frc.robot.commands.Elevateur.L3;
|
|
||||||
import frc.robot.commands.Elevateur.L4;
|
|
||||||
import frc.robot.commands.Elevateur.StationPince;
|
|
||||||
import frc.robot.commands.Limelight.AprilTag3;
|
|
||||||
import frc.robot.commands.Limelight.AprilTag3G;
|
|
||||||
import frc.robot.commands.Limelight.Forme3;
|
|
||||||
import frc.robot.commands.Pince.AlgueExpire;
|
|
||||||
import frc.robot.commands.Pince.Algue_inspire;
|
|
||||||
import frc.robot.commands.Pince.CorailAspir;
|
|
||||||
import frc.robot.commands.Pince.CoralAlgueInspire;
|
|
||||||
import frc.robot.commands.Pince.CoralExpire;
|
|
||||||
import frc.robot.commands.Pince.PinceManuel;
|
|
||||||
import frc.robot.commands.requin.BalayeuseAlgue;
|
|
||||||
import frc.robot.commands.requin.BalayeuseCoral;
|
|
||||||
import frc.robot.commands.requin.BalayeuseHaut;
|
|
||||||
import frc.robot.commands.requin.ExpireCorail;
|
|
||||||
import frc.robot.commands.requin.L1Requin;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
import frc.robot.subsystems.Limelight3;
|
|
||||||
import frc.robot.subsystems.Limelight3G;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
import frc.robot.commands.requin.exspire;
|
|
||||||
import frc.robot.commands.Pince.DepartPince;
|
|
||||||
import frc.robot.commands.Elevateur.balonL2;
|
|
||||||
import frc.robot.commands.Elevateur.balonL3;
|
|
||||||
|
|
||||||
public class RobotContainer {
|
public class RobotContainer {
|
||||||
ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
|
|
||||||
.withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
|
|
||||||
GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
||||||
GenericEntry sortirAngle = layoutauto.add("Cote?",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
||||||
GenericEntry ReculerB = layoutauto.add("ReculerB",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
||||||
GenericEntry ReculerR = layoutauto.add("ReculerR",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
||||||
GenericEntry L4 = layoutauto.add("L4",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
||||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
|
|
||||||
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
|
||||||
|
|
||||||
/* Setting up bindings for necessary control of the swerve drive platform */
|
private final CommandXboxController joystick = new CommandXboxController(0);
|
||||||
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
|
||||||
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
|
||||||
.withDriveRequestType(DriveRequestType.OpenLoopVoltage
|
|
||||||
); // Use open-loop control for drive motors
|
|
||||||
|
|
||||||
private final Telemetry logger = new Telemetry(MaxSpeed);
|
|
||||||
private final CommandXboxController manette1 = new CommandXboxController(0);
|
|
||||||
private final CommandXboxController manette2 = new CommandXboxController(1);
|
|
||||||
private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2
|
|
||||||
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
|
||||||
public double getAngle() {
|
|
||||||
return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
|
|
||||||
}
|
|
||||||
|
|
||||||
Elevateur elevateur = new Elevateur();
|
|
||||||
Pince pince = new Pince();
|
|
||||||
ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
|
|
||||||
PinceManuel pinceManuel = new PinceManuel(pince,manette2::getRightY);
|
|
||||||
Bougie bougie = new Bougie();
|
|
||||||
Limelight3G limelight3g = new Limelight3G();
|
|
||||||
Limelight3 limelight3 = new Limelight3();
|
|
||||||
Pose2d pose = new Pose2d();
|
|
||||||
Requin requin = new Requin();
|
|
||||||
CorailAspir corailAspir = new CorailAspir(pince, bougie);
|
|
||||||
public RobotContainer() {
|
public RobotContainer() {
|
||||||
CameraServer.startAutomaticCapture();
|
|
||||||
configureBindings();
|
configureBindings();
|
||||||
}
|
}
|
||||||
|
|
||||||
private void configureBindings() {
|
private void configureBindings() {
|
||||||
drivetrain.registerTelemetry(logger::telemeterize);
|
|
||||||
drivetrain.setDefaultCommand(
|
|
||||||
// Drivetrain will execute this command periodically
|
|
||||||
drivetrain.applyRequest(() ->
|
|
||||||
drive.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*-manette1.getLeftX()*-manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward)
|
|
||||||
.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*-manette1.getLeftY()*-manette1.getLeftY()*MaxSpeed, 0.05)) // Drive left with negative X (left)
|
|
||||||
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*-manette1.getRightX()*-manette1.getRightX()*MaxAngularRate, 0.05)) // Drive counterclockwise with negative X (left)
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
/* Manette 1 */
|
|
||||||
//pince
|
|
||||||
manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
|
|
||||||
manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie));
|
|
||||||
manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie));
|
|
||||||
manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
||||||
manette1.povRight().whileTrue(new CoralExpire(pince, bougie));
|
|
||||||
manette1.leftTrigger().whileTrue(new DepartPince(pince));
|
|
||||||
manette1.povDown().whileTrue(new Algue_inspire(pince,bougie));
|
|
||||||
manette1.leftStick().whileTrue(new CorailAspir(pince, bougie));
|
|
||||||
//elevateur
|
|
||||||
manette1.a().toggleOnTrue(new Depart(elevateur, pince));
|
|
||||||
manette1.b().toggleOnTrue(new L2(elevateur,pince));
|
|
||||||
manette1.x().toggleOnTrue(new L3(elevateur, pince));
|
|
||||||
manette1.y().toggleOnTrue(new L4(elevateur, pince));
|
|
||||||
manette1.povUp().toggleOnTrue(new balonL2(elevateur));
|
|
||||||
manette1.start().toggleOnTrue(new balonL3(elevateur));
|
|
||||||
|
|
||||||
|
|
||||||
/* Manette 2 */
|
|
||||||
//requin
|
|
||||||
manette2.rightBumper().whileTrue(new BalayeuseAlgue(requin,bougie));
|
|
||||||
manette2.leftBumper().whileTrue(new L1Requin(requin, bougie));
|
|
||||||
manette2.rightTrigger().whileTrue(new BalayeuseHaut(requin));
|
|
||||||
manette2.leftTrigger().whileTrue(new BalayeuseCoral(requin, bougie));
|
|
||||||
manette2.y().whileTrue(new exspire(requin, bougie));
|
|
||||||
manette2.x().whileTrue(new ExpireCorail(requin, bougie));
|
|
||||||
|
|
||||||
//limelight
|
|
||||||
manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
||||||
manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
|
|
||||||
|
|
||||||
//Pince manuel
|
|
||||||
pince.setDefaultCommand(new RunCommand(()->{
|
|
||||||
pince.pivote(MathUtil.applyDeadband((manette2.getRightY()*manette2.getRightY()*manette2.getRightY())/3, 0.05));
|
|
||||||
}, pince));
|
|
||||||
|
|
||||||
//Elevateur manuel
|
|
||||||
elevateur.setDefaultCommand(new RunCommand(()->{
|
|
||||||
elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
|
|
||||||
}, elevateur));
|
|
||||||
|
|
||||||
//Reset encodeur
|
|
||||||
manette2.start().whileTrue(new reset(elevateur, pince, requin));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public Command getAutonomousCommand() {
|
public Command getAutonomousCommand() {
|
||||||
return new SequentialCommandGroup(
|
return Commands.print("");
|
||||||
drivetrain.applyRequest(()->
|
|
||||||
drive.withVelocityX(0.1*MaxSpeed)
|
|
||||||
.withVelocityY(0)
|
|
||||||
.withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
|
|
||||||
drivetrain.applyRequest(()->
|
|
||||||
drive.withVelocityX(-0.1*MaxSpeed)
|
|
||||||
.withVelocityY(0)
|
|
||||||
.withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
|
|
||||||
drivetrain.applyRequest(()->
|
|
||||||
drive.withVelocityX(-0.1*MaxSpeed)
|
|
||||||
.withVelocityY(0)
|
|
||||||
.withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
|
|
||||||
drivetrain.applyRequest(()->
|
|
||||||
drive.withVelocityX(0.1*MaxSpeed)
|
|
||||||
.withVelocityY(0)
|
|
||||||
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
|
|
||||||
drivetrain.applyRequest(()->
|
|
||||||
drive.withVelocityX(-0.1*MaxSpeed)
|
|
||||||
.withVelocityY(0)
|
|
||||||
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
|
|
||||||
drivetrain.applyRequest(()->
|
|
||||||
drive.withVelocityX(0)
|
|
||||||
.withVelocityY(0)
|
|
||||||
.withRotationalRate(0)).withTimeout(0.1),
|
|
||||||
new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
|
|
||||||
new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
|
|
||||||
new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
|
|
||||||
new RainBow(bougie));
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -1,124 +0,0 @@
|
|||||||
package frc.robot;
|
|
||||||
|
|
||||||
import com.ctre.phoenix6.SignalLogger;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
|
||||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
|
||||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
|
||||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
|
||||||
import edu.wpi.first.networktables.DoubleArrayPublisher;
|
|
||||||
import edu.wpi.first.networktables.DoublePublisher;
|
|
||||||
import edu.wpi.first.networktables.NetworkTable;
|
|
||||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
|
||||||
import edu.wpi.first.networktables.StringPublisher;
|
|
||||||
import edu.wpi.first.networktables.StructArrayPublisher;
|
|
||||||
import edu.wpi.first.networktables.StructPublisher;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
||||||
import edu.wpi.first.wpilibj.util.Color;
|
|
||||||
import edu.wpi.first.wpilibj.util.Color8Bit;
|
|
||||||
|
|
||||||
public class Telemetry {
|
|
||||||
private final double MaxSpeed;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Construct a telemetry object, with the specified max speed of the robot
|
|
||||||
*
|
|
||||||
* @param maxSpeed Maximum speed in meters per second
|
|
||||||
*/
|
|
||||||
public Telemetry(double maxSpeed) {
|
|
||||||
MaxSpeed = maxSpeed;
|
|
||||||
SignalLogger.start();
|
|
||||||
}
|
|
||||||
|
|
||||||
/* What to publish over networktables for telemetry */
|
|
||||||
private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
|
|
||||||
|
|
||||||
/* Robot swerve drive state */
|
|
||||||
private final NetworkTable driveStateTable = inst.getTable("DriveState");
|
|
||||||
private final StructPublisher<Pose2d> drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish();
|
|
||||||
private final StructPublisher<ChassisSpeeds> driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish();
|
|
||||||
private final StructArrayPublisher<SwerveModuleState> driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish();
|
|
||||||
private final StructArrayPublisher<SwerveModuleState> driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish();
|
|
||||||
private final StructArrayPublisher<SwerveModulePosition> driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish();
|
|
||||||
private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish();
|
|
||||||
private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish();
|
|
||||||
|
|
||||||
/* Robot pose for field positioning */
|
|
||||||
private final NetworkTable table = inst.getTable("Pose");
|
|
||||||
private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
|
|
||||||
private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
|
|
||||||
|
|
||||||
/* Mechanisms to represent the swerve module states */
|
|
||||||
private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
|
|
||||||
new Mechanism2d(1, 1),
|
|
||||||
new Mechanism2d(1, 1),
|
|
||||||
new Mechanism2d(1, 1),
|
|
||||||
new Mechanism2d(1, 1),
|
|
||||||
};
|
|
||||||
/* A direction and length changing ligament for speed representation */
|
|
||||||
private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
|
|
||||||
m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
|
|
||||||
m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
|
|
||||||
m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
|
|
||||||
m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
|
|
||||||
};
|
|
||||||
/* A direction changing and length constant ligament for module direction */
|
|
||||||
private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
|
|
||||||
m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
|
|
||||||
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
|
|
||||||
m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
|
|
||||||
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
|
|
||||||
m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
|
|
||||||
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
|
|
||||||
m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
|
|
||||||
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
|
|
||||||
};
|
|
||||||
|
|
||||||
private final double[] m_poseArray = new double[3];
|
|
||||||
private final double[] m_moduleStatesArray = new double[8];
|
|
||||||
private final double[] m_moduleTargetsArray = new double[8];
|
|
||||||
|
|
||||||
/** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
|
|
||||||
public void telemeterize(SwerveDriveState state) {
|
|
||||||
/* Telemeterize the swerve drive state */
|
|
||||||
drivePose.set(state.Pose);
|
|
||||||
driveSpeeds.set(state.Speeds);
|
|
||||||
driveModuleStates.set(state.ModuleStates);
|
|
||||||
driveModuleTargets.set(state.ModuleTargets);
|
|
||||||
driveModulePositions.set(state.ModulePositions);
|
|
||||||
driveTimestamp.set(state.Timestamp);
|
|
||||||
driveOdometryFrequency.set(1.0 / state.OdometryPeriod);
|
|
||||||
|
|
||||||
/* Also write to log file */
|
|
||||||
m_poseArray[0] = state.Pose.getX();
|
|
||||||
m_poseArray[1] = state.Pose.getY();
|
|
||||||
m_poseArray[2] = state.Pose.getRotation().getDegrees();
|
|
||||||
for (int i = 0; i < 4; ++i) {
|
|
||||||
m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians();
|
|
||||||
m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond;
|
|
||||||
m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians();
|
|
||||||
m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond;
|
|
||||||
}
|
|
||||||
|
|
||||||
SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray);
|
|
||||||
SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray);
|
|
||||||
SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray);
|
|
||||||
SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
|
|
||||||
|
|
||||||
/* Telemeterize the pose to a Field2d */
|
|
||||||
fieldTypePub.set("Field2d");
|
|
||||||
fieldPub.set(m_poseArray);
|
|
||||||
|
|
||||||
/* Telemeterize the module states to a Mechanism2d */
|
|
||||||
for (int i = 0; i < 4; ++i) {
|
|
||||||
m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
|
|
||||||
m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
|
|
||||||
m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));
|
|
||||||
|
|
||||||
SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,290 +0,0 @@
|
|||||||
package frc.robot.TunerConstants;
|
|
||||||
|
|
||||||
import static edu.wpi.first.units.Units.*;
|
|
||||||
|
|
||||||
import com.ctre.phoenix6.CANBus;
|
|
||||||
import com.ctre.phoenix6.configs.*;
|
|
||||||
import com.ctre.phoenix6.hardware.*;
|
|
||||||
import com.ctre.phoenix6.signals.*;
|
|
||||||
import com.ctre.phoenix6.swerve.*;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.Matrix;
|
|
||||||
import edu.wpi.first.math.numbers.N1;
|
|
||||||
import edu.wpi.first.math.numbers.N3;
|
|
||||||
import edu.wpi.first.units.measure.*;
|
|
||||||
|
|
||||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
||||||
|
|
||||||
// Generated by the Tuner X Swerve Project Generator
|
|
||||||
// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
|
|
||||||
public class TunerConstants {
|
|
||||||
// Both sets of gains need to be tuned to your individual robot.
|
|
||||||
|
|
||||||
// The steer motor uses any SwerveModule.SteerRequestType control request with the
|
|
||||||
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
|
|
||||||
//private static final Slot0Configs steerGains = new Slot0Configs()
|
|
||||||
// .withKP(68.294).withKI(0).withKD(4.7806)
|
|
||||||
// .withKS(0.20754).withKV(2.4832).withKA(0.099824)
|
|
||||||
// .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
|
||||||
private static final Slot0Configs steerGains = new Slot0Configs()
|
|
||||||
.withKP(43.502).withKI(0).withKD(2.7353)
|
|
||||||
.withKS(0.027275).withKV(2.5818).withKA(0.1055)
|
|
||||||
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
|
||||||
// When using closed-loop control, the drive motor uses the control
|
|
||||||
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
|
|
||||||
private static final Slot0Configs driveGains = new Slot0Configs()
|
|
||||||
.withKP(63.167).withKI(0).withKD(0.54521)
|
|
||||||
.withKS(0.18227).withKV(0.12483);
|
|
||||||
|
|
||||||
// The closed-loop output type to use for the steer motors;
|
|
||||||
// This affects the PID/FF gains for the steer motors
|
|
||||||
private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
|
|
||||||
// The closed-loop output type to use for the drive motors;
|
|
||||||
// This affects the PID/FF gains for the drive motors
|
|
||||||
private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
|
|
||||||
|
|
||||||
// The type of motor used for the drive motor
|
|
||||||
private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
|
|
||||||
// The type of motor used for the drive motor
|
|
||||||
private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
|
|
||||||
|
|
||||||
// The remote sensor feedback type to use for the steer motors;
|
|
||||||
// When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
|
|
||||||
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
|
|
||||||
|
|
||||||
// The stator current at which the wheels start to slip;
|
|
||||||
// This needs to be tuned to your individual robot
|
|
||||||
private static final Current kSlipCurrent = Amps.of(120.0);
|
|
||||||
|
|
||||||
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
|
|
||||||
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
|
|
||||||
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
|
|
||||||
private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
|
|
||||||
.withCurrentLimits(
|
|
||||||
new CurrentLimitsConfigs()
|
|
||||||
// Swerve azimuth does not require much torque output, so we can set a relatively low
|
|
||||||
// stator current limit to help avoid brownouts without impacting performance.
|
|
||||||
.withStatorCurrentLimit(Amps.of(60))
|
|
||||||
.withStatorCurrentLimitEnable(true)
|
|
||||||
);
|
|
||||||
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
|
|
||||||
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
|
|
||||||
private static final Pigeon2Configuration pigeonConfigs = null;
|
|
||||||
|
|
||||||
// CAN bus that the devices are located on;
|
|
||||||
// All swerve devices must share the same CAN bus
|
|
||||||
public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot");
|
|
||||||
|
|
||||||
// Theoretical free speed (m/s) at 12 V applied output;
|
|
||||||
// This needs to be tuned to your individual robot
|
|
||||||
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(6);
|
|
||||||
|
|
||||||
// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
|
|
||||||
// This may need to be tuned to your individual robot
|
|
||||||
private static final double kCoupleRatio = 3.5714285714285716;
|
|
||||||
|
|
||||||
private static final double kDriveGearRatio = 6.122448979591837;
|
|
||||||
private static final double kSteerGearRatio = 21.428571428571427;
|
|
||||||
private static final Distance kWheelRadius = Inches.of(2);
|
|
||||||
|
|
||||||
private static final boolean kInvertLeftSide = false;
|
|
||||||
private static final boolean kInvertRightSide = true;
|
|
||||||
|
|
||||||
private static final int kPigeonId = 13;
|
|
||||||
|
|
||||||
// These are only used for simulation
|
|
||||||
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
|
|
||||||
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
|
|
||||||
// Simulated voltage necessary to overcome friction
|
|
||||||
private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
|
|
||||||
private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
|
|
||||||
|
|
||||||
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
|
|
||||||
.withCANBusName(kCANBus.getName())
|
|
||||||
.withPigeon2Id(kPigeonId)
|
|
||||||
.withPigeon2Configs(pigeonConfigs);
|
|
||||||
|
|
||||||
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
|
|
||||||
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
|
|
||||||
.withDriveMotorGearRatio(kDriveGearRatio)
|
|
||||||
.withSteerMotorGearRatio(kSteerGearRatio)
|
|
||||||
.withCouplingGearRatio(kCoupleRatio)
|
|
||||||
.withWheelRadius(kWheelRadius)
|
|
||||||
.withSteerMotorGains(steerGains)
|
|
||||||
.withDriveMotorGains(driveGains)
|
|
||||||
.withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
|
|
||||||
.withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
|
|
||||||
.withSlipCurrent(kSlipCurrent)
|
|
||||||
.withSpeedAt12Volts(kSpeedAt12Volts)
|
|
||||||
.withDriveMotorType(kDriveMotorType)
|
|
||||||
.withSteerMotorType(kSteerMotorType)
|
|
||||||
.withFeedbackSource(kSteerFeedbackType)
|
|
||||||
.withDriveMotorInitialConfigs(driveInitialConfigs)
|
|
||||||
.withSteerMotorInitialConfigs(steerInitialConfigs)
|
|
||||||
.withEncoderInitialConfigs(encoderInitialConfigs)
|
|
||||||
.withSteerInertia(kSteerInertia)
|
|
||||||
.withDriveInertia(kDriveInertia)
|
|
||||||
.withSteerFrictionVoltage(kSteerFrictionVoltage)
|
|
||||||
.withDriveFrictionVoltage(kDriveFrictionVoltage);
|
|
||||||
|
|
||||||
|
|
||||||
// Front Left
|
|
||||||
private static final int kFrontLeftDriveMotorId = 4;
|
|
||||||
private static final int kFrontLeftSteerMotorId = 5;
|
|
||||||
private static final int kFrontLeftEncoderId = 12;
|
|
||||||
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.353271484375);
|
|
||||||
private static final boolean kFrontLeftSteerMotorInverted = true;
|
|
||||||
private static final boolean kFrontLeftEncoderInverted = false;
|
|
||||||
|
|
||||||
private static final Distance kFrontLeftXPos = Inches.of(13.5);
|
|
||||||
private static final Distance kFrontLeftYPos = Inches.of(10.5);
|
|
||||||
|
|
||||||
// Front Right
|
|
||||||
private static final int kFrontRightDriveMotorId = 2;
|
|
||||||
private static final int kFrontRightSteerMotorId = 6;
|
|
||||||
private static final int kFrontRightEncoderId = 9;
|
|
||||||
private static final Angle kFrontRightEncoderOffset = Rotations.of(0.2119140625);
|
|
||||||
private static final boolean kFrontRightSteerMotorInverted = true;
|
|
||||||
private static final boolean kFrontRightEncoderInverted = false;
|
|
||||||
|
|
||||||
private static final Distance kFrontRightXPos = Inches.of(13.5);
|
|
||||||
private static final Distance kFrontRightYPos = Inches.of(-10.5);
|
|
||||||
|
|
||||||
// Back Left
|
|
||||||
private static final int kBackLeftDriveMotorId = 18;
|
|
||||||
private static final int kBackLeftSteerMotorId = 8;
|
|
||||||
private static final int kBackLeftEncoderId = 11;
|
|
||||||
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.236572265625);
|
|
||||||
private static final boolean kBackLeftSteerMotorInverted = true;
|
|
||||||
private static final boolean kBackLeftEncoderInverted = false;
|
|
||||||
|
|
||||||
private static final Distance kBackLeftXPos = Inches.of(-13.5);
|
|
||||||
private static final Distance kBackLeftYPos = Inches.of(10.5);
|
|
||||||
|
|
||||||
// Back Right
|
|
||||||
private static final int kBackRightDriveMotorId = 3;
|
|
||||||
private static final int kBackRightSteerMotorId = 7;
|
|
||||||
private static final int kBackRightEncoderId = 10;
|
|
||||||
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.330078125);
|
|
||||||
private static final boolean kBackRightSteerMotorInverted = true;
|
|
||||||
private static final boolean kBackRightEncoderInverted = false;
|
|
||||||
|
|
||||||
private static final Distance kBackRightXPos = Inches.of(-13.5);
|
|
||||||
private static final Distance kBackRightYPos = Inches.of(-10.5);
|
|
||||||
|
|
||||||
|
|
||||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
|
|
||||||
ConstantCreator.createModuleConstants(
|
|
||||||
kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
|
|
||||||
kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
|
|
||||||
);
|
|
||||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
|
|
||||||
ConstantCreator.createModuleConstants(
|
|
||||||
kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
|
|
||||||
kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
|
|
||||||
);
|
|
||||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
|
|
||||||
ConstantCreator.createModuleConstants(
|
|
||||||
kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
|
|
||||||
kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
|
|
||||||
);
|
|
||||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
|
|
||||||
ConstantCreator.createModuleConstants(
|
|
||||||
kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
|
|
||||||
kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
|
|
||||||
);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates a CommandSwerveDrivetrain instance.
|
|
||||||
* This should only be called once in your robot program,.
|
|
||||||
*/
|
|
||||||
public static CommandSwerveDrivetrain createDrivetrain() {
|
|
||||||
return new CommandSwerveDrivetrain(
|
|
||||||
DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
|
|
||||||
*/
|
|
||||||
public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
|
|
||||||
/**
|
|
||||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
||||||
* <p>
|
|
||||||
* This constructs the underlying hardware devices, so users should not construct
|
|
||||||
* the devices themselves. If they need the devices, they can access them through
|
|
||||||
* getters in the classes.
|
|
||||||
*
|
|
||||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
||||||
* @param modules Constants for each specific module
|
|
||||||
*/
|
|
||||||
public TunerSwerveDrivetrain(
|
|
||||||
SwerveDrivetrainConstants drivetrainConstants,
|
|
||||||
SwerveModuleConstants<?, ?, ?>... modules
|
|
||||||
) {
|
|
||||||
super(
|
|
||||||
TalonFX::new, TalonFX::new, CANcoder::new,
|
|
||||||
drivetrainConstants, modules
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
||||||
* <p>
|
|
||||||
* This constructs the underlying hardware devices, so users should not construct
|
|
||||||
* the devices themselves. If they need the devices, they can access them through
|
|
||||||
* getters in the classes.
|
|
||||||
*
|
|
||||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
||||||
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
|
||||||
* unspecified or set to 0 Hz, this is 250 Hz on
|
|
||||||
* CAN FD, and 100 Hz on CAN 2.0.
|
|
||||||
* @param modules Constants for each specific module
|
|
||||||
*/
|
|
||||||
public TunerSwerveDrivetrain(
|
|
||||||
SwerveDrivetrainConstants drivetrainConstants,
|
|
||||||
double odometryUpdateFrequency,
|
|
||||||
SwerveModuleConstants<?, ?, ?>... modules
|
|
||||||
) {
|
|
||||||
super(
|
|
||||||
TalonFX::new, TalonFX::new, CANcoder::new,
|
|
||||||
drivetrainConstants, odometryUpdateFrequency, modules
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
||||||
* <p>
|
|
||||||
* This constructs the underlying hardware devices, so users should not construct
|
|
||||||
* the devices themselves. If they need the devices, they can access them through
|
|
||||||
* getters in the classes.
|
|
||||||
*
|
|
||||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
||||||
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
|
||||||
* unspecified or set to 0 Hz, this is 250 Hz on
|
|
||||||
* CAN FD, and 100 Hz on CAN 2.0.
|
|
||||||
* @param odometryStandardDeviation The standard deviation for odometry calculation
|
|
||||||
* in the form [x, y, theta]ᵀ, with units in meters
|
|
||||||
* and radians
|
|
||||||
* @param visionStandardDeviation The standard deviation for vision calculation
|
|
||||||
* in the form [x, y, theta]ᵀ, with units in meters
|
|
||||||
* and radians
|
|
||||||
* @param modules Constants for each specific module
|
|
||||||
*/
|
|
||||||
public TunerSwerveDrivetrain(
|
|
||||||
SwerveDrivetrainConstants drivetrainConstants,
|
|
||||||
double odometryUpdateFrequency,
|
|
||||||
Matrix<N3, N1> odometryStandardDeviation,
|
|
||||||
Matrix<N3, N1> visionStandardDeviation,
|
|
||||||
SwerveModuleConstants<?, ?, ?>... modules
|
|
||||||
) {
|
|
||||||
super(
|
|
||||||
TalonFX::new, TalonFX::new, CANcoder::new,
|
|
||||||
drivetrainConstants, odometryUpdateFrequency,
|
|
||||||
odometryStandardDeviation, visionStandardDeviation, modules
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,54 +0,0 @@
|
|||||||
// 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.commands;
|
|
||||||
|
|
||||||
import static edu.wpi.first.units.Units.*;
|
|
||||||
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.TunerConstants.TunerConstants;
|
|
||||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class AvancerAuto extends Command {
|
|
||||||
private CommandSwerveDrivetrain commandSwerveDrivetrain = TunerConstants.createDrivetrain();
|
|
||||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
|
|
||||||
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of
|
|
||||||
private final SwerveRequest.RobotCentric drive = new SwerveRequest.RobotCentric()
|
|
||||||
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
|
|
||||||
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
|
|
||||||
/** Creates a new AvancerAuto. */
|
|
||||||
public AvancerAuto(SwerveRequest.RobotCentric drive, CommandSwerveDrivetrain commandSwerveDrivetrain) {
|
|
||||||
// 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() {
|
|
||||||
commandSwerveDrivetrain.applyRequest(()->
|
|
||||||
drive.withVelocityY(0.5*MaxSpeed)
|
|
||||||
.withVelocityX(0)
|
|
||||||
.withRotationalRate(0));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
drive.withVelocityY(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,58 +0,0 @@
|
|||||||
// 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.commands.Elevateur;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class Depart extends Command {
|
|
||||||
private Elevateur elevateur;
|
|
||||||
private Pince pince;
|
|
||||||
/** Creates a new L2. */
|
|
||||||
public Depart(Elevateur elevateur, Pince pince) {
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
this.pince = pince;
|
|
||||||
addRequirements(elevateur, pince);
|
|
||||||
// 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() {
|
|
||||||
if(pince.position()){
|
|
||||||
pince.pivote(0);
|
|
||||||
pince.reset();
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.pivote(-0.2);
|
|
||||||
}
|
|
||||||
if(elevateur.limit2()==true){
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
elevateur.reset();
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
elevateur.vitesse(.5);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return elevateur.limit2() == true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,49 +0,0 @@
|
|||||||
// 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.commands.Elevateur;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class ElevateurManuel extends Command {
|
|
||||||
private DoubleSupplier doubleSupplier;
|
|
||||||
private Elevateur elevateur;
|
|
||||||
/** Creates a new ElevateurManuel. */
|
|
||||||
public ElevateurManuel(Elevateur elevateur,DoubleSupplier doubleSupplier) {
|
|
||||||
this.doubleSupplier = doubleSupplier;
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
addRequirements(elevateur);
|
|
||||||
// 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() {
|
|
||||||
if (elevateur.limit2()){
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
elevateur.vitesse(doubleSupplier.getAsDouble()/3.5);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,53 +0,0 @@
|
|||||||
// 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.commands.Elevateur;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class L2 extends Command {
|
|
||||||
private Elevateur elevateur;
|
|
||||||
private Pince pince;
|
|
||||||
/** Creates a new L2. */
|
|
||||||
public L2(Elevateur elevateur,Pince pince) {
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
this.pince = pince;
|
|
||||||
addRequirements(elevateur,pince);
|
|
||||||
// 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() {
|
|
||||||
if(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
else if(pince.encodeurpivot()>=15){
|
|
||||||
pince.pivote(-0.2);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.pivote(0.1);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,64 +0,0 @@
|
|||||||
// 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.commands.Elevateur;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class L3 extends Command {
|
|
||||||
private Elevateur elevateur;
|
|
||||||
private Pince pince;
|
|
||||||
/** Creates a new L2. */
|
|
||||||
public L3(Elevateur elevateur,Pince pince) {
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
this.pince = pince;
|
|
||||||
addRequirements(elevateur,pince);
|
|
||||||
// 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() {
|
|
||||||
if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
pince.pivote(-0.15);
|
|
||||||
if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
else if(pince.encodeurpivot()>=20){
|
|
||||||
pince.pivote(-0.15);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.pivote(0.15);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){
|
|
||||||
elevateur.vitesse(-0.7);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
elevateur.vitesse(0.25);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,63 +0,0 @@
|
|||||||
// 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.commands.Elevateur;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class L4 extends Command {
|
|
||||||
private Elevateur elevateur;
|
|
||||||
private Pince pince;
|
|
||||||
/** Creates a new L2. */
|
|
||||||
public L4(Elevateur elevateur,Pince pince) {
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
this.pince = pince;
|
|
||||||
addRequirements(elevateur,pince);
|
|
||||||
// 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() {
|
|
||||||
if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
pince.pivote(-0.15);
|
|
||||||
if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
else if(pince.encodeurpivot()>=20){
|
|
||||||
pince.pivote(-0.15);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.pivote(0.15);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){
|
|
||||||
elevateur.vitesse(-0.7);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
elevateur.vitesse(.25);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,90 +0,0 @@
|
|||||||
// 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.commands.Elevateur;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class StationPince extends Command {
|
|
||||||
ShuffleboardTab teb = Shuffleboard.getTab("teb");
|
|
||||||
private Pince pince;
|
|
||||||
private Elevateur elevateur;
|
|
||||||
private Bougie bougie;
|
|
||||||
/** Creates a new L2Pince. */
|
|
||||||
public StationPince(Pince pince,Elevateur elevateur, Bougie bougie) {
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
this.pince = pince;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(pince, elevateur, bougie);
|
|
||||||
// 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() {
|
|
||||||
|
|
||||||
if(pince.emperagecoral() >= 18){
|
|
||||||
pince.x = true;
|
|
||||||
}
|
|
||||||
if(elevateur.position()<=-0.4 && elevateur.position()>= -0.5){
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
}
|
|
||||||
else if(elevateur.position()>=-0.4){
|
|
||||||
elevateur.vitesse(-0.7);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
elevateur.vitesse(0.25);
|
|
||||||
}
|
|
||||||
if(pince.x){
|
|
||||||
pince.aspirecoral(0);
|
|
||||||
bougie.Bleu();
|
|
||||||
pince.pivote(-0.2);
|
|
||||||
if(pince.position()){
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.aspirecoral(0.25);
|
|
||||||
|
|
||||||
}
|
|
||||||
if (!pince.x){
|
|
||||||
pince.pivote(-0.15);
|
|
||||||
if(pince.encodeurpivot()>=10 && pince.encodeurpivot()<=11){
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
else if(pince.encodeurpivot()>=10.5){
|
|
||||||
pince.pivote(-0.15);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.pivote(0.15);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
pince.pivote(0);
|
|
||||||
pince.aspirecoral(0);
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
pince.x =false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,49 +0,0 @@
|
|||||||
// 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.commands.Elevateur;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class balonL2 extends Command {
|
|
||||||
private Elevateur elevateur;
|
|
||||||
/** Creates a new L2. */
|
|
||||||
public balonL2(Elevateur elevateur) {
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
addRequirements(elevateur);
|
|
||||||
// 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() {
|
|
||||||
if(elevateur.position()<=-2 && elevateur.position()>=-2.2){
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
}
|
|
||||||
else if(elevateur.position()>= -1.95){
|
|
||||||
elevateur.vitesse(-0.7);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
elevateur.vitesse(0.25);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,49 +0,0 @@
|
|||||||
// 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.commands.Elevateur;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class balonL3 extends Command {
|
|
||||||
private Elevateur elevateur;
|
|
||||||
/** Creates a new L2. */
|
|
||||||
public balonL3(Elevateur elevateur) {
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
addRequirements(elevateur);
|
|
||||||
// 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() {
|
|
||||||
if(elevateur.position()<=-4 && elevateur.position()>=-4.1){
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
}
|
|
||||||
else if(elevateur.position()>= -4){
|
|
||||||
elevateur.vitesse(-0.7);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
elevateur.vitesse(0.25);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
elevateur.vitesse(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,81 +0,0 @@
|
|||||||
// 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.commands.Limelight;
|
|
||||||
import static edu.wpi.first.units.Units.*;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.TunerConstants.TunerConstants;
|
|
||||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
||||||
import frc.robot.subsystems.Limelight3;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class AprilTag3 extends Command {
|
|
||||||
private Limelight3 limelight3;
|
|
||||||
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
|
||||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
|
|
||||||
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
|
||||||
private DoubleSupplier x;
|
|
||||||
private DoubleSupplier y;
|
|
||||||
/* Setting up bindings for necessary control of the swerve drive platform */
|
|
||||||
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
|
||||||
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
|
||||||
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
|
|
||||||
/** Creates a new AprilTag3G. */
|
|
||||||
public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
|
|
||||||
this.limelight3 = limelight3;
|
|
||||||
this.drivetrain = drivetrain;
|
|
||||||
this.x = x;
|
|
||||||
this.y = y;
|
|
||||||
addRequirements(limelight3,drivetrain);
|
|
||||||
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
limelight3.Apriltag();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
double a = limelight3.getX();
|
|
||||||
|
|
||||||
if(limelight3.getV() == true){
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(a/10).
|
|
||||||
withVelocityX(x.getAsDouble()).
|
|
||||||
withVelocityY(y.getAsDouble()));
|
|
||||||
System.out.println(a/10);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(0).
|
|
||||||
withVelocityX(0).
|
|
||||||
withVelocityY(0));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(0)
|
|
||||||
.withVelocityX(0)
|
|
||||||
.withVelocityY(0));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,78 +0,0 @@
|
|||||||
// 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.commands.Limelight;
|
|
||||||
import static edu.wpi.first.units.Units.*;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.TunerConstants.TunerConstants;
|
|
||||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
||||||
import frc.robot.subsystems.Limelight3G;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class AprilTag3G extends Command {
|
|
||||||
private Limelight3G limelight3g;
|
|
||||||
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
|
||||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
|
|
||||||
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
|
||||||
private DoubleSupplier x;
|
|
||||||
private DoubleSupplier y;
|
|
||||||
/* Setting up bindings for necessary control of the swerve drive platform */
|
|
||||||
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
|
||||||
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
|
||||||
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
|
|
||||||
/** Creates a new AprilTag3G. */
|
|
||||||
/** Creates a new AprilTag3G. */
|
|
||||||
public AprilTag3G(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
|
|
||||||
this.limelight3g = limelight3g;
|
|
||||||
this.drivetrain = drivetrain;
|
|
||||||
this.x = x;
|
|
||||||
this.y = y;
|
|
||||||
addRequirements(limelight3g,drivetrain);
|
|
||||||
// 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() {
|
|
||||||
double a = limelight3g.getX();
|
|
||||||
if(limelight3g.getV() == true){
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(-a/7).
|
|
||||||
withVelocityY(x.getAsDouble()).
|
|
||||||
withVelocityX(-y.getAsDouble()));
|
|
||||||
System.out.println(a/7);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(0).
|
|
||||||
withVelocityX(0).
|
|
||||||
withVelocityY(0));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(0).
|
|
||||||
withVelocityX(0).
|
|
||||||
withVelocityY(0));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,79 +0,0 @@
|
|||||||
// 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.commands.Limelight;
|
|
||||||
import static edu.wpi.first.units.Units.*;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.TunerConstants.TunerConstants;
|
|
||||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
||||||
import frc.robot.subsystems.Limelight3;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class Forme3 extends Command {
|
|
||||||
private Limelight3 limelight3;
|
|
||||||
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
|
|
||||||
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
|
||||||
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
|
||||||
private DoubleSupplier x;
|
|
||||||
private DoubleSupplier y;
|
|
||||||
/* Setting up bindings for necessary control of the swerve drive platform */
|
|
||||||
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
|
||||||
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
|
||||||
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
|
|
||||||
/** Creates a new Forme3. */
|
|
||||||
public Forme3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
|
|
||||||
this.limelight3 = limelight3;
|
|
||||||
this.drivetrain = drivetrain;
|
|
||||||
this.x = x;
|
|
||||||
this.y = y;
|
|
||||||
addRequirements(limelight3,drivetrain);
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
limelight3.Forme();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
double a = limelight3.getX();
|
|
||||||
if(limelight3.getV() == true){
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(a/10).
|
|
||||||
withVelocityX(x.getAsDouble()).
|
|
||||||
withVelocityY(y.getAsDouble()));
|
|
||||||
System.out.println(a/10);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(0).
|
|
||||||
withVelocityX(0).
|
|
||||||
withVelocityY(0));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
drivetrain.setControl(drive.
|
|
||||||
withRotationalRate(0).
|
|
||||||
withVelocityX(0).
|
|
||||||
withVelocityY(0));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,51 +0,0 @@
|
|||||||
// 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.commands.Pince;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class AlgueExpire extends Command {
|
|
||||||
private Pince pince;
|
|
||||||
private Bougie bougie;
|
|
||||||
/** Creates a new CoralAlgue. */
|
|
||||||
public AlgueExpire(Pince pince,Bougie bougie) {
|
|
||||||
this.pince = pince;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(pince,bougie);
|
|
||||||
// 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() {
|
|
||||||
if(pince.emperagealgue()>60){
|
|
||||||
pince.aspirealgue(-0.5);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.aspirealgue(-0.5);
|
|
||||||
bougie.Jaune();
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
pince.aspirealgue(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,54 +0,0 @@
|
|||||||
// 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.commands.Pince;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class Algue_inspire extends Command {
|
|
||||||
/** Creates a new Algue_inspire. */
|
|
||||||
private Pince pince;
|
|
||||||
private Bougie bougie;
|
|
||||||
public Algue_inspire(Pince pince, Bougie bougie) {
|
|
||||||
this.pince = pince;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(pince, bougie);
|
|
||||||
// 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.
|
|
||||||
|
|
||||||
//ajouter l'amperage pour arreter les moteurs
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
|
|
||||||
if(pince.emperagealgue()>60){
|
|
||||||
pince.aspirealgue(0);
|
|
||||||
bougie.Bleu();
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.aspirealgue(0.5);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
pince.aspirealgue(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,51 +0,0 @@
|
|||||||
// 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.commands.Pince;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class CorailAspir extends Command {
|
|
||||||
/** Creates a new CorailAspir. */
|
|
||||||
private Pince pince;
|
|
||||||
Bougie bougie;
|
|
||||||
public CorailAspir(Pince pince, Bougie bougie) {
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
this.pince = pince;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(pince,bougie);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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() {
|
|
||||||
if(pince.emperagecoral() > 15){
|
|
||||||
pince.aspirecoral(0);
|
|
||||||
bougie.Bleu();
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.aspirecoral(0.5);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
pince.aspirecoral(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return pince.emperagecoral()>13;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,53 +0,0 @@
|
|||||||
// 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.commands.Pince;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class CoralAlgueInspire extends Command {
|
|
||||||
private Pince pince;
|
|
||||||
private Bougie bougie;
|
|
||||||
/** Creates a new CoralAlgue. */
|
|
||||||
public CoralAlgueInspire(Pince pince, Bougie bougie) {
|
|
||||||
this.pince = pince;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(pince,bougie);
|
|
||||||
// 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() {
|
|
||||||
pince.aspirecoral(-.5);
|
|
||||||
if(pince.emperagealgue()>60){
|
|
||||||
pince.aspirealgue(0);
|
|
||||||
bougie.Bleu();
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.aspirealgue(0.5);
|
|
||||||
bougie.Jaune();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
pince.aspirecoral(0);
|
|
||||||
pince.aspirealgue(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,49 +0,0 @@
|
|||||||
// 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.commands.Pince;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class CoralExpire extends Command {
|
|
||||||
private Pince pince;
|
|
||||||
Bougie bougie;
|
|
||||||
/** Creates a new CoralAlgue. */
|
|
||||||
public CoralExpire(Pince pince, Bougie bougie) {
|
|
||||||
this.pince = pince;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(pince,bougie);
|
|
||||||
// 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() {
|
|
||||||
/* je crois que ce nest pas necessaire
|
|
||||||
if(pince.emperagecoral() > 60){
|
|
||||||
pince.aspirecoral(0);
|
|
||||||
}
|
|
||||||
*/
|
|
||||||
pince.aspirecoral(-.5);
|
|
||||||
bougie.Jaune();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
pince.aspirecoral(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,47 +0,0 @@
|
|||||||
// 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.commands.Pince;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class DepartPince extends Command {
|
|
||||||
private Pince pince;
|
|
||||||
/** Creates a new DepartPince. */
|
|
||||||
public DepartPince(Pince pince) {
|
|
||||||
this.pince = pince;
|
|
||||||
addRequirements(pince);
|
|
||||||
// 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() {
|
|
||||||
if(pince.position()==true){
|
|
||||||
pince.pivote(0);
|
|
||||||
pince.reset();
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.pivote(-0.2);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,50 +0,0 @@
|
|||||||
// 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.commands.Pince;
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class PinceManuel extends Command {
|
|
||||||
private Pince pince;
|
|
||||||
private DoubleSupplier x;
|
|
||||||
/** Creates a new PinceManuel. */
|
|
||||||
public PinceManuel(Pince pince, DoubleSupplier x) {
|
|
||||||
this.pince = pince;
|
|
||||||
this.x = x;
|
|
||||||
//this.doubleSupplier = doubleSupplier;
|
|
||||||
addRequirements(pince);
|
|
||||||
// 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() {
|
|
||||||
if(pince.position()){
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pince.pivote(x.getAsDouble()/3.5);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
pince.pivote(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,37 +0,0 @@
|
|||||||
// 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.commands;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class RainBow extends Command {
|
|
||||||
/** Creates a new RainBow. */
|
|
||||||
private Bougie bougie;
|
|
||||||
public RainBow(Bougie bougie) {
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(bougie);
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {bougie.RainBow();}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {bougie.RainBowStop();}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,67 +0,0 @@
|
|||||||
// 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.commands.requin;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class BalayeuseAlgue extends Command {
|
|
||||||
private Requin requin;
|
|
||||||
private Bougie bougie;
|
|
||||||
/** Creates a new Balayeuse. */
|
|
||||||
public BalayeuseAlgue(Requin requin, Bougie bougie) {
|
|
||||||
this.requin = requin;
|
|
||||||
this.bougie =bougie;
|
|
||||||
addRequirements(requin,bougie);
|
|
||||||
// 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() {
|
|
||||||
double cibleMin = 550;
|
|
||||||
double cibleMax = 650;
|
|
||||||
|
|
||||||
if(requin.amp()>=78.2){
|
|
||||||
requin.xRequin = true;
|
|
||||||
}
|
|
||||||
if(requin.xRequin){
|
|
||||||
bougie.Vert();
|
|
||||||
requin.balaye(0);
|
|
||||||
}
|
|
||||||
if(!requin.xRequin){
|
|
||||||
if(requin.encodeur()<=cibleMax && requin.encodeur()>=cibleMin){
|
|
||||||
requin.rotationer(0);
|
|
||||||
requin.balaye(-0.4);
|
|
||||||
}
|
|
||||||
else if(requin.encodeur()>=cibleMax){
|
|
||||||
requin.rotationer(-0.1);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
requin.rotationer(0.3);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
requin.rotationer(0);
|
|
||||||
requin.balaye(0);
|
|
||||||
requin.xRequin = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,40 +0,0 @@
|
|||||||
// 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.commands.requin;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class BalayeuseBas extends Command {
|
|
||||||
private Requin requin;
|
|
||||||
/** Creates a new Balayeuse. */
|
|
||||||
public BalayeuseBas(Requin requin) {
|
|
||||||
this.requin = requin;
|
|
||||||
addRequirements(requin);
|
|
||||||
// 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() {
|
|
||||||
requin.rotationer(0.2);
|
|
||||||
}
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
requin.rotationer(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,66 +0,0 @@
|
|||||||
// 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.commands.requin;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class BalayeuseCoral extends Command {
|
|
||||||
private Requin requin;
|
|
||||||
private Bougie bougie;
|
|
||||||
/** Creates a new Balayeuse. */
|
|
||||||
public BalayeuseCoral(Requin requin, Bougie bougie) {
|
|
||||||
this.requin = requin;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(requin,bougie);
|
|
||||||
// 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() {
|
|
||||||
double cibleMax = 1100;
|
|
||||||
double cibleMin = 900;
|
|
||||||
if(requin.amp()>=78.2){
|
|
||||||
requin.xRequin = true;
|
|
||||||
}
|
|
||||||
if(requin.xRequin){
|
|
||||||
requin.balaye(0);
|
|
||||||
bougie.Vert();
|
|
||||||
}
|
|
||||||
if (!requin.xRequin) {
|
|
||||||
if(requin.encodeur()<=cibleMax && requin.encodeur()>=cibleMin){
|
|
||||||
requin.rotationer(0);
|
|
||||||
requin.balaye(0.7);
|
|
||||||
}
|
|
||||||
else if(requin.encodeur()>=cibleMax){
|
|
||||||
requin.rotationer(-0.5);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
requin.rotationer(0.5);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
requin.rotationer(0);
|
|
||||||
requin.balaye(0);
|
|
||||||
requin.xRequin = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,47 +0,0 @@
|
|||||||
// 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.commands.requin;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class BalayeuseHaut extends Command {
|
|
||||||
private Requin requin;
|
|
||||||
/** Creates a new Balayeuse. */
|
|
||||||
public BalayeuseHaut(Requin requin) {
|
|
||||||
this.requin = requin;
|
|
||||||
addRequirements(requin);
|
|
||||||
// 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() {
|
|
||||||
if(requin.enHaut()==true){
|
|
||||||
requin.rotationer(0);
|
|
||||||
requin.reset();
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
requin.rotationer(-0.5);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
requin.rotationer(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,51 +0,0 @@
|
|||||||
// 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.commands.requin;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class ExpireCorail extends Command {
|
|
||||||
private Requin requin;
|
|
||||||
private Bougie bougie;
|
|
||||||
/** Creates a new ExpireAlgue. */
|
|
||||||
public ExpireCorail(Requin requin, Bougie bougie) {
|
|
||||||
this.requin = requin;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(requin,bougie);
|
|
||||||
// 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() {
|
|
||||||
if(requin.amp()> 60){
|
|
||||||
requin.balaye(-0.1);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
bougie.Rouge();
|
|
||||||
requin.balaye(-0.1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
requin.balaye(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,53 +0,0 @@
|
|||||||
// 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.commands.requin;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class L1Requin extends Command {
|
|
||||||
private Requin requin;
|
|
||||||
private Bougie bougie;
|
|
||||||
/** Creates a new Balayeuse. */
|
|
||||||
public L1Requin(Requin requin,Bougie bougie) {
|
|
||||||
this.requin = requin;
|
|
||||||
this.bougie = bougie;
|
|
||||||
addRequirements(requin,bougie);
|
|
||||||
// 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() {
|
|
||||||
if(requin.encodeur()<=485 && requin.encodeur()>=385){
|
|
||||||
requin.rotationer(0);
|
|
||||||
}
|
|
||||||
else if(requin.encodeur()>=485){
|
|
||||||
requin.rotationer(-0.5);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
requin.rotationer(0.5);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
requin.rotationer(0);
|
|
||||||
requin.balaye(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,53 +0,0 @@
|
|||||||
// 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.commands.requin;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Bougie;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class exspire extends Command {
|
|
||||||
/** Creates a new aspire. */
|
|
||||||
private Requin requin;
|
|
||||||
private Bougie bougie
|
|
||||||
;
|
|
||||||
public exspire(Requin requin,Bougie bougie) {
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
this.requin = requin;
|
|
||||||
this.bougie
|
|
||||||
=bougie;
|
|
||||||
addRequirements(requin,bougie);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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() {
|
|
||||||
if(requin.amp()> 15)
|
|
||||||
{
|
|
||||||
requin.balaye(0.2);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
bougie.Rouge();
|
|
||||||
requin.balaye(0.2);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
requin.balaye(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,49 +0,0 @@
|
|||||||
// 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.commands.requin;
|
|
||||||
|
|
||||||
import java.util.function.DoubleSupplier;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class requin_manuel extends Command {
|
|
||||||
/** Creates a new requin_manuel. */
|
|
||||||
private Requin requin;
|
|
||||||
private DoubleSupplier x;
|
|
||||||
public requin_manuel(Requin requin) {
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
this.requin = requin;
|
|
||||||
addRequirements(requin);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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() {
|
|
||||||
if(requin.enHaut()){
|
|
||||||
requin.rotationer(0);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
requin.rotationer(x.getAsDouble());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
requin.rotationer(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,47 +0,0 @@
|
|||||||
// 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.commands;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc.robot.subsystems.Elevateur;
|
|
||||||
import frc.robot.subsystems.Pince;
|
|
||||||
import frc.robot.subsystems.Requin;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class reset extends Command {
|
|
||||||
/** Creates a new reset. */
|
|
||||||
private Elevateur elevateur;
|
|
||||||
private Pince pince;
|
|
||||||
private Requin requin;
|
|
||||||
public reset(Elevateur elevateur, Pince pince, Requin requin) {
|
|
||||||
// Use addRequirements() here to declare subsystem dependencies.
|
|
||||||
this.elevateur = elevateur;
|
|
||||||
this.pince = pince;
|
|
||||||
this.requin = requin;
|
|
||||||
addRequirements(elevateur,pince, requin);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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() {
|
|
||||||
elevateur.reset();
|
|
||||||
pince.reset();
|
|
||||||
requin.reset();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,78 +0,0 @@
|
|||||||
// 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.subsystems;
|
|
||||||
|
|
||||||
import com.ctre.phoenix.led.CANdle;
|
|
||||||
import com.ctre.phoenix.led.CANdleConfiguration;
|
|
||||||
import com.ctre.phoenix.led.FireAnimation;
|
|
||||||
import com.ctre.phoenix.led.LarsonAnimation;
|
|
||||||
import com.ctre.phoenix.led.RainbowAnimation;
|
|
||||||
import com.ctre.phoenix.led.TwinkleAnimation;
|
|
||||||
import com.ctre.phoenix.led.TwinkleOffAnimation;
|
|
||||||
import com.ctre.phoenix.led.TwinkleOffAnimation.TwinkleOffPercent;
|
|
||||||
import com.ctre.phoenix.led.LarsonAnimation.BounceMode;
|
|
||||||
import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
|
|
||||||
public class Bougie extends SubsystemBase {
|
|
||||||
CANdle candle = new CANdle(23);
|
|
||||||
CANdleConfiguration config = new CANdleConfiguration();
|
|
||||||
LarsonAnimation rainbowAnim = new LarsonAnimation(256,0,0,0,0.1,68,BounceMode.Front,10,8);
|
|
||||||
//TwinkleOffAnimation rainbowAnim = new TwinkleOffAnimation(256, 0, 0,0,0.5,68,TwinkleOffPercent.Percent88,8);
|
|
||||||
/** Creates a new Bougie. */
|
|
||||||
public Bougie() {
|
|
||||||
config.brightnessScalar = 0.5;
|
|
||||||
candle.configAllSettings(config);
|
|
||||||
}
|
|
||||||
public void Rouge() {
|
|
||||||
candle.setLEDs(255, 0, 0,0,8,8);
|
|
||||||
candle.setLEDs(255, 0, 0,0,24,8);
|
|
||||||
candle.setLEDs(255, 0, 0,0,40,8);
|
|
||||||
candle.setLEDs(255, 0, 0,0,56,8);
|
|
||||||
candle.setLEDs(255, 0, 0,0,72,8);
|
|
||||||
candle.setLEDs(255, 0, 0,0,88,8);
|
|
||||||
candle.setLEDs(255, 0, 0,0,104,8);
|
|
||||||
candle.setLEDs(255, 0, 0,0,120,8);
|
|
||||||
}
|
|
||||||
public void Vert() {
|
|
||||||
candle.setLEDs(0, 255, 0,0,8,8);
|
|
||||||
candle.setLEDs(0, 255, 0,0,24,8);
|
|
||||||
candle.setLEDs(0, 255, 0,0,40,8);
|
|
||||||
candle.setLEDs(0, 255, 0,0,56,8);
|
|
||||||
candle.setLEDs(0, 255, 0,0,72,8);
|
|
||||||
candle.setLEDs(0, 255, 0,0,88,8);
|
|
||||||
candle.setLEDs(0, 255, 0,0,104,8);
|
|
||||||
candle.setLEDs(0, 255, 0,0,120,8);
|
|
||||||
}
|
|
||||||
public void Bleu() {
|
|
||||||
candle.setLEDs(0, 0, 255,0,16,8);
|
|
||||||
candle.setLEDs(0, 0, 255,0,32,8);
|
|
||||||
candle.setLEDs(0, 0, 255,0,48,8);
|
|
||||||
candle.setLEDs(0, 0, 255,0,64,8);
|
|
||||||
candle.setLEDs(0, 0, 255,0,80,8);
|
|
||||||
candle.setLEDs(0, 0, 255,0,96,8);
|
|
||||||
candle.setLEDs(0, 0, 255,0,112,8);
|
|
||||||
}
|
|
||||||
public void Jaune() {
|
|
||||||
candle.setLEDs(255, 215, 0,0,16,8);
|
|
||||||
candle.setLEDs(255, 215, 0,0,32,8);
|
|
||||||
candle.setLEDs(255, 215, 0,0,48,8);
|
|
||||||
candle.setLEDs(255, 215, 0,0,64,8);
|
|
||||||
candle.setLEDs(255, 215, 0,0,80,8);
|
|
||||||
candle.setLEDs(255, 215, 0,0,96,8);
|
|
||||||
candle.setLEDs(255, 215, 0,0,112,8);
|
|
||||||
}
|
|
||||||
public void RainBow(){
|
|
||||||
candle.animate(rainbowAnim);
|
|
||||||
}
|
|
||||||
public void RainBowStop(){
|
|
||||||
candle.animate(null);
|
|
||||||
}
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
// This method will be called once per scheduler run
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,211 +0,0 @@
|
|||||||
package frc.robot.subsystems;
|
|
||||||
|
|
||||||
import java.util.function.Supplier;
|
|
||||||
import com.ctre.phoenix6.Utils;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
|
||||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
||||||
import com.pathplanner.lib.auto.AutoBuilder;
|
|
||||||
import com.pathplanner.lib.config.PIDConstants;
|
|
||||||
import com.pathplanner.lib.config.RobotConfig;
|
|
||||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
|
||||||
import edu.wpi.first.math.Matrix;
|
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
|
||||||
import edu.wpi.first.math.numbers.N1;
|
|
||||||
import edu.wpi.first.math.numbers.N3;
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
||||||
import edu.wpi.first.wpilibj.Notifier;
|
|
||||||
import edu.wpi.first.wpilibj.RobotController;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Subsystem;
|
|
||||||
import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
|
|
||||||
* Subsystem so it can easily be used in command-based projeScts.
|
|
||||||
*/
|
|
||||||
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
|
|
||||||
|
|
||||||
private static final double kSimLoopPeriod = 0.005; // 5 ms
|
|
||||||
private Notifier m_simNotifier = null;
|
|
||||||
private double m_lastSimTime;
|
|
||||||
|
|
||||||
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
|
|
||||||
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
|
|
||||||
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
|
|
||||||
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
|
|
||||||
/* Keep track if we've ever applied the operator perspective before or not */
|
|
||||||
private boolean m_hasAppliedOperatorPerspective = false;
|
|
||||||
|
|
||||||
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
|
|
||||||
|
|
||||||
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
|
|
||||||
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
|
|
||||||
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
|
|
||||||
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
|
|
||||||
|
|
||||||
// private void configureAutoBuilder() {
|
|
||||||
// try {
|
|
||||||
// var config = RobotConfig.fromGUISettings();
|
|
||||||
// AutoBuilder.configure(
|
|
||||||
|
|
||||||
// () -> getState().Pose, // Supplier of current robot pose
|
|
||||||
// this::resetPose, // Consumer for seeding pose against auto
|
|
||||||
// () -> getState().Speeds, // Supplier of current robot speeds
|
|
||||||
|
|
||||||
// // Consumer of ChassisSpeeds and feedforwards to drive the robot
|
|
||||||
// (speeds, feedforwards) -> setControl(
|
|
||||||
// m_pathApplyRobotSpeeds.withSpeeds(speeds)
|
|
||||||
// .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
|
|
||||||
// .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
|
|
||||||
// ),
|
|
||||||
// new PPHolonomicDriveController(
|
|
||||||
// // PID constants for translation
|
|
||||||
// new PIDConstants(63.167, 0, 0.54521),
|
|
||||||
// // // PID constants for rotation
|
|
||||||
// // new PIDConstants(7.9735, 0, 0.038499)
|
|
||||||
// // PID constants for rotation
|
|
||||||
// new PIDConstants(43.502,0,2.7353)
|
|
||||||
// ),
|
|
||||||
// config,
|
|
||||||
// // Assume the path needs to be flipped for Red vs Blue, this is normally the case
|
|
||||||
// () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
|
|
||||||
// this // Subsystem for requirements
|
|
||||||
|
|
||||||
// );
|
|
||||||
// } catch (Exception ex) {
|
|
||||||
// DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
|
|
||||||
// }
|
|
||||||
// PPHolonomicDriveController.overrideRotationFeedback(()->{
|
|
||||||
// return 0;
|
|
||||||
// });
|
|
||||||
// }
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
||||||
* <p>
|
|
||||||
* This constructs the underlying hardware devices, so users should not construct
|
|
||||||
* the devices themselves. If they need the devices, they can access them through
|
|
||||||
* getters in the classes.
|
|
||||||
*
|
|
||||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
||||||
* @param modules Constants for each specific module
|
|
||||||
*/
|
|
||||||
public CommandSwerveDrivetrain(
|
|
||||||
SwerveDrivetrainConstants drivetrainConstants,
|
|
||||||
SwerveModuleConstants<?, ?, ?>... modules
|
|
||||||
) {
|
|
||||||
super(drivetrainConstants, modules);
|
|
||||||
if (Utils.isSimulation()) {
|
|
||||||
startSimThread();
|
|
||||||
}
|
|
||||||
//configureAutoBuilder();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
||||||
* <p>
|
|
||||||
* This constructs the underlying hardware devices, so users should not construct
|
|
||||||
* the devices themselves. If they need the devices, they can access them through
|
|
||||||
* getters in the classes.
|
|
||||||
*
|
|
||||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
||||||
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
|
||||||
* unspecified or set to 0 Hz, this is 250 Hz on
|
|
||||||
* CAN FD, and 100 Hz on CAN 2.0.
|
|
||||||
* @param modules Constants for each specific module
|
|
||||||
*/
|
|
||||||
public CommandSwerveDrivetrain(
|
|
||||||
SwerveDrivetrainConstants drivetrainConstants,
|
|
||||||
double odometryUpdateFrequency,
|
|
||||||
SwerveModuleConstants<?, ?, ?>... modules
|
|
||||||
) {
|
|
||||||
super(drivetrainConstants, odometryUpdateFrequency, modules);
|
|
||||||
if (Utils.isSimulation()) {
|
|
||||||
startSimThread();
|
|
||||||
}
|
|
||||||
// configureAutoBuilder();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
||||||
* <p>
|
|
||||||
* This constructs the underlying hardware devices, so users should not construct
|
|
||||||
* the devices themselves. If they need the devices, they can access them through
|
|
||||||
* getters in the classes.
|
|
||||||
*
|
|
||||||
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
||||||
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
|
||||||
* unspecified or set to 0 Hz, this is 250 Hz on
|
|
||||||
* CAN FD, and 100 Hz on CAN 2.0.
|
|
||||||
* @param odometryStandardDeviation The standard deviation for odometry calculation
|
|
||||||
* in the form [x, y, theta]ᵀ, with units in meters
|
|
||||||
* and radians
|
|
||||||
* @param visionStandardDeviation The standard deviation for vision calculation
|
|
||||||
* in the form [x, y, theta]ᵀ, with units in meters
|
|
||||||
* and radians
|
|
||||||
* @param modules Constants for each specific module
|
|
||||||
*/
|
|
||||||
public CommandSwerveDrivetrain(
|
|
||||||
SwerveDrivetrainConstants drivetrainConstants,
|
|
||||||
double odometryUpdateFrequency,
|
|
||||||
Matrix<N3, N1> odometryStandardDeviation,
|
|
||||||
Matrix<N3, N1> visionStandardDeviation,
|
|
||||||
SwerveModuleConstants<?, ?, ?>... modules
|
|
||||||
) {
|
|
||||||
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
|
|
||||||
if (Utils.isSimulation()) {
|
|
||||||
startSimThread();
|
|
||||||
}
|
|
||||||
//configureAutoBuilder();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Returns a command that applies the specified control request to this swerve drivetrain.
|
|
||||||
*
|
|
||||||
* @param request Function returning the request to apply
|
|
||||||
* @return Command to run
|
|
||||||
*/
|
|
||||||
public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
|
|
||||||
return run(() -> this.setControl(requestSupplier.get()));
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
/*
|
|
||||||
* Periodically try to apply the operator perspective.
|
|
||||||
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
|
|
||||||
* This allows us to correct the perspective in case the robot code restarts mid-match.
|
|
||||||
* Otherwise, only check and apply the operator perspective if the DS is disabled.
|
|
||||||
* This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
|
|
||||||
*/
|
|
||||||
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
|
|
||||||
DriverStation.getAlliance().ifPresent(allianceColor -> {
|
|
||||||
setOperatorPerspectiveForward(
|
|
||||||
allianceColor == Alliance.Red
|
|
||||||
? kRedAlliancePerspectiveRotation
|
|
||||||
: kBlueAlliancePerspectiveRotation
|
|
||||||
);
|
|
||||||
m_hasAppliedOperatorPerspective = true;
|
|
||||||
});
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
private void startSimThread() {
|
|
||||||
m_lastSimTime = Utils.getCurrentTimeSeconds();
|
|
||||||
|
|
||||||
/* Run simulation at a faster rate so PID gains behave more reasonably */
|
|
||||||
m_simNotifier = new Notifier(() -> {
|
|
||||||
final double currentTime = Utils.getCurrentTimeSeconds();
|
|
||||||
double deltaTime = currentTime - m_lastSimTime;
|
|
||||||
m_lastSimTime = currentTime;
|
|
||||||
|
|
||||||
/* use the measured time delta, get battery voltage from WPILib */
|
|
||||||
updateSimState(deltaTime, RobotController.getBatteryVoltage());
|
|
||||||
});
|
|
||||||
m_simNotifier.startPeriodic(kSimLoopPeriod);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,98 +0,0 @@
|
|||||||
// 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.subsystems;
|
|
||||||
import edu.wpi.first.networktables.GenericEntry;
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
import com.revrobotics.spark.SparkMax;
|
|
||||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
||||||
public class Elevateur extends SubsystemBase {
|
|
||||||
ShuffleboardTab teb = Shuffleboard.getTab("teb");
|
|
||||||
/** Creates a new Elevateur. */
|
|
||||||
private GenericEntry encodeurelevateurL2bas =
|
|
||||||
teb.add("encodeurelevateurL2bas", -1).getEntry();
|
|
||||||
private GenericEntry encodeurelevateurL2haut =
|
|
||||||
teb.add("encodeurelevateurL2haut", -0.9).getEntry();
|
|
||||||
private GenericEntry encodeurelevateurL3bas =
|
|
||||||
teb.add("encodeurelevateurL3bas", -3).getEntry();
|
|
||||||
private GenericEntry encodeurelevateurL3haut =
|
|
||||||
teb.add("encodeurelevateurL3haut", -3.1).getEntry();
|
|
||||||
private GenericEntry encodeurelevateurL4bas =
|
|
||||||
teb.add("encodeurelevateurL4bas", -6.3).getEntry();
|
|
||||||
private GenericEntry encodeurelevateurL4haut =
|
|
||||||
teb.add("encodeurelevateurL4haut", -6.5).getEntry();
|
|
||||||
private GenericEntry encodeurelevateurstationbas =
|
|
||||||
teb.add("encodeurelevateursationbas", -0.5).getEntry();
|
|
||||||
private GenericEntry encodeurelevateurstationhaut =
|
|
||||||
teb.add("encodeurelevateursationhaut", -0.4).getEntry();
|
|
||||||
private GenericEntry distanceDeploiePince =
|
|
||||||
teb.add("encodeurDeploiePince", 0.2).getEntry();
|
|
||||||
|
|
||||||
public Elevateur() {
|
|
||||||
teb.addDouble("encodeur elevateur",this::position);
|
|
||||||
teb.addBoolean("limit elevateur", this::limit2);
|
|
||||||
}
|
|
||||||
final SparkMax monte = new SparkMax(22, MotorType.kBrushless);
|
|
||||||
final DigitalInput limit2 = new DigitalInput(0);
|
|
||||||
|
|
||||||
public double position(){
|
|
||||||
return monte.getEncoder().getPosition();
|
|
||||||
}
|
|
||||||
|
|
||||||
public void vitesse(double vitesse){
|
|
||||||
if (limit2()) {
|
|
||||||
if (vitesse > 0) {
|
|
||||||
monte.set(0);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
monte.set(vitesse);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
monte.set(vitesse);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
public boolean limit2(){
|
|
||||||
return limit2.get();
|
|
||||||
}
|
|
||||||
public void reset(){
|
|
||||||
monte.getEncoder().setPosition(0);
|
|
||||||
}
|
|
||||||
public double encodeurelevateurL2bas(){
|
|
||||||
return encodeurelevateurL2bas.getDouble(-1);
|
|
||||||
}
|
|
||||||
public double encodeurelevateurL2haut(){
|
|
||||||
return encodeurelevateurL2haut.getDouble(-0.9);
|
|
||||||
}
|
|
||||||
public double encodeurelevateurL3bas(){
|
|
||||||
return encodeurelevateurL3bas.getDouble(-2.8);
|
|
||||||
}
|
|
||||||
public double encodeurelevateurL3haut(){
|
|
||||||
return encodeurelevateurL3haut.getDouble(-3);
|
|
||||||
}
|
|
||||||
public double encodeurelevateurL4bas(){
|
|
||||||
return encodeurelevateurL4bas.getDouble(-6.3);
|
|
||||||
}
|
|
||||||
public double encodeurelevateurL4haut(){
|
|
||||||
return encodeurelevateurL4haut.getDouble(-6.5);
|
|
||||||
}
|
|
||||||
public double encodeurelevateurstationbas(){
|
|
||||||
return encodeurelevateurstationbas.getDouble(-0.5);
|
|
||||||
}
|
|
||||||
public double encodeurelevateurstationhaut(){
|
|
||||||
return encodeurelevateurstationhaut.getDouble(-0.4);
|
|
||||||
}
|
|
||||||
public double distanceDeploiePince(){
|
|
||||||
return distanceDeploiePince.getDouble(0.2);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
// This method will be called once per scheduler run
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,39 +0,0 @@
|
|||||||
// 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.subsystems;
|
|
||||||
|
|
||||||
import edu.wpi.first.net.PortForwarder;
|
|
||||||
import edu.wpi.first.networktables.NetworkTable;
|
|
||||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
|
||||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
import frc.robot.LimelightHelpers;
|
|
||||||
|
|
||||||
public class Limelight3 extends SubsystemBase {
|
|
||||||
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
||||||
NetworkTableEntry pipeline = table.getEntry("pipeline");
|
|
||||||
/** Creates a new Limelight3. */
|
|
||||||
public Limelight3() {
|
|
||||||
for(int port = 5800; port <=5807; port++){
|
|
||||||
PortForwarder.add(port, "limelight.local", port);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
public double getX(){
|
|
||||||
return LimelightHelpers.getTX("limelight-balon");
|
|
||||||
}
|
|
||||||
public boolean getV(){
|
|
||||||
return LimelightHelpers.getTV("limelight-balon");
|
|
||||||
}
|
|
||||||
public void Apriltag(){
|
|
||||||
pipeline.setNumber(1);
|
|
||||||
}
|
|
||||||
public void Forme(){
|
|
||||||
pipeline.setNumber(0);
|
|
||||||
}
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
// This method will be called once per scheduler run
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,28 +0,0 @@
|
|||||||
// 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.subsystems;
|
|
||||||
|
|
||||||
import edu.wpi.first.net.PortForwarder;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
import frc.robot.LimelightHelpers;
|
|
||||||
|
|
||||||
public class Limelight3G extends SubsystemBase {
|
|
||||||
/** Creates a new Limelight3. */
|
|
||||||
public Limelight3G() {
|
|
||||||
for(int port = 5800; port <=5807; port++){
|
|
||||||
PortForwarder.add(port, "limelight.local", port);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
public double getX(){
|
|
||||||
return LimelightHelpers.getTX("limelight-tag");
|
|
||||||
}
|
|
||||||
public boolean getV(){
|
|
||||||
return LimelightHelpers.getTV("limelight-tag");
|
|
||||||
}
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
// This method will be called once per scheduler run
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,71 +0,0 @@
|
|||||||
// 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.subsystems;
|
|
||||||
|
|
||||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
||||||
import com.revrobotics.spark.SparkMax;
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
|
|
||||||
public class Pince extends SubsystemBase {
|
|
||||||
/** Creates a new Pince. */
|
|
||||||
ShuffleboardTab teb = Shuffleboard.getTab("teb");
|
|
||||||
public Pince() {
|
|
||||||
teb.addBoolean("limit pince",this::position);
|
|
||||||
teb.addDouble("encodeur pince", this::encodeurpivot);
|
|
||||||
teb.addDouble("amperage corail", this::emperagecoral);
|
|
||||||
teb.addDouble("amperage algue", this::emperagealgue);
|
|
||||||
}
|
|
||||||
final SparkMax coral = new SparkMax(20, MotorType.kBrushless);
|
|
||||||
final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless);
|
|
||||||
final SparkMax algue1 = new SparkMax(16, MotorType.kBrushless);
|
|
||||||
final SparkMax algue2 = new SparkMax(19, MotorType.kBrushless);
|
|
||||||
final DigitalInput limit6 = new DigitalInput(9);
|
|
||||||
|
|
||||||
|
|
||||||
public void aspirecoral(double vitesse){
|
|
||||||
coral.set(vitesse);
|
|
||||||
}
|
|
||||||
public void pivote(double vitesse){
|
|
||||||
if (position()) {
|
|
||||||
if (vitesse < 0) {
|
|
||||||
pivoti.set(0);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pivoti.set(vitesse);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
pivoti.set(vitesse);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
public void aspirealgue(double vitesse){
|
|
||||||
algue2.set(-vitesse);
|
|
||||||
algue1.set(-vitesse);
|
|
||||||
}
|
|
||||||
public double encodeurpivot(){
|
|
||||||
return pivoti.getEncoder().getPosition();
|
|
||||||
}
|
|
||||||
public boolean position(){
|
|
||||||
return limit6.get();
|
|
||||||
}
|
|
||||||
public void reset(){
|
|
||||||
pivoti.getEncoder().setPosition(0);
|
|
||||||
}
|
|
||||||
public double emperagecoral(){
|
|
||||||
return coral.getOutputCurrent();
|
|
||||||
}
|
|
||||||
public double emperagealgue(){
|
|
||||||
return algue1.getOutputCurrent();
|
|
||||||
}
|
|
||||||
public boolean x = false;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
// This method will be called once per scheduler run
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,51 +0,0 @@
|
|||||||
// 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.subsystems;
|
|
||||||
|
|
||||||
import com.revrobotics.spark.SparkFlex;
|
|
||||||
import com.revrobotics.spark.SparkMax;
|
|
||||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
||||||
import edu.wpi.first.wpilibj.DigitalInput;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
||||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
||||||
|
|
||||||
public class Requin extends SubsystemBase {
|
|
||||||
/** Creates a new Requin. */
|
|
||||||
ShuffleboardTab teb = Shuffleboard.getTab("teb");
|
|
||||||
public Requin() {
|
|
||||||
teb.addBoolean("limit requin", this::enHaut);
|
|
||||||
teb.addDouble("amparge requin", this::amp);
|
|
||||||
teb.addDouble("encodeur requin", this::encodeur);
|
|
||||||
}
|
|
||||||
|
|
||||||
final SparkFlex balaye = new SparkFlex(15, MotorType.kBrushless);
|
|
||||||
final SparkMax rotatione = new SparkMax(17, MotorType.kBrushless);
|
|
||||||
final DigitalInput limit3 = new DigitalInput(1);
|
|
||||||
|
|
||||||
public void balaye(double vitesse){
|
|
||||||
balaye.set(vitesse);
|
|
||||||
}
|
|
||||||
public void rotationer(double vitesse){
|
|
||||||
rotatione.set(vitesse);
|
|
||||||
}
|
|
||||||
public boolean enHaut(){
|
|
||||||
return limit3.get();
|
|
||||||
}
|
|
||||||
public double encodeur(){
|
|
||||||
return rotatione.getEncoder().getPosition();
|
|
||||||
}
|
|
||||||
public void reset(){
|
|
||||||
rotatione.getEncoder().setPosition(0);
|
|
||||||
}
|
|
||||||
public double amp(){
|
|
||||||
return balaye.getOutputCurrent();
|
|
||||||
}
|
|
||||||
public boolean xRequin = false;
|
|
||||||
@Override
|
|
||||||
public void periodic() {
|
|
||||||
// This method will be called once per scheduler run
|
|
||||||
}
|
|
||||||
}
|
|
||||||
File diff suppressed because one or more lines are too long
@ -1,7 +1,7 @@
|
|||||||
{
|
{
|
||||||
"fileName": "PathplannerLib-2025.2.3.json",
|
"fileName": "PathplannerLib-2025.2.2.json",
|
||||||
"name": "PathplannerLib",
|
"name": "PathplannerLib",
|
||||||
"version": "2025.2.3",
|
"version": "2025.2.2",
|
||||||
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
||||||
"frcYear": "2025",
|
"frcYear": "2025",
|
||||||
"mavenUrls": [
|
"mavenUrls": [
|
||||||
@ -12,7 +12,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.pathplanner.lib",
|
"groupId": "com.pathplanner.lib",
|
||||||
"artifactId": "PathplannerLib-java",
|
"artifactId": "PathplannerLib-java",
|
||||||
"version": "2025.2.3"
|
"version": "2025.2.2"
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"jniDependencies": [],
|
"jniDependencies": [],
|
||||||
@ -20,7 +20,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.pathplanner.lib",
|
"groupId": "com.pathplanner.lib",
|
||||||
"artifactId": "PathplannerLib-cpp",
|
"artifactId": "PathplannerLib-cpp",
|
||||||
"version": "2025.2.3",
|
"version": "2025.2.2",
|
||||||
"libName": "PathplannerLib",
|
"libName": "PathplannerLib",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": false,
|
"sharedLibrary": false,
|
||||||
@ -1,7 +1,7 @@
|
|||||||
{
|
{
|
||||||
"fileName": "Phoenix6-25.2.2.json",
|
"fileName": "Phoenix6-25.2.1.json",
|
||||||
"name": "CTRE-Phoenix (v6)",
|
"name": "CTRE-Phoenix (v6)",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"frcYear": "2025",
|
"frcYear": "2025",
|
||||||
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
|
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
|
||||||
"mavenUrls": [
|
"mavenUrls": [
|
||||||
@ -19,14 +19,14 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6",
|
"groupId": "com.ctre.phoenix6",
|
||||||
"artifactId": "wpiapi-java",
|
"artifactId": "wpiapi-java",
|
||||||
"version": "25.2.2"
|
"version": "25.2.1"
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"jniDependencies": [
|
"jniDependencies": [
|
||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6",
|
"groupId": "com.ctre.phoenix6",
|
||||||
"artifactId": "api-cpp",
|
"artifactId": "api-cpp",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -40,7 +40,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6",
|
"groupId": "com.ctre.phoenix6",
|
||||||
"artifactId": "tools",
|
"artifactId": "tools",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -54,7 +54,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "api-cpp-sim",
|
"artifactId": "api-cpp-sim",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -68,7 +68,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "tools-sim",
|
"artifactId": "tools-sim",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -82,7 +82,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simTalonSRX",
|
"artifactId": "simTalonSRX",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -96,7 +96,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simVictorSPX",
|
"artifactId": "simVictorSPX",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -110,7 +110,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simPigeonIMU",
|
"artifactId": "simPigeonIMU",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -124,7 +124,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simCANCoder",
|
"artifactId": "simCANCoder",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -138,7 +138,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProTalonFX",
|
"artifactId": "simProTalonFX",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -152,7 +152,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProTalonFXS",
|
"artifactId": "simProTalonFXS",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -166,7 +166,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProCANcoder",
|
"artifactId": "simProCANcoder",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -180,7 +180,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProPigeon2",
|
"artifactId": "simProPigeon2",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -194,7 +194,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProCANrange",
|
"artifactId": "simProCANrange",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"isJar": false,
|
"isJar": false,
|
||||||
"skipInvalidPlatforms": true,
|
"skipInvalidPlatforms": true,
|
||||||
"validPlatforms": [
|
"validPlatforms": [
|
||||||
@ -210,7 +210,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6",
|
"groupId": "com.ctre.phoenix6",
|
||||||
"artifactId": "wpiapi-cpp",
|
"artifactId": "wpiapi-cpp",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_Phoenix6_WPI",
|
"libName": "CTRE_Phoenix6_WPI",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -226,7 +226,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6",
|
"groupId": "com.ctre.phoenix6",
|
||||||
"artifactId": "tools",
|
"artifactId": "tools",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_PhoenixTools",
|
"libName": "CTRE_PhoenixTools",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -242,7 +242,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "wpiapi-cpp-sim",
|
"artifactId": "wpiapi-cpp-sim",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_Phoenix6_WPISim",
|
"libName": "CTRE_Phoenix6_WPISim",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -258,7 +258,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "tools-sim",
|
"artifactId": "tools-sim",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_PhoenixTools_Sim",
|
"libName": "CTRE_PhoenixTools_Sim",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -274,7 +274,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simTalonSRX",
|
"artifactId": "simTalonSRX",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimTalonSRX",
|
"libName": "CTRE_SimTalonSRX",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -290,7 +290,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simVictorSPX",
|
"artifactId": "simVictorSPX",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimVictorSPX",
|
"libName": "CTRE_SimVictorSPX",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -306,7 +306,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simPigeonIMU",
|
"artifactId": "simPigeonIMU",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimPigeonIMU",
|
"libName": "CTRE_SimPigeonIMU",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -322,7 +322,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simCANCoder",
|
"artifactId": "simCANCoder",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimCANCoder",
|
"libName": "CTRE_SimCANCoder",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -338,7 +338,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProTalonFX",
|
"artifactId": "simProTalonFX",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimProTalonFX",
|
"libName": "CTRE_SimProTalonFX",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -354,7 +354,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProTalonFXS",
|
"artifactId": "simProTalonFXS",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimProTalonFXS",
|
"libName": "CTRE_SimProTalonFXS",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -370,7 +370,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProCANcoder",
|
"artifactId": "simProCANcoder",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimProCANcoder",
|
"libName": "CTRE_SimProCANcoder",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -386,7 +386,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProPigeon2",
|
"artifactId": "simProPigeon2",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimProPigeon2",
|
"libName": "CTRE_SimProPigeon2",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
@ -402,7 +402,7 @@
|
|||||||
{
|
{
|
||||||
"groupId": "com.ctre.phoenix6.sim",
|
"groupId": "com.ctre.phoenix6.sim",
|
||||||
"artifactId": "simProCANrange",
|
"artifactId": "simProCANrange",
|
||||||
"version": "25.2.2",
|
"version": "25.2.1",
|
||||||
"libName": "CTRE_SimProCANrange",
|
"libName": "CTRE_SimProCANrange",
|
||||||
"headerClassifier": "headers",
|
"headerClassifier": "headers",
|
||||||
"sharedLibrary": true,
|
"sharedLibrary": true,
|
||||||
Loading…
x
Reference in New Issue
Block a user