diff --git a/src/main/deploy/pathplanner/autos/1.auto b/src/main/deploy/pathplanner/autos/1.auto new file mode 100644 index 0000000..9adf902 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3, + "y": 5.55 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1.1" + } + }, + { + "type": "path", + "data": { + "pathName": "1.2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1.1.path b/src/main/deploy/pathplanner/paths/1.1.path new file mode 100644 index 0000000..a66e167 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1.1.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 1.2733425960299216, + "y": 5.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.59859163592971, + "y": 5.55 + }, + "prevControl": { + "x": 2.534011049823752, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.1243356864854, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1.2.path b/src/main/deploy/pathplanner/paths/1.2.path new file mode 100644 index 0000000..5529fc3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1.2.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.6266574039700785, + "y": 5.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3, + "y": 5.55 + }, + "prevControl": { + "x": 1.2354194138940418, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.1243356864854, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json new file mode 100644 index 0000000..669097e --- /dev/null +++ b/src/main/deploy/swerve/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.5, + "heading": { + "p": 0.4, + "i": 0, + "d": 0.01 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json new file mode 100644 index 0000000..a9a2d30 --- /dev/null +++ b/src/main/deploy/swerve/modules/backleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -12.375, + "left": 12.375 + }, + "absoluteEncoderOffset": 80.332, + "drive": { + "type": "sparkmax", + "id": 8, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 9, + "canbus": null + }, + "inverted": { + "angle": true, + "drive": false + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 7, + "canbus": null + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json new file mode 100644 index 0000000..679f08d --- /dev/null +++ b/src/main/deploy/swerve/modules/backright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": -12.375, + "left": -12.375 + }, + "absoluteEncoderOffset": 28.74, + "drive": { + "type": "sparkmax", + "id": 11, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 12, + "canbus": null + }, + "inverted": { + "angle": true, + "drive": false + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 6, + "canbus": null + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json new file mode 100644 index 0000000..3c37753 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 12.375, + "left": 12.375 + }, + "absoluteEncoderOffset": 98.438, + "drive": { + "type": "sparkmax", + "id": 2, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 3, + "canbus": null + }, + "inverted": { + "angle": true, + "drive": false + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 4, + "canbus": null + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json new file mode 100644 index 0000000..87f8cd9 --- /dev/null +++ b/src/main/deploy/swerve/modules/frontright.json @@ -0,0 +1,30 @@ +{ + "location": { + "front": 12.375, + "left": -12.375 + }, + "absoluteEncoderOffset": 17.227, + "drive": { + "type": "sparkmax", + "id": 17, + "canbus": null + }, + "angle": { + "type": "sparkmax", + "id": 18, + "canbus": null + }, + "inverted": { + "angle": true, + "drive": false + }, + "conversionFactor": { + "angle": 0, + "drive": 0 + }, + "encoder": { + "type": "cancoder", + "id": 5, + "canbus": null + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json new file mode 100644 index 0000000..4994775 --- /dev/null +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "conversionFactor": { + "angle": 16.8, + "drive": 0.04 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json new file mode 100644 index 0000000..3834a36 --- /dev/null +++ b/src/main/deploy/swerve/modules/pidfproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json new file mode 100644 index 0000000..1604d61 --- /dev/null +++ b/src/main/deploy/swerve/swervedrive.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "pigeon", + "id": 0, + "canbus": null + }, + "invertedIMU": false, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 04557d8..cf31264 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,17 +6,47 @@ package frc.robot; /** Add your docs here. */ public class Constants { - //moteur - public static int lanceur = 1; - public static int lancer2 = 2; - public static int roue = 3; - public static int etoile = 4; - public static int Moteuracc2 = 5; - public static int Moteuracc = 6; - public static int guideur = 7; - // limit switsh - public static int guideurhaut = 8; - public static int guideurbas = 9; - public static int limitacc = 58; + // Lanceur + public static int lanceur = 10; + public static int lancer2 = 13; + public static int lancer3 = 14; + public static int lancer4 = 15; + // Ballayeuse + public static int roue = 16; + public static int etoile = 19; + + // Accumulateur + public static int Moteuracc2 = 20; + public static int Moteuracc = 21; + + // Guideur + public static int guideur = 22; + + //grimpeur + public static int grimpeurd = 26; + public static int grimpeurg = 27; + + // Swerve + public static int AvantDroitDrive = 17; + public static int AvantDroitAngle = 18; + public static int AvantGaucheDrive = 2; + public static int AvantGaucheAngle = 3; + public static int ArriereDroitDrive = 11; + public static int ArriereDroitAngle = 12; + public static int ArriereGaucheDrive = 8; + public static int ArriereGaucheAngle = 9; + + // CanCoder + public static int AvantDroit = 5; + public static int AvantGauche = 4; + public static int ArriereDroit = 6; + public static int ArriereGauche = 7; + + // Limit switsh + public static int guideurhaut = 23; + public static int guideurbas = 24; + public static int limitacc = 25; + public static int limithaut = 28; + public static int limitbas = 29; } diff --git a/src/main/java/frc/robot/subsystem/Accumulateur.java b/src/main/java/frc/robot/subsystem/Accumulateur.java index 6b6918c..622df2e 100644 --- a/src/main/java/frc/robot/subsystem/Accumulateur.java +++ b/src/main/java/frc/robot/subsystem/Accumulateur.java @@ -13,6 +13,7 @@ import frc.robot.Constants; public class Accumulateur extends SubsystemBase { /** Creates a new Accumulateur. */ public Accumulateur() {} + final WPI_TalonSRX Moteuracc2 = new WPI_TalonSRX(Constants.Moteuracc2); final WPI_TalonSRX Moteuracc = new WPI_TalonSRX(Constants.Moteuracc); final DigitalInput limitacc = new DigitalInput(0); diff --git a/src/main/java/frc/robot/subsystem/Balayeuse.java b/src/main/java/frc/robot/subsystem/Balayeuse.java index b18f7ee..83f7404 100644 --- a/src/main/java/frc/robot/subsystem/Balayeuse.java +++ b/src/main/java/frc/robot/subsystem/Balayeuse.java @@ -6,7 +6,6 @@ package frc.robot.subsystem; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/subsystem/Drive.java b/src/main/java/frc/robot/subsystem/Drive.java index 9d4d95a..c4ea3ad 100644 --- a/src/main/java/frc/robot/subsystem/Drive.java +++ b/src/main/java/frc/robot/subsystem/Drive.java @@ -4,11 +4,41 @@ package frc.robot.subsystem; +import java.io.File; +import java.io.IOException; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import swervelib.SwerveDrive; +import swervelib.parser.SwerveParser; public class Drive extends SubsystemBase { /** Creates a new Drive. */ - public Drive() {} + + SwerveDrive swerveDrive; + + File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(),"swerve"); + + public void drive(double x, double y, double zRotation){ + swerveDrive.drive(new Translation2d(x*2, y*2), zRotation, true, false); + } + + public Drive() { + + try { + this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2); + } catch (IOException e) { + e.printStackTrace(); + } + } + + public SwerveModulePosition[] distance(){ + return swerveDrive.getModulePositions(); + } +public void reset(){ + +} @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystem/Grimpeur.java b/src/main/java/frc/robot/subsystem/Grimpeur.java new file mode 100644 index 0000000..3f78416 --- /dev/null +++ b/src/main/java/frc/robot/subsystem/Grimpeur.java @@ -0,0 +1,60 @@ +// 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.subsystem; + +import com.kauailabs.navx.frc.AHRS; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Grimpeur extends SubsystemBase { + /** Creates a new Acrocheur. */ + // moteur + public Grimpeur() {} + final CANSparkMax grimpeurd = new CANSparkMax(Constants.grimpeurd,MotorType.kBrushless); + final CANSparkMax grimpeurg = new CANSparkMax(Constants.grimpeurg,MotorType.kBrushless); + // limit switch + final DigitalInput limitdroite = new DigitalInput(Constants.limithaut); + final DigitalInput limitgauche = new DigitalInput(Constants.limitbas); + //fonction +public void droit(double vitesse){ + grimpeurd.set(vitesse); +} +public void gauche(double vitesse){ + grimpeurg.set(vitesse); +} +public boolean droite(){ + return limitdroite.get(); +} +public boolean gauche(){ + return limitgauche.get(); +} +public void resetencodeurd(){ + grimpeurd.getEncoder().setPosition(0); +} +public void resetencodeurg(){ + grimpeurg.getEncoder().setPosition(0); +} +public void grimpeur(){ + grimpeurg.follow(grimpeurd); +} +public AHRS gyroscope = new AHRS(); + public double getpitch(){ + return gyroscope.getPitch(); + } + @Override + public void periodic() { + // This method will be called once per scheduler run + if(droite()) { + resetencodeurd(); + } + if(gauche()) { + resetencodeurg(); + } + } +} diff --git a/src/main/java/frc/robot/subsystem/Guideur.java b/src/main/java/frc/robot/subsystem/Guideur.java index 4c29dc4..5a0282d 100644 --- a/src/main/java/frc/robot/subsystem/Guideur.java +++ b/src/main/java/frc/robot/subsystem/Guideur.java @@ -13,9 +13,20 @@ import frc.robot.Constants; public class Guideur extends SubsystemBase { /** Creates a new Guideur. */ public Guideur() {} + final WPI_TalonSRX guideur = new WPI_TalonSRX(Constants.guideur); - private DigitalInput guideurhaut = new DigitalInput(Constants.guideurhaut); - private DigitalInput guideurbas = new DigitalInput(Constants.guideurhaut); + final DigitalInput guideurhaut = new DigitalInput(Constants.guideurhaut); + final DigitalInput guideurbas = new DigitalInput(Constants.guideurhaut); + + public void guider(double vitesse){ + guideur.set(vitesse); + } + public boolean haut(){ + return guideurhaut.get(); + } + public boolean bas(){ + return guideurbas.get(); + } @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystem/Lanceur.java b/src/main/java/frc/robot/subsystem/Lanceur.java index dc7ee09..260f263 100644 --- a/src/main/java/frc/robot/subsystem/Lanceur.java +++ b/src/main/java/frc/robot/subsystem/Lanceur.java @@ -18,16 +18,20 @@ public class Lanceur extends SubsystemBase { public Lanceur() {} final CANSparkMax lancer = new CANSparkMax(Constants.lanceur, MotorType.kBrushless); final CANSparkMax lancer2 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless); + final CANSparkMax lancer3 = new CANSparkMax(Constants.lanceur, MotorType.kBrushless); + final CANSparkMax lancer4 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless); + public void lancer(double vitesse){ lancer.set(vitesse); } public void lanceur(){ lancer2.follow(lancer); + lancer3.follow(lancer); + lancer4.follow(lancer); + lancer3.setInverted(true); + lancer4.setInverted(true); } - - - @Override public void periodic() {} // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystem/Pixy.java b/src/main/java/frc/robot/subsystem/Pixy.java new file mode 100644 index 0000000..f56c0d2 --- /dev/null +++ b/src/main/java/frc/robot/subsystem/Pixy.java @@ -0,0 +1,18 @@ +// 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.subsystem; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Pixy extends SubsystemBase { + + /** Creates a new Pixy. */ + public Pixy() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}